Kalman filter and its application in vehicle postion estimate

1prepare

you should read :An Introduction to the Kalman Filter 

Of course  you can read other example :https://blog.csdn.net/App_12062011/article/details/51758989 (easy for you understanding)

 https://blog.csdn.net/zengxiantao1994/article/details/71170728 (with matlab code)

2Problem

 Taking speed as system input and GPS position as system measurement, write down the whole process model. Design a Kalman filter to estimate the position x

xk^-=x(k-1)+u

z(k)=1.xk^-;

 

 

 

speed:--u /m/s

gps:--z /m

polyfit

%ployfit gps with pos and get the sd value
sz=size(z)
t=1:sz(1);
p = polyfit(t',z,1)
y=polyval(p,t');
plot(t',y,'r',t',z,'k.'),xlabel('time/s'),ylabel('position/m'),title('polyfit the gps position')
legend('polyfit curve','GPS point')
delt=y-z;
%standard derivation
R=std(delt)

R=1.354

follow the  kalman procedure to code it

%R=std(z)
%x(k)=x(k-1)+1*u
%z(k)=x(k)+sqr(R)*rdn


%%
A=1;B=1;H=1;
Q=0.2; 
sz=size(z);
xk=zeros(sz);% prior estimate
xpk=zeros(sz);%post estimate value;
Pmins=zeros(sz);% post covinrance 
pk=zeros(sz);%prior convinrance
kg=zeros(sz);kg(1)=1;
for k=2:sz(1)
    xpk(k)=A*xk(k-1)+B*u(k-1);
    Pmins(k)=A*pk(k-1)*A'+Q;
    kg(k)=Pmins(k)*H'/(H*Pmins(k)*H'+R);
    xk(k)=xpk(k-1)+kg(k)*(z(k)-H*xpk(k-1));
    pk(k)=(1-kg(k)*H)*Pmins(k);    
end
plot(1:sz(1),z,'k',1:sz(1),xk,'r')
legend('GPS','estimate position')
xlabel('time/s')
ylabel('distance/m')
title('speed with gps to estimate kanlman')

Edit:   xk(k)=xpk(k)+kg(k)*(z(k)-H*xpk(k));

 

As shown in the code,

xk(k-1) --GPS position,

xpk(k)-- the posteriori estimate value.

Pmins(k)--

pk(k-1)--

kg(k):kalman gain