|
马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。
您需要 登录 才可以下载或查看,没有账号?我要加入
x
对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);
************************************************************************************************************
这是根据论坛上出现的一维卡尔曼滤波程序改的。请高手多加指点,告诉我犯了哪些错误!!
论文马上就得写了,必须得做一个滤波处理,所以很是着急!!!
拜托各位了!!!!! |
|