[求助]卡尔曼滤波程序---求高手指点

fjh009003

对matlab不熟，搞了好久也没做出来，望高手多加指点，谢谢各位朋友！！！程序如下：
************************************************************************************************************
clear
N=500;%定义数据个数%
%w(1)=0;
w=5*randn(1,N)+0; %此为获得方差为o，均值为u的白噪声，o*randn(M,N)+u%
x(1)=2500;
x(2)=2500;
realdistance(1)=2500;%定义不变值%
a=[2,-1;1,0];%状态矩阵
for k=3:N;
x(k)=2500;
realdistance(k)=2500;
%(x(k),x(k-1))'=a*(x(k-1),x(k-2))'+(w(k-1),0);%系统方程%
x(k)=2*x(k-1)-x(k-2)+w(k-2);%这个地方老提示错误，所以我把方程分写
x(k-1)=x(k-1);
end

V=200*randn(1,N)+0;
q1=std(V);
Rvv=q1.^2;%定义观测误差
q2=std(x);%求取矩阵方差 %
Rxx=q2.^2;%观测协方差%对应R
q3=std(w);
Rww=[q3.^2,0;0,0;]%定义系统协方差%对应Q
c=[1,0];%观测矩阵
Y=c*[x(k) x(k-1)].'+V;%观测方程

p=[100 0;0 100];%p(1)=0,定义初始方差为5%
s(1)=2800;
s(2)=2900;%s(1)=0定义初始状态值为30%
for t=3:N;%由p(1)，s(1)驱动整个滤波过程%
p1=a*p*a'+Rww;%第二个方程 P(k|k-1)=A P(k-1|k-1) A’+Q ……… %right
b=p1*c'/(c*p1*c'+Rvv);%Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)%
[s(t),s(t-1)].'=a*[s(t-1),s(t-2)].'+b*(Y(t)-c*a*[s(t-1),s(t-2)].');%X(k|k)= X(k|k-1)+Kg(k) (Z(k)-%H X(k|k-1)) ……… (3)%注意里面已经包括了一步预测方程%这个方程称也老提示错误
p=p1-b*c*p1;%P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)%
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,realdistance);
%plot(t,s,'r',t,Y,'g',t,x,'b',t,realdistance);
************************************************************************************************************
这是根据论坛上出现的一维卡尔曼滤波程序改的。请高手多加指点，告诉我犯了哪些错误！！
论文马上就得写了，必须得做一个滤波处理，所以很是着急！！！
拜托各位了！！！！！

yangzj

[s(t),s(t-1)].'=a*[s(t-1),s(t-2)].'+b*(Y(t)-c*a*[s(t-1),s(t-2)].')
改成s(t:-1:t-1)=a*[s(t-1),s(t-2)].'+b*(Y(t)-c*a*[s(t-1),s(t-2)].');
运行,不过结果是不对的.
呵呵,建议还是看看matlab的一些基本知识

fjh009003

这个程序我已经改正过来了，那个地方确实是有问题。我是这样做的aa=[s(t),s(t-1)].',s(t)=aa(1),s(t-1)=aa(2).
因为和c不一样，所以犯了这个错误，谢谢！！！！
yangzj！！
继续这个话题
我用这个程序得出了滤波结果，但是和预料的很有偏差，请指教！！用各次得到的值，可以得到曲线，由于测量环境影响，有偏差，所以进行滤波，但是可以看到，滤波后曲线和实际位置有较大偏差，这是为什么呢？我试了几个p0,都不能得到理想的结果，请指教！
图片如下：

chill16060

一般对于KF来说，初始值并不影响滤波的最后结果，这是一个不断预测修正的过程，所以理论上说你任意选择X0和P0都不会对最后结果有影响（多次滤波后）。你采用的是离散KF方程，但不知你的系统状态和观测方程是不是离散的，如果不是，应该有c2d来离散化。你的程序有些乱，我有些看不明白。用你的程序做了一下仿真，和你给出的也不一致。

花瓣雨

q2=std(x);%求取矩阵方差 %
是这样求的吗？为什么？

zcl001

回复 1 # fjh009003 的帖子

您好！能把你的这篇论文发我邮箱吗，也做这个毕业论文呢，不会呀，谢谢！
  zhuchengliang0@126.com

zcl001

邮箱错了，是zhuchengliang0@126.com

zcl001

急用呢，谢谢！！！

woimpc

你应该先仔细读读KALMAN滤波的五个方程，哈哈