admin管理员组文章数量:1660207
1prepare
you should read :An Introduction to the Kalman Filter
Of course you can read other example :https://blog.csdn/App_12062011/article/details/51758989 (easy for you understanding)
https://blog.csdn/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
本文标签: applicationFilterKalmanestimatepostion
版权声明:本文标题:Kalman filterand its application in vehicle postion estimate 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://m.elefans.com/dianzi/1729850191a1215306.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论