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

10 views (last 30 days)
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;

Answers (1)

Jon
Jon on 16 Nov 2023
Edited: Jon on 16 Nov 2023
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;

Products


Release

R2020a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!