卡尔曼滤波器的一个最简单的实现

卡尔曼是本科毕设期间学习的一个东西，一开始也是云里雾里，最近要做视觉与INS（惯导）的数据融合，需要进行kalman的再一次实现。先将一个最简单的代码实现放上去。供大家参考。没错，是基于MATLAB的，我也想搞个Python的，之前进行视觉导航输出结果计算用的Python，感觉已经爱上不能自拔。

Clear

ts = 1; % Samplingtime

t = [0:ts:200];

T = length(t);
w(1)=0;
w=randn(1,T)

V=randn(1,T);
x(1)=0;
A=eye(1);
for k=2:T;
x(k)=A*x(k-1)+w(k-1);
end

q1=std(w);
Rww=q1.^2;
q2=std(V);
Rvv=q2.^2;
q3=std(x);
Rxx=q3.^2; 

c=0.2*eye(1);
Y=c*x+V;

p(1)=0;
s(1)=0;
for t=2:T;
p1(t)=A.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=A*s(t-1)+b(t)*(Y(t)-A*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end

t=1:T;
plot(t,s,'r',t,Y,'g',t,x,'b');

legend(‘估计值’,’测量值’,’真实值’);