卡尔曼滤波。。。求大神们帮忙解答解答...

lowweihang123

clear;
FS=100;
T=1/FS;
t=0:T:5;
wv=randn(1,(5-0)/T+1)*0.2;%产生均值为1.均方差为sqrt(5)正态分布的随机数
wa=randn(1,(5-0)/T+1);
vi=sin(5*t)+wv;           %加速度信号产生的角度？？？？
ai=cos(5*t)-10+wa;        %陀螺仪产生的角速度？？？？？
A=[1,-T;0,1];
B=[T;0];
H=[1,0];
Q=[0.0001,0;0,0];
R=[0.04];
I=[1,0;0,1];
kg=[0;0];
x=[0;0];
p=[1,0;0,1];
xx=[0;0];
pp=[0,0;0,0];
for k=2:501
    xx=A*x+B*ai(k-1);      %X(k|k-1)=A X(k-1|k-1)+B U(k)
    pp=A*p*A'+Q;           %P(k|k-1)=A P(k-1|k-1) A’+Q
    kg=pp*H'/(H*pp*H'+R);  %Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    x=xx+kg*(vi(k)-H*xx);  %X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
    p=(I-kg*H)*pp;         %P(k|k)=（I-Kg(k) H）P(k|k-1)
    vo(k)=x(1)-5;          %x(1)即x的横坐标代表什么？为什么要减去5？不减5为什么会变成一条直线？
    b(k)=x(2);             %x(2)是什么？？
end
subplot(3,1,1);plot(t,wv,'r',t,wa)
subplot(3,1,2);plot(t,vi,'r',t,ai,'b')
subplot(3,1,3);plot(t,vo,'b',t,b,'y');

幻天者

卡尔曼滤波的全部程序？

lowweihang123

幻天者 发表于 2012-5-13 19:58
卡尔曼滤波的全部程序？

是的，matlab实现