卡尔曼滤波两例题含matlab程序Word文档格式.docx
《卡尔曼滤波两例题含matlab程序Word文档格式.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波两例题含matlab程序Word文档格式.docx（21页珍藏版）》请在冰豆网上搜索。
14
1.6867
1.6036
1.5092
1.4076
1.2944
1.1724
1.0399
15
16
17
18
19
20
速度协方差
从以上的结果，可以得到高度和速度的估计值，再通过所得到的高度协方差和速度协方差，可见用卡尔曼滤波法，虽然刚开始的初始高度协方差很大为100，但通过2步之后减小到不超过1，逐渐接近于0，同样的速度协方差刚开始的时候也比较大，为2，但是通过5步之后迅速减小，到10步之后接近于0。
2.有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；
1）初始条件改变时，改变初始高度值，和速度值
由实验结果分析可得
度滤波值和速度滤波值在开始几步接近初始值，协方差值基本不变。
2）当初始协方差值改变时，改为
实验结果分析
高度和速度滤波值基本不变，速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。
但是经过几步之后，都趋于0。
二．同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所示。
如果，且雷达测距和测角的测量噪声是高斯白噪声随机序列，均值为零、方差阵，试根据下列测量数据确定物体的高度和速度随时间变化的估计值。
时间[s]*1000斜距[km]俯仰角[rad]*1000
0.000500000000002.827416437818910.00075850435876
0.001000000000002.825198117297710.00083282260478
0.001500000000002.820666869662360.00067808241639
0.002000000000002.814872331059010.00085279036802
0.002500000000002.806717865362440.00072900768452
0.003000000000002.797252689740890.00080072481819
0.003500000000002.786642734750390.00075095576213
0.004000000000002.773203650263130.00065762725379
0.004500000000002.759195354645510.00081186148545
0.005000000000002.743312886281950.00079783727034
0.005500000000002.725388884828120.00073060712986
0.006000000000002.706649677123120.00063242006530
0.006500000000002.686324034064730.00063656524495
0.007000000000002.663865338522200.00080659845639
0.007500000000002.640935297073330.00067704740069
0.008000000000002.616211117273570.00076573767706
0.008500000000002.590381098507850.00054955759081
0.009000000000002.562987942728430.00058487913971
0.009500000000002.534983179507970.00055602747368
.010*********
0.011000000000002.445606760009820.00056694491978
0.011500000000002.414036907720880.00059380631025
0.012000000000002.382522286116960.00053681916544
0.012500000000002.350165011823320.00065871960781
0.013000000000002.317909398371370.00068598344328
0.013500000000002.285976166564530.00060922471348
0.014000000000002.254184316814010.00057086018918
0.014500000000002.222593202195350.00041308535708
0.015000000000002.192373989694660.00047302026281
0.015500000000002.162901779972710.00030949309972
0.016000000000002.134********7060.00040552624986
0.016500000000002.108110646907270.00037545033142
0.017000000000002.083221798231950.00017282319262
0.017500000000002.061481090267670.00020758327980
0.018000000000002.042198850940310.00037186464579
0.018500000000002.026102353143570.00018082163465
0.019000000000002.012903268635790.00023323830160
0.019500000000002.00463157388395-0.00004536186964
.020*********
雷达
物体
V
h
d0
斜距
俯仰角
题2示意图
解：
1.
令t=0.5Q（k）=0根据离散时间扩展卡尔曼滤波公式，则有：
状态方程：
＝
测量方程：
辅助方程：
一步预测：
滤波增益：
滤波计算：
滤波初值：
2.实验结果
高度随时间变化估计
速度随时间变化的估计
高度协方差：
速度协方差估计
实验结果分析：
根据图，可得高度和速度的估计值，通过扩展卡尔曼滤波法，高度协方差和速度协方差，刚开始的值比较大，但是迅速减小，在几步之后逐渐趋近于0。
3.有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；
1）初始条件发生变化，改变高度和速度的初始值为
分析：
高度和速度在刚开始的时候波动比较大，不过经过10步之后，逐渐趋于平稳，高度协方差收敛变快，速度协方差基本不变
2）初始噪声改变，
高度和速度滤波值基本不变，速度协方差也基本不变，高度协方差刚开始的时候有波动，10步之后趋于稳定。
附源程序：
第一题
t=1;
A=[1-t;
01];
g=9.8;
U=[-0.5*g*t^2;
g*t];
C=[10];
R=1;
I=[10;
X=zeros（2,1）;
K=zeros（2,1）;
P=zeros（2,2）;
P=[1500;
010];
X=[1900;
10]
y=[19001994.51979.41955.41921.41877.71825.01759.81686.71603.61509.21407.61294.41172.41039.9898.0745.5585.0412.5231.8399];
fori=2:
21
Pi=A*P*A'
;
%一步预测
Xi=A*X+U;
K=Pi*C'
*（C*Pi*C'
+R）^（-1）;
%增益
X=Xi+K*（y（i）-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）;
end
he
（1）=1900;
ve
（1）=10;
ph
（1）=150;
pv
（1）=10;
figure;
t=1:
21;
plot（t,he（t）,'
r'
）;
title（'
高度滤波值'
）
plot（t,ve（t））;
速度滤波值'
plot（t,ph（t））;
高度协方差'
plot（t,pv（t））;
速度度协方差'
第二题
d=1995;
t=0.5;
A=[100;
01-t;
001];
%表示状态方程中的fai
U=[0;
-0.5*g*t^2;
%表示状态方程中的u
R=[1.040;
01.01];
%表示测量噪声
I=[100;
010;
P=[500;
050;
002];
X=[1995;
2005;
1];
%滤波初值
y=[2.8284;
2.82741643781891;
2.82066686966236;
2.81487233105901;
2.80671786536244;
2.79725268974089;
2.78664273475039;
2.77320365026313;
2.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.35016501182332;
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
升级会员 