09卡尔曼滤波器的应用

1、2. 5. 1卡尔曼滤波器在雷达跟踪系统中的应用1. 雷达跟踪原理运动方程利用飞行目标对雷达发射脉冲的反射，根据回波脉冲与发射脉冲的时 间间隔，确定目标的径向距离，方位角和速度等状态参数，从而达到跟踪目 标的目的.设时刻，目标距离 A)+Q,径向速度“)； 方位角&S),方位角速度0(砒；其中，Qo表示平均距离，卩)表示距离偏离量.经过T秒后，到达时刻n + 目标的上述参数相应为为：距离Qo+qG + 1),径向速度QS + 1)；方位角&S + 1),方位角速度妙+ 1);若环是太大，则有下列近似的距离方程和方位角方程： fyO(M + l) = /?(«)+ ?/

2、?(«)10(n + 1) =(9(h) + T(2.5.20)则经过T秒后，(2.5.21a)另设目标的径向加速度和方位角加速度为"5诩 目标的径向速度和方位角速度的变化量分别为(«) = T p(n) = p(n + 1)- p(n)u2 (h) = T- 0(n +1) - 3(n)或pn + 1) =" + ux (n)0(/1 +1) = 0n) + u2n)(2.5.21 b)径向速度和方位角速度的变化通常是由于大气扰动或发动机引擎拉力 瞬时变化引起的.下面的分析中，假定绚)和血)都是零均值平稳白噪声，即满足Eux (n)u (n +1) =

3、 0Eu2 («)«2 + 1)1 = 0Eu (n)w2() = 0«1(«)和2 (")的方差分别为Euf(n) = CFj2' Eu (n) = a;2. 卡尔曼滤波信号模型的状态方程引入状态变量：s )=、52(h) = pn)$3(砒=0(砒,sri) = O(n)于是式(25.20)和(2.5.21)表示的四个方程可表示为(S ( +1) = $ (n) + Ts2(n)s2(n +1) = 52(n) + ux (n)s3 (n +1) = s3 (n) + Ts4 (n)I 54(H + 1) = 54(H)+ U2(H

4、) 写成矩阵形式$(H + 1)s2(n +1)必(卅+ 1)为(n + 1)1000T10000100巾5)+uxrT)$3(死)0$4何u2(n)00T1(2.5.22)(2.5.23)(2.5.24)(2.5.25)吩)的自相关函数。(2.5.26)(2.5.27)(2.5.28)(2.5.29)(2.5.30)由此得到卡尔曼滤波信号模型的状态方程 sn + 1) = As(n) + w(n)其中s(n) = s (n) 52(n)巾(>)卩 = q()p(n) 6>(n) ©S)TTw(n) = 0 ux (n) 0 u2 (n)0 10 0A_ 0 0 1 T0

5、 0 0 1Q(n) = Ew(n)wn)= f 0 0 0 o-f3.量测方程设雷达天线每T秒旋转一周并对目标的距离和方位角进行一次测量,测量噪声(即距离和方位角观测偏差)分别用儿)和冬何表示，得到测量 方程为(2.5.31)(2.5.32)%! (n) = s (n) + Vj (n) X2(H)= $3(H)+ V2 ()其中，气刑§是零均值高斯白噪声，方差分别为，和/ 以上二式给岀距离和方位角的估计值.写成矢量矩谓形式(2.5.33)(2.5.34)(2.5.35)(2.5.36)x(n) =Cs(n) + v(n)其中Tx(n) = xj (n) %! (n)Tv(n) =

6、 V1(n) v2 (n)10 0 00 0 10由式(25.35)可求出v(«)的自相关函数R(n) = £v(n)vT(/i)=0(2.5.37)4卡尔曼滤波按以下矢量卡尔曼滤波递推公式进行迭代运算:(2.5.38a)(2.5.38c)(2.5.38b)(2.5.38d)sn I n) = A(n)s(n 11 n 1) + Gn x(h) A(n)C(n)s(n 11 n 1) Gn = P(h)Ct (n)C(/i)P(n)CT S) + Rn)YxP(n) = A(ri)(n V)A1 (n) + 0() = Z-C(h)G(h)P(h)为了进行卡尔曼滤波，需要确

7、定加权系数矩阵G(n)的初值为此，应采用适当方法先确定均方误差矢量议喲初始值.根据雷达接收到的相邻两个回波脉冲，可以测得目标在初始时刻斤=1 和H = 四个数据：距离观测数据：坷和可(2)；方位角观测数据：勺和勺(2)根据测量方程，即式(2.5.31) 和式(2.5.32).倚一鼻.A爲"(2)§3&£(2)_02)孔2)由测量方程式(25. 33)可得坷勺(1)_00刃-“兀2(2)尢2-勺巾+儿必 v2(l)S (1) + 儿(1)必(1)+ 卩2(1)为耳气v2(2)5,(2) + (2)$3(2) + 卩2(2)(2.5.39)(2.5.40)(2

8、.5.41)将以上二式结果代入式(2.5.39),得(2.5.42)M+叩)1 (2) ® (1) + (2) - Vj (1) S(2)= $3 + 勺(2)亍$3(2) 一 $3 + v2 v2 (1)1另由状态方程式(2.5.26),并考虑到式(2.5.22)和(25.24),得陀)=耳(2) $2 比 *100001匕(1)r o _0$2+坷T比01丿4(1)_”2(1)_0010(2.5.43)$1(1) +吒2 $2(1)+坷 比(1) +吒454(1) + w2(1)可山1(2) 山(1)1 + 血(1) 必(2)丁 *(2)比+ 弘2(1)因此,£(2)-

9、§(2)=-U"1(1)一*儿(2) 儿(1) -v2(2)u2-M-儿由上式，可根据定义确定均方误差飢砒的初始值为歹二1 /T21 XT2丹2 2 2严+501 ZT2G0222尹内+”22 o因而可计算OS）值.为了得到人（砒直，需其中，b和巧通常是给定的， 指定坷和血的方差bj和&为简单起见，假定在各个方向上，加速度"包括径向加速度“)和方 位角加速度冰加服从均匀分布，在土M范围内，其概率密度函数因此，加速度方差=-M2u 3,于是可求得由于u(n) = Tp(n) : u2(n) = T<9(n)£=云丨氏Q5)=oooo0心00o

10、oo0000“7加5.举例设Pq = 160km,雷达天线旋转同期T = 15s,目标最大加速度M = 2. Im/s2. 雷达测距误差均方根值等于1 km, Tb；二10m;雷达方位角测量误差均 方根值等于1°或0.017弧度.由b；和可算出矩阵/?;由(可算出:, b； =1.3x10迭代计算§(斤)，戶和g4 :(1)求得§的初始值:歹二6.7 xlO406.7 xlO40.9 xlO400002.9 xlO-41.9x10001.9x102.6x10"(2)由g计算戶:P(3) = 4g(2)4+ 0_5xl062xl05002xl059.3 xlO3000014.5 x IO-45.8x10005.8 x IO-52.6x10（3）由尸计算qG3 =P(3)CtCP(3)Ct +町11.330_3.3x10-2001.3303.3x10-2（4）由戶和Gs计算歹:（3） = Z-CG3P（3）再返回步骤（2）,确良迭代计算下一时刻的尸（4） ,二.迭代计算彳（