卡尔曼滤波两例题含matlab程序_第1页
卡尔曼滤波两例题含matlab程序_第2页
卡尔曼滤波两例题含matlab程序_第3页
卡尔曼滤波两例题含matlab程序_第4页
卡尔曼滤波两例题含matlab程序_第5页
已阅读5页,还剩12页未读 继续免费阅读

下载本文档

版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领

文档简介

1、一. 已知一物体作自由落体运动,对其高度进行了20次测量,测量值如下表:时间s1234567高度km1.99451.97941.95541.92141.87771.82501.7598时间s891011121314高度km1.68671.60361.50921.40761.29441.17241.0399时间s151617181920高度km0.89800.74550.58500.41250.23180.0399设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列,该物体的初始高度和速度也是高斯分布的随机变量,且。试求该物体高度和速度随时间变化的最优估计。()解: 1. 令 t=1 r(k

2、)=1 q(k)=0 根据离散时间卡尔曼滤波公式,则有: 滤波初值: 一步预测: 滤波增益:滤波计算: 2. 实验结果高度随时间变化估计速度随时间变化的最优估计高度协方差速度协方差从以上的结果,可以得到高度和速度的估计值,再通过所得到的高度协方差和速度协方差,可见用卡尔曼滤波法,虽然刚开始的初始高度协方差很大为100,但通过2步之后减小到不超过1,逐渐接近于0, 同样的速度协方差刚开始的时候也比较大,为2,但是通过5步之后迅速减小,到10步之后接近于0。3. 有关参数的影响(例如初始条件、噪声统计特性对滤波结果的影响等);1)初始条件改变时,改变初始高度值,和速度值 由实验结果分析可得度滤波值

3、和速度滤波值在开始几步接近初始值,协方差值基本不变。2)当初始协方差值改变时,改为实验结果分析高度和速度滤波值基本不变,速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。但是经过几步之后,都趋于0。二 同样考虑自由落体运动的物体,用雷达(和物体落地点在同一水平面)进行测量,如图所示。如果,且雷达测距和测角的测量噪声是高斯白噪声随机序列,均值为零、方差阵,试根据下列测量数据确定物体的高度和速度随时间变化的估计值。时间s*1000斜距km俯仰角rad*1000 0.00050000000000 2.82741643781891 0.00075850435876 0.001000000

4、00000 2.82519811729771 0.00083282260478 0.00150000000000 2.82066686966236 0.00067808241639 0.00200000000000 2.81487233105901 0.00085279036802 0.00250000000000 2.80671786536244 0.00072900768452 0.00300000000000 2.79725268974089 0.00080072481819 0.00350000000000 2.78664273475039 0.00075095576213 0.004

5、00000000000 2.77320365026313 0.00065762725379 0.00450000000000 2.75919535464551 0.00081186148545 0.00500000000000 2.74331288628195 0.00079783727034 0.00550000000000 2.72538888482812 0.00073060712986 0.00600000000000 2.70664967712312 0.00063242006530 0.00650000000000 2.68632403406473 0.00063656524495

6、 0.00700000000000 2.66386533852220 0.00080659845639 0.00750000000000 2.64093529707333 0.00067704740069 0.00800000000000 2.61621111727357 0.00076573767706 0.00850000000000 2.59038109850785 0.00054955759081 0.00900000000000 2.56298794272843 0.00058487913971 0.00950000000000 2.53498317950797 0.00055602

7、747368 0.01000000000000 2.50647589372246 0.00033550412588 0.01050000000000 2.47571075016386 0.00056012688452 0.01100000000000 2.44560676000982 0.00056694491978 0.01150000000000 2.41403690772088 0.00059380631025 0.01200000000000 2.38252228611696 0.00053681916544 0.01250000000000 2.35016501182332 0.00

8、065871960781 0.01300000000000 2.31790939837137 0.00068598344328 0.01350000000000 2.28597616656453 0.00060922471348 0.01400000000000 2.25418431681401 0.00057086018918 0.01450000000000 2.22259320219535 0.00041308535708 0.01500000000000 2.19237398969466 0.00047302026281 0.01550000000000 2.1629017799727

9、1 0.00030949309972 0.01600000000000 2.13441725793706 0.00040552624986 0.01650000000000 2.10811064690727 0.00037545033142 0.01700000000000 2.08322179823195 0.00017282319262 0.01750000000000 2.06148109026767 0.00020758327980 0.01800000000000 2.04219885094031 0.00037186464579 0.01850000000000 2.0261023

