Why does my Kalman filter not work for non-zero input?

I have built a simple kalman filter for an arbritary that seems to work well as long as I do not use a non-zero input. When I run this script with a zero input I can see from my plots at the end that the estimated state is a better representation of the true state than the noisy measured state. This can be seen here:

However, when I run the same script, but this time using a sinusoidal input vector the estimated state diverges completely from the true state. This can be seen here:

Does anyone know why this might be? I have checked my equations and tried to look up other example scripts, but to be honest I am pretty stumped at this point.

Here is my code:

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 inputs are zeros

%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

[Jon]

When you generate your actual state vector you simulated it as a continuous time system,

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

Note the additional argument dt for ss(Phi,...) which tells MATLAB it is a discrete time system

Here is the corrected code with the dt argument added to ss

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 inputs are zeros

%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),dt),u,t,x0)' + w; % Modified to make discrete
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;