[求助]卡尔曼滤波程序---求高手指点
对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=;%状态矩阵
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=%定义系统协方差%对应Q
c=;%观测矩阵
Y=c*.'+V;%观测方程
p=;%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)%
.'=a*.'+b*(Y(t)-c*a*.');%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);
************************************************************************************************************
这是根据论坛上出现的一维卡尔曼滤波程序改的。请高手多加指点，告诉我犯了哪些错误！！
论文马上就得写了，必须得做一个滤波处理，所以很是着急！！！
拜托各位了！！！！！ .'=a*.'+b*(Y(t)-c*a*.')
改成s(t:-1:t-1)=a*.'+b*(Y(t)-c*a*.');
运行,不过结果是不对的.
呵呵,建议还是看看matlab的一些基本知识
回复：（yangzj）[s(t),s(t-1)].'=a*[s(t-1),s(t-2)]....
这个程序我已经改正过来了，那个地方确实是有问题。我是这样做的aa=.',s(t)=aa(1),s(t-1)=aa(2).因为和c不一样，所以犯了这个错误，谢谢！！！！
yangzj！！
继续这个话题
我用这个程序得出了滤波结果，但是和预料的很有偏差，请指教！！用各次得到的值，可以得到曲线，由于测量环境影响，有偏差，所以进行滤波，但是可以看到，滤波后曲线和实际位置有较大偏差，这是为什么呢？我试了几个p0,都不能得到理想的结果，请指教！
图片如下：
说一句
一般对于KF来说，初始值并不影响滤波的最后结果，这是一个不断预测修正的过程，所以理论上说你任意选择X0和P0都不会对最后结果有影响（多次滤波后）。你采用的是离散KF方程，但不知你的系统状态和观测方程是不是离散的，如果不是，应该有c2d来离散化。你的程序有些乱，我有些看不明白。用你的程序做了一下仿真，和你给出的也不一致。回复 楼主 的帖子
q2=std(x);%求取矩阵方差 %是这样求的吗？为什么？ 本帖最后由 zcl001 于 2011-4-25 14:37 编辑
回复 1 # fjh009003 的帖子
您好！能把你的这篇论文发我邮箱吗，也做这个毕业论文呢，不会呀，谢谢！
zhuchengliang0@126.com 邮箱错了，是zhuchengliang0@126.com 急用呢，谢谢！！！ 你应该先仔细读读KALMAN滤波的五个方程，哈哈
页:
[1]