10、5314357 0.00018082163465 0.01900000000000 2.01290326863579 0.00023323830160 0.01950000000000 2.00463157388395 -0.00004536186964 0.02000000000000 2.00058143251913 0.00003246284068雷达物体vhd0斜距俯仰角题2 示意图解: 1. 令 t=0.5 q(k)=0 根据离散时间扩展卡尔曼滤波公式,则有:状态方程: 测量方程: 辅助方程:一步预测: 滤波增益:滤波计算:滤波初值: 2. 实验结果高度随时间变化估计速度随时间变化的

11、估计高度协方差:速度协方差估计实验结果分析:根据图,可得高度和速度的估计值,通过扩展卡尔曼滤波法,高度协方差和速度协方差,刚开始的值比较大,但是迅速减小,在几步之后逐渐趋近于0。3. 有关参数的影响(例如初始条件、噪声统计特性对滤波结果的影响等);1)初始条件发生变化,改变高度和速度的初始值为分析:高度和速度在刚开始的时候波动比较大,不过经过10步之后,逐渐趋于平稳,高度协方差收敛变快,速度协方差基本不变2)初始噪声改变,分析:高度和速度滤波值基本不变,速度协方差也基本不变,高度协方差刚开始的时候有波动,10步之后趋于稳定。附源程序:第一题t=1;a=1 -t;0 1;g=9.8;u=-0.5

12、*g*t2;g*t;c=1 0;r=1;i=1 0;0 1;x=zeros(2,1);k=zeros(2,1);p=zeros(2,2);p=150 0;0 10;x=1900;10y=1900 1994.5 1979.4 1955.4 1921.4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.4 1172.4 1039.9 898.0 745.5 585.0 412.5 231.8 399;for i=2:21pi=a*p*a;%一步预测xi=a*x+u;k=pi*c*(c*pi*c+r)(-1);%增益x=xi+k*(y(i)

13、-c*xi);%滤波he(i)=x(1,1);ve(i)=x(2,1);p=(i-k*c)*pi;ph(i)=p(1,1);pv(i)=p(2,2);endhe(1)=1900;ve(1)=10;ph(1)=150;pv(1)=10;figure;t=1:21;plot(t,he(t),r);title(高度滤波值)figure;t=1:21;plot(t,ve(t);title(速度滤波值)figure;t=1:21;plot(t,ph(t);title(高度协方差)figure;t=1:21;plot(t,pv(t);title(速度度协方差)第二题d=1995;t=0.5;a=1 0 0

14、;0 1 -t;0 0 1; %表示状态方程中的faig=9.8;u=0;-0.5*g*t2;g*t; %表示状态方程中的ur=1.04 0;0 1.01; %表示测量噪声i=1 0 0;0 1 0;0 0 1;p=5 0 0;0 5 0;0 0 2;x=1995;2005;1;%滤波初值y=2.8284; 2.82741643781891; 2.82741643781891; 2.82066686966236; 2.81487233105901; 2.80671786536244; 2.79725268974089; 2.78664273475039; 2.77320365026313; 2

15、.75919535464551; 2.74331288628195; 2.72538888482812; 2.70664967712312; 2.68632403406473; 2.66386533852220; 2.64093529707333; 2.61621111727357; 2.59038109850785; 2.56298794272843; 2.53498317950797; 2.50647589372246; 2.47571075016386 ; 2.44560676000982; 2.41403690772088; 2.38252228611696; 2.3501650118

16、2332; 2.31790939837137; 2.28597616656453; 2.25418431681401; 2.22259320219535; 2.19237398969466; 2.16290177997271; 2.13441725793706; 2.10811064690727; 2.08322179823195; 2.06148109026767; 2.04219885094031; 2.02610235314357; 2.01290326863579; 2.00463157388395; 2.00058143251913;y=y*1000;yy=0.00078789815

17、8189; 0.00075850435876; 0.00083282260478; 0.00067808241639; 0.00085279036802; 0.00072900768452; 0.00080072481819; 0.00075095576213; 0.00065762725379; 0.00081186148545; 0.00079783727034; 0.00073060712986; 0.00063242006530; 0.00063656524495; 0.00080659845639; 0.00067704740069; 0.00076573767706; 0.0005

18、4955759081; 0.00058487913971; 0.00055602747368; 0.00033550412588; 0.00056012688452; 0.00056694491978; 0.00059380631025; 0.00053681916544; 0.00065871960781; 0.00068598344328; 0.00060922471348; 0.00057086018918; 0.00041308535708; 0.00047302026281; 0.00030949309972; 0.00040552624986; 0.00037545033142; 0.00017282319262; 0.00020758327980; 0.00037

温馨提示

  • 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

评论

0/150

提交评论