Matlab：如何在卡尔曼滤波器状态估计后模拟模型

Posted 2023-02-21

【中文标题】Matlab：如何在卡尔曼滤波器状态估计后模拟模型

【英文标题】：Matlab: How do I simulate the model after state estimation from Kalman filter

【发布时间】：2015-03-10 02:47:38

【问题描述】：

我正在尝试为以下一维 AR 模型实现卡尔曼滤波器的基本方程：

x(t) = a_1x(t-1) + a_2x(t-2) + w(t)  

y(t) = Cx(t) + v(t);

KF 状态空间模型：

x(t+1) = Ax(t) + w(t)

y(t) = Cx(t) + v(t)

w(t) = N(0,Q)

v(t) = N(0,R)

在哪里

 % A - state transition matrix
% C - observation (output) matrix
% Q - state noise covariance
% R - observation noise covariance
% x0 - initial state mean
% P0 - initial state covariance

%%% Matlab script to simulate data and process usiung Kalman for the state
%%% estimation of AR(2) time series.
% Linear system representation
% x_n+1 = A x_n + Bw_n
% y_n = Cx_n + v_n
% w = N(0,Q); v = N(0,R)
clc
clear all

T = 100; % number of data samples
order = 2;
% True coefficients of AR model
  a1 = 0.195;
  a2 = -0.95;

A = [ a1  a2;
      1  0 ];
C = [ 1 0 ];
B = [1;
      0];

 x =[ rand(order,1) zeros(order,T-1)];



sigma_2_w =1;  % variance of the excitation signal for driving the AR model(process noise)
sigma_2_v = 0.01; % variance of measure noise


Q=eye(order);
P=Q;




%Simulate AR model time series, x;



sqrtW=sqrtm(sigma_2_w);
%simulation of the system
for t = 1:T-1
    x(:,t+1) = A*x(:,t) + B*sqrtW*randn(1,1);
end

%noisy observation

y = C*x + sqrt(sigma_2_v)*randn(1,T);

%R=sigma_2_v*diag(diag(x));
%R = diag(R);

R = var(y);
z = zeros(1,length(y));
z = y;

 x0=mean(y);
for i=1:T-1
[xpred, Ppred] = predict(x0,P, A, Q);
[nu, S] = innovation(xpred, Ppred, z(i), C, R);
[xnew, P] = innovation_update(xpred, Ppred, nu, S, C);
end

%plot
xhat = xnew';


plot(xhat(:,1),'red');
hold on;
plot(x(:,1));



function [xpred, Ppred] = predict(x0,P, A, Q)
xpred = A*x0;
Ppred = A*P*A' + Q;
end

function [nu, S] = innovation(xpred, Ppred, y, C, R)
nu = y - C*xpred; %% innovation
S = R + C*Ppred*C'; %% innovation covariance
end

function [xnew, Pnew] = innovation_update(xpred, Ppred, nu, S, C)
K = Ppred*C'*inv(S); %% Kalman gain
xnew = xpred + K*nu; %% new state
Pnew = Ppred - Ppred*K*C; %% new covariance
end

问题：我想通过绘图查看估计状态xnew 与实际状态x 的接近程度。但是，innovation_update 函数返回的 xnew 是一个 2by2 矩阵！如何使用估计值模拟时间序列？

【问题讨论】：

输入 x =[ rand(order,1) zeros(order,T-1)] 也是一个 2 x 2 数组，所以如果你不明白为什么它是 2 x 2 你可能不会明白为什么输出是 2 x 2 。我的建议是先了解您的意见。

@TryHard: x 不是 2 x 2 数组。我再次检查，它给了我 2 x 100 矩阵，其中第一列的第一个和第二个元素是随机数。

你是对的，我的错。除非我再次弄错，否则它实际上被初始化为 2 x 1 向量，即rand(order,1)。数组的其余部分为零，以便为演化状态预分配空间。

执行 rand(order,1) 意味着只有第一列的第一行和第二行的元素具有初始化为它们的随机值。其余为零。

【参考方案1】：

您不需要将x 初始化为任何内容，只需设置初始状态x(:,1)，“系统模拟”循环将填充其余部分。 哎呀，我看到你已经这样做了！

稍后，在从嘈杂观察 y 推断状态 xnew 的循环中，您可以添加以下行：

[xnew, P] = innovation_update(xpred, Ppred, nu, S, C);
yhat(i) = C*xnew; % Observed value at time step i, assuming inferred state xnew

最后，您应该绘制yhat 和y 进行比较。

如果您想为估计的不确定性添加误差线，那么您还应该存储Phat(i) = sqrt(C*P*C') 并调用errorbar(yhat,Phat) 而不是plot(yhat)。

【讨论】：

感谢您的回复。 (1) 我将初始 A 矩阵的第二行修正为 1 0。 (2) 答案不起作用，因为函数 innovation_update 返回的 xnew 是 2 x 2。但是，yhat 应该是 1 x T 向量。因此，有一个错误

在包含对innovation_update 的调用的循环之前，您应该将yhat 初始化为一个1xT 向量。在循环内部，我们只是写入i-th 元素。 xnew 应该是一个 2x1 向量。

我做了 yhat 的初始化。我不知道为什么 xnew 会变成 2by2 而不是 2 by 1。所有这些麻烦都是由于 xnew 造成的。你能检查一下代码和esp吗？函数 predict 因为它在那里 xpred = A*x0 我得到 2 x 2 而不是 2 x 1。

现在就知道我的错误了。我的印象是，由于状态是隐藏的，我怎么知道它的平均值以便将它传递给过滤器。我认为 x0 = mean(x,2) 所以 xpred 现在是 2 乘 1。现在一切正常

如果系统中没有太多噪音，

y 和 yhat 将彼此接近 - 尝试增加 sigma_2_v 和/或 sigma_2_w 并查看 y 和yhat 分歧。另请查看Phat 是否会随着噪声协方差的增加而增加（应该如此）。