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

9 次查看（过去 30 天）
meysam ghanavati
meysam ghanavati 2022-10-18
回答： Sarthak 2023-9-11
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

请先登录，再回答此问题。

回答（1 个）

Sarthak
Sarthak 2023-9-11
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.

类别

在 Help Center 和 File Exchange 中查找有关 State Estimation 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by