MATLAB：为什么我的卡尔曼滤波器不适用于非零输入？

Question

我已经为任意值构建了一个简单的卡尔曼滤波器，只要我不使用非零输入，它似乎就可以很好地工作。当我以零输入运行此脚本时，我可以从最后的图中看到，估计状态比噪声测量状态更好地表示了真实状态。这可以在这里看到：

但是，当我运行相同的脚本，但这次使用正弦输入向量时，估计状态完全偏离真实状态。这可以在这里看到：

有谁知道为什么会这样？我已经检查了我的方程并尝试查找其他示例脚本，但说实话，我在这一点上感到非常困惑。 这是我的代码：

“”“

clear all;

clf;
close all;

Phi = [0.5,1.0;-0.5,0.5];
Psi = eye(2);%zeros(2,2);
Gamma = eye(2);
H = eye(2);

dt = 0.01;
t = 0:dt:5;
f = 0.2;
%u = [sin(2*pi*f*t); cos(2*pi*f*t)];
u = zeros(2,length(t)); %Works if input is 0s

%Initial values
x0 = [0; 0];
P0 = eye(2);

%process noise
Q = [0.01 0; 0 0.02];
w = sqrtm(Q)*randn(2,length(t));
%sensor noise
R = [0.2 0; 0 0.2];
v = sqrtm(R)*randn(2,length(t)); %We use randn since it generates 0-mean normally distributed values

%Initialise true state and measured output vectors
x_true = lsim(ss(Phi,Psi,eye(2),zeros(2,2)),u,t,x0)' + w;
z_measured =  zeros(2,length(t));

x_estimates = zeros(2,length(t));

%Kalman filter loop
x_estimate = x0;
P_estimate = P0;

for i = 1:length(t)
   z_measured(:,i) = H*x_true(:,i) + v(:,i);

   x_predict = Phi*x_estimate + Psi*u(:,i);
   P_predict = Phi*P_estimate*Phi' + Gamma*Q*Gamma';

   K = P_predict*H'/(H*P_predict*H'+R);

   x_estimate = x_predict + K*(z_measured(:,i) - H*x_predict); 
   P_estimate = (eye(2) - K*H)*P_predict;

   x_estimates(:,i) = x_estimate;
end


% Plot the results
subplot(2,1,1)
plot(t, x_true(1,:), 'g', 'DisplayName', 'True State 1');
hold on;
plot(t, z_measured(1,:), 'r', 'DisplayName', 'Noisy Measurements 1');
plot(t, x_estimates(1,:), 'b', 'DisplayName', 'Estimated State 1');
xlabel('Time');
ylabel('State 1');
title('Kalman Filter Estimation State 1');
legend;


subplot(2,1,2)
plot(t, x_true(2,:), 'g', 'DisplayName', 'True State 2');
hold on;
plot(t, z_measured(2,:), 'r', 'DisplayName', 'Noisy Measurements 2');
plot(t, x_estimates(2,:), 'b', 'DisplayName', 'Estimated State 2');
xlabel('Time');
ylabel('State 2');
title('Kalman Filter Estimation State 2');
legend;

Answer 1

问题是我使用的是连续系统而不是离散系统。

这是通过使用采样时间定义

x_true

来解决的：

x_true = lsim(ss(Phi,Psi,eye(2),zeros(2,2)),u,t,x0)' + w;