声振论坛

 找回密码
 我要加入

QQ登录

只需一步,快速开始

查看: 5262|回复: 8

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

[复制链接]
发表于 2006-5-14 13:00 | 显示全部楼层 |阅读模式

马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。

您需要 登录 才可以下载或查看,没有账号?我要加入

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);
************************************************************************************************************
这是根据论坛上出现的一维卡尔曼滤波程序改的。请高手多加指点,告诉我犯了哪些错误!!
论文马上就得写了,必须得做一个滤波处理,所以很是着急!!!
拜托各位了!!!!!

本帖被以下淘专辑推荐:

回复
分享到:

使用道具 举报

发表于 2006-5-18 20:33 | 显示全部楼层
[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的一些基本知识
 楼主| 发表于 2006-5-19 17:32 | 显示全部楼层

回复:(yangzj)[s(t),s(t-1)].'=a*[s(t-1),s(t-2)]....

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

说一句

一般对于KF来说,初始值并不影响滤波的最后结果,这是一个不断预测修正的过程,所以理论上说你任意选择X0和P0都不会对最后结果有影响(多次滤波后)。你采用的是离散KF方程,但不知你的系统状态和观测方程是不是离散的,如果不是,应该有c2d来离散化。你的程序有些乱,我有些看不明白。用你的程序做了一下仿真,和你给出的也不一致。
发表于 2008-1-13 18:34 | 显示全部楼层

回复 楼主 的帖子

q2=std(x);%求取矩阵方差 %
是这样求的吗?为什么?
发表于 2011-4-25 14:35 | 显示全部楼层
本帖最后由 zcl001 于 2011-4-25 14:37 编辑

回复 1 # fjh009003 的帖子

您好!能把你的这篇论文发我邮箱吗,也做这个毕业论文呢,不会呀,谢谢!
  zhuchengliang0@126.com
发表于 2011-4-25 14:36 | 显示全部楼层
邮箱错了,是zhuchengliang0@126.com
发表于 2011-4-25 14:37 | 显示全部楼层
急用呢,谢谢!!!
发表于 2011-10-3 23:57 | 显示全部楼层
你应该先仔细读读KALMAN滤波的五个方程,哈哈
您需要登录后才可以回帖 登录 | 我要加入

本版积分规则

QQ|小黑屋|Archiver|手机版|联系我们|声振论坛

GMT+8, 2024-11-6 09:44 , Processed in 0.066498 second(s), 24 queries , Gzip On.

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表