I don't know why the Kalman filter algorithm I have written is not working

Hello friends.

I have written a kalman filter algorithm and I wanted to apply it on the simple motion system of truck (position and velocity of the truck) with state position and velocity. i have attached the description of the simple position and velocity system

In my code, I created the data using the system first and then applied the kalman filter algorithm, but the result is not is correct, I assume. Because the position difference in the first iteration becomes zero, (X-Xhat), which does not match with kalman filter function,

Also, velocity estimation has always a delay, I attached both figure to this message . I would appriciate if you can help me to fix the issue.

close all

clear all

Fs=100;

N=1000;

dt=1/Fs;

F=[1,dt;0,1]

G=[1/2*dt^2;dt]

X(:,1)=[0;0];

H=[1,0];

sigma_r=1*10^-2;

%%%%%%%%%%create data with system X(k+1)=F*X(k)+ G*accelerometer

for n=1:N

accelormeter(n)=sin(n/200*(2*pi));

X(:,n+1)=F*X(:,n)+G*accelormeter(n);

Y(:,n)=H*X(:,n)+sigma_r*randn(size(H,1),1);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%Initial value for kalman filter

Xhat_initial=X(:,1)+1;

Xhat(:,1)=Xhat_initial;

Yhat(:,1)=H*Xhat_initial;

Z=Y(:,2);

Q=G*G';

R=sigma_r*eye(size(H,1));

P_initial=eye(2)*1;

P(:,:,1)=P_initial;

%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%calling kalman filter recursively

for n=2:N-1

[X_update,y_update,P_update,K1]= kalman_Filter(F,H,Xhat_initial,Z,Q,R,P_initial)

Xhat(:,n)=X_update;

Yhat(:,n)=y_update;

P(:,:,n)=P_update;

K(:,:,n)=K1;

Xhat_initial=Xhat(:,n);

Z=Y(:,n+1);

P_initial=P(:,:,n);

end

figure(1)

plot(X(1,:))

hold on

plot(Xhat(1,:))

figure(2)

plot(X(2,:))

hold on

plot(Xhat(2,:))

%%%%%%%%%%%%% kalman filter function

function [X_update,y_update,P_update,K]= kalman_Filter(F,H,Xhat_initial,Z,Q,R,P)

%%%%%%predict

X_priori=F*Xhat_initial;

P_priori=F*P*F'+Q;

e_priori=Z-H*X_priori;

%%%%%%%%%%%%update

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

X_update=X_priori+K*e_priori;

P_update=(eye(size(P_priori))-K*H)*P_priori;

y_update=H*X_update;

end

Answer 1

[Sarthak]

Hi Meysam,

The reason why the position difference in the first iteration becomes zero is because you are initializing the state estimate, “Xhat_initial”, to be one unit away from the true state, “X(:,1)”. This means that the error covariance matrix, “P_initial”, will be non-zero, and the Kalman filter will update the state estimate to be closer to the true state.

The reason why the velocity estimation has always been delayed is because the Kalman filter is using a linearized model of the system. This means that the filter is assuming that the system is not changing very much between time steps. However, in reality, the system is changing, and this can cause a delay in the velocity estimation.

To fix these issues, you can try the following:

• Initialize the state estimate, “Xhat_initial”, to be the same as the true state, “X(:,1)”. This will make the error covariance matrix, “P_initial”, zero, and the Kalman filter will not update the state estimate in the first iteration.
• Use a nonlinear Kalman filter instead of a linear Kalman filter. This will allow the filter to take into account the changes in the system between time steps.

I hope this helps and you can try rewriting your code following the steps above.