卡尔曼滤波两例题含matlab程序

含两个递进例题,有详细解释,以及matlab程序

设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，该物体的初始高度h0和速度V0也是高斯分布的随机变量，且

Eh0 1900m h0 1000

。试求该物体高度 ,P var 0

EV0 10m/s V0 02

和速度随时间变化的最优估计。（g 9.80m/s2） 解：

1. 令X(k)

h(k)

t=1 R（k）=1 Q(k)=0 v(k)

根据离散时间卡尔曼滤波公式，则有： X(k 1) (k 1,k)X(k) U(k) Y(k 1) H(k 1)X(k 1) V(k 1)

0.5gt2 1 t

k＝) (k 1, U(k)＝ ＝ 10 H(k 1) 1 gt

1900

滤波初值：X(0|0) EX(0)

10

^

P(0|0) var[X(0)] P0

100

2

一步预测：X(k 1|k) (k 1,k)X(k|k) U(k) P(k 1|k) (k 1,k)P(k|k) (k 1,k)

滤波增益：K(k 1) P(k 1|k)H(k 1)[H(k 1)P(k 1|k)H(k 1) R(k 1)] 滤波计算：X(k 1|k 1) X(k 1|k) K(k 1)[Y(k 1) H(k 1)X(k 1|k)] P(k 1|k 1) [I K(k 1)H(k 1)]P(k 1|k) 2. 实验结果

^

^

^

^^

T

TT 1

含两个递进例题,有详细解释,以及matlab程序

高度随时间变化估计

速度随时间变化的最优估计

高度协方差

含两个递进例题,有详细解释,以及matlab程序

速度协方差

从以上的结果，可以得到高度和速度的估计值，再通过所得到的高度协方差和速度协方差，可见用卡尔曼滤波法，虽然刚开始的初始高度协方差很大为100，但通过2步之后减小到不超过1，逐渐接近于0， 同样的速度协方差刚开始的时候也比较大，为2，但是通过5步之后迅速减小，到10步之后接近于0。

3. 有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；

Eh0 2300m 1）初始条件改变时，改变初始高度值，和速度值 30m/s

EV 0

含两个递进例题,有详细解释,以及matlab程序

含两个递进例题,有详细解释,以及matlab程序

由实验结果分析可得

度滤波值和速度滤波值在开始几步接近初始值，协方差值基本不变。

2）当初始协方差值改变时，改为P0 var

h0 1500

V0

010

含两个递进例题,有详细解释,以及matlab程序

实验结果分析

高度和速度滤波值基本不变，速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。但是经过几步之后，都趋于0。

二．同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所

m Ed0 1995 d0 500

,P var h 050 ，且雷达测距和测角的测量噪声

m示。如果 Eh0 2005 0 0

1m/s 002 EV0 V0

是高斯白噪声随机序列，均值为零、方差阵R

0.040

，试根据下列测量数据确定 0.01 0

物体的高度和速度随时间变化的估计值。

时间[s]*1000 斜距[km] 俯仰角[rad]*1000 0.00050000000000 2.82741643781891 0.00075850435876 0.00100000000000 2.82519811729771 0.00083282260478 0.00150000000000 2.82066686966236 0.00067808241639

含两个递进例题,有详细解释,以及matlab程序

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.00400000000000 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 0.00700000000000 0.00750000000000 0.00800000000000 0.00850000000000 0.00900000000000 0.00950000000000 0.01000000000000 0.01050000000000 0.01100000000000 0.01150000000000 0.01200000000000 0.01250000000000 0.01300000000000 0.01350000000000 0.01400000000000 0.01450000000000 0.01500000000000 0.01550000000000 0.01600000000000 0.01650000000000 0.01700000000000 0.01750000000000 0.01800000000000 0.01850000000000 0.01900000000000 0.01950000000000 0.02000000000000

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.00058143251913 0.00063656524495 0.00080659845639 0.00067704740069 0.00076573767706 0.00054955759081 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.00037186464579 0.00018082163465 0.00023323830160 -0.00004536186964 0.00003246284068

含两个递进例题,有详细解释,以及matlab程序

解： 1.

d

0.04 l(k) 令X(k) h(k) t=0.5 R(k) Q(k)=0 根据离散时间Y(k) 0.01 (k) v(k)

扩展卡尔曼滤波公式，则有：

0

2 gt 状态方程：X(k 1) (k 1,k)X(k) U(k) W(k) U(k) 0.5 gt

1

1 t (k 1,k)＝ 1

V(k)

测量方程：Y(k 1) arctanh(k)

d

辅助方程：

含两个递进例题,有详细解释,以及matlab程序

^

h[X(k 1),k 1]H(k 1) | ^

x(k 1) x(k 1|k)

X(k 1)

d ^2

2 h(k 1|k) d

^0 h(k 1|k)

^2

2 h(k 1|k) d

一步预测：X(k 1|k) (k 1,k)X(k|k) U(k) P(k 1|k) (k 1,k)P(k|k) T(k 1,k)

滤波增益：K(k 1) P(k 1|k)HT(k 1)[H(k 1)P(k 1|k)HT(k 1) R(k 1)] 1 滤波计算：X(k 1|k 1) X(k 1|k) K(k 1)[Y(k 1) h(X(k 1|k),k 1)]

^

^

^

^^

P(k 1|k 1) [I K(k 1)H(k 1)]P(k 1|k)

1995 5

)滤波初值： X(0|0) 2005 P(0|0 1

^

5 2

2. 实验结果

高度随时间变化估计

速度随时间变化的估计

含两个递进例题,有详细解释,以及matlab程序

高度协方差：

速度协方差估计

含两个递进例题,有详细解释,以及matlab程序

实验结果分析：

根据图，可得高度和速度的估计值，通过扩展卡尔曼滤波法，高度协方差和速度协方差，刚开始的值比较大，但是迅速减小，在几步之后逐渐趋近于0。

3. 有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；

1995

1）初始条件发生变化，改变高度和速度的初始值为X(0|0) 2300 10

^

含两个递进例题,有详细解释,以及matlab程序

分析：高度和速度在刚开始的时候波动比较大，不过经过10步之后，逐渐趋于平稳，高度协方差收敛变快，速度协方差基本不变

含两个递进例题,有详细解释,以及matlab程序

2）初始噪声改变，R(k)

1

1

含两个递进例题,有详细解释,以及matlab程序

分析：高度和速度滤波值基本不变，速度协方差也基本不变，高度协方差刚开始的时候有波动，10步之后趋于稳定。

附源程序： 第一题

t=1;A=[1 -t;0 1];g=9.8;U=[-0.5*g*t^2;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;10]

y=[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: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('高度滤波值') 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('速度度协方差')

含两个递进例题,有详细解释,以及matlab程序

第二题 d=1995; t=0.5;

A=[1 0 0;0 1 -t;0 0 1]; %表示状态方程中的fai g=9.8;

U=[0;-0.5*g*t^2;g*t]; %表示状态方程中的u R=[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.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;

含两个递进例题,有详细解释,以及matlab程序

2.06148109026767; 2.04219885094031; 2.02610235314357; 2.01290326863579; 2.00463157388395; 2.00058143251913]; y=y*1000;

yy=[0.000787898158189; 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.00054955759081; 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.00037186464579;

含两个递进例题,有详细解释,以及matlab程序

0.00018082163465; 0.00023323830160; -0.00004536186964; 0.00003246284068]; yy=1000*yy; Pi=zeros(3,3); Xi=zeros(3,1); for i=2:41

Pi=A*P*A';%一步预测协方差 Xi=A*X+U;%一步预测状态

h=[sqrt(Xi(2,1)^2+d^2);atan(Xi(2,1)/d)];

H=[Xi(2,1)/sqrt(Xi(2,1)^2+d^2) d/sqrt(Xi(2,1)^2+d^2) 0;d/(Xi(2,1)^2+d^2) -Xi(2,1)/(Xi(2,1)^2+d^2) 0];

K=Pi*H'*inv((H*Pi*H'+R));%滤波增益 %滤波计算

X=Xi+K*([y(i);yy(i)]-h); he(i)=X(2,1); ve(i)=X(3,1); P=(I-K*H)*Pi; ph(i)=P(2,2); pv(i)=P(3,3); end

he(1)=2005; ve(1)=1; ph(1)=5; pv(1)=2;

figure;t=1:41;plot(t,he(t));title('高度滤波值') figure;t=1:41;plot(t,ve(t));title('速度滤波值') figure;t=1:41;plot(t,ph(t));title('高度协方差') figure;t=1:41;plot(t,pv(t));title('速度协方差')

本文来源：https://www.bwwdw.com/article/v03n.html