第二章状态估计基础

资源描述

《第二章状态估计基础》由会员分享，可在线阅读，更多相关《第二章状态估计基础（44页珍藏版）》请在金锄头文库上搜索。

1、第二章状态估计基础状态估计的目的是对目标过去的状态进行平滑、对目标现在的运动状态进行滤波和对目标未来的运动状态进行预测。这些运动状态包括目标位置、速度、加速度等。本章讨论在多传感器跟踪系统中广泛应用的状态估计技术，这些技术包括 Kalman 滤波技术，滤波与滤波技术、最小二乘滤波技术和非线性系统的状态估计技术。线性系统估计卡尔曼滤波技术线性系统描述1 .离散时间线性动态系统的状态方程线性系统采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的一般状态方程可描述为X(k 1) (k)X (k) G(k)V(k)其中，X(k) Rn是k时刻目标的状态向量，V(k)Rn是

2、过程噪声，它是具有均值为零、方差矩阵为 Q(k) 的高斯噪声向量，即EV(k) 0EV(k)VT(l)Q(k) kl(:狄拉克函数，或单位冲激函数)，(k)Rn n是状态转移矩阵，G(k) Rn n是过程。2 . 传感器的测量(观测)方程传感器的通用观测方程为Z(k) H(k)X(k) W(k)这里，Z(k) Rm是传感器在k时刻的观测向量，观测噪声W(k) Rm是具有零均值和正定协方差矩阵R(k) 的高斯分布测量噪声向量，即EW(k) 0EW(k)WT (k) R(k) kl3初始状态的描述初始状态X(0)是高斯的，具有均值 /0|0)和协方差P(0|0),即E X(0) X?(0| 0 X

3、(0) X?(0|0T P(0 |0)以上描述比较抽象，下面结合具体的例子加以说明。例 1：目标沿 x 轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。解：目标的状态为X(k)x(k)x(k)用 T 表示时间间隔， u x 表速度噪声，则有x(k 1)x(k) Tx(k) Tux (k)x(k 1) x(k) ux (k)写成矩阵形式为x(k 1)x(k 1)1T01x(k)x(k)T1 ux(k)(k)1TTG 1, V(k) ux(k)X(k 1)(k)X(k) G(k)V(k)其中Q(k) EV(k)V T (k) Eux2 (k)qux(k) ：均值为0，方差为q 的高斯噪

4、声。例 2：目标为二维空间中作匀速直线运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为二维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为x(k)x(k)TX (k)x(k) x(k) y(k) y(k) Ty(k)y(k)用T表示时间问隔，Ux，Uy分别表示x,y方向的加速度噪声，则有x(k1)x(k)Tx(k)gux(k)x(k1)x(k)Tux(k)T2y(k1)y(k)Ty(k).(k)y(k1)y(k)Tuy(k)写成矩阵的形式有T2x(k 1)1 T 0 0 x(k) 0Ux(k)Uy(k)2x(k1)0100x(k)T

5、02y(k1)00 1Ty(k)0T_y(k1)00 01y(k)0Tx(k)人x(k)令 X(k),(k)y(k)y(k)0 10 00 0 1 TG(k)T20T2V(k)Ux(k)Uy(k)X(k 1)(k)X(k) G(k)V(k)Q(k) EV(k)VT(k)Ux(k)uy(k)Ux(k)Uy(k)_2一E(ux(k)E(ux(k)uy(k)_ 2E(Ux(k)Uy(k)E(Uy(k)假定ux(k),Uy(k)为均值为0,方差分别为qx和qy的相互独立的高斯白噪声，则E(u2(k)E(ux(k)uy(k)qx0Q(k)2-E(ux(k)uy(k)E(u2(k)0qy例3:目标为沿x轴

6、做匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程解：目标的状态可写为：x(k)X1(k) x(k)x(k)用T表小时间问隔，口*,口丫,“分别表示x, y,z方向的加速度噪声，则有x(k 1)x(k) Tx(k)T2x(k)T2Ux(k)x(k 1)x(k) Tx(k) Tux(k)x(k 1) x(k) Ux(k)0.5T2写成矩阵的形式有x(k1)1T0.5T2x(k)0.5Tx(k1)01Tx(k)Tx(k1)001x(k)12Ux(k)0.5T21(k),G(k),S(k) ux(k)X1(k1)1(k)X1(k) G1(k)S(k)其中,Q1(k)EV(k)VT(k)

7、 Eu2(k) qxqx为x方向加速度噪声的方差。例4:在例3的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为X1（k）X（k） X2（k）X3（k）其中x(k)y(k)z(k)X1(k)x( k),X2(k)y(k),X3(k)z(k)x(k)y(k)z(k)目标的状态方程可写为X(k 1)(k)X(k) G(k)V(k)其中(k)00G1(k)00ux(k)01(k)0,G(k)0G1(k)0,V(k)uy (k)001(k)00G1(k)uz

8、(k)1(k)而过程噪声协方差矩阵为qxQ(k) 0qx,qy,qz分别为x方向,y 方向和 z 方向加速度噪声方差。0qy0qz0例 5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为x(k)x( k)X(k)y(k)y(k)由题意得目标的观测方程为zx(k) x(k) wx(k) zy (k) y(k) wy (k)其中Wx(k),Wy(k)分别为x和y方向的观测噪声。将上式写成矩阵的形式，有x(k)zx(k)1000x(k)wx(k)zy(k)0010y(k)wy(k)y(k

9、)zx(k)令 Z(k),H (k)zy(k)10000010,W(k)wx(k)wy(k)Z(k) H(k)X(k) W(k)R(k) E W(k)W T(k)wx(k)Ewx(k) wy(k)wy(k)2E wx (k)E wx(k)wy(k)2E wx(k)wy(k) E wy (k)假定Wx(k),Wy(k)为均值为零，方差分别为rx,ry的高斯白噪声，则R(k)2E wx (k) E wx(k)wy(k)2E wx(k)wy(k)E wy (k)rx 00ry例 6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，三

10、维空间中作匀速直线运动的目标，其状态向量为x(k) x(k) y(k) X(k)y(k) z(k) z(k)由题意得目标的观测方程为zx(k)x(k)wx(k)zy(k)y(k)wy(k)zz(k)z(k)wz(k)其中Wx(k), Wy(k),Wz(k)分别为x, y和z方向的观测噪声。将上式写成矩阵的形式，有x(k)x(k)zx(k)100000wx(k)y(k)zy(k)001000wy(k)y(k)zz(k)000010wz(k)z(k) z(k)有Z(k) H(k)X(k) W(k)假定Wx(k),Wy(k),Wz(k)为均值为零，方差分别为rxJy/z的高斯白噪声，则rx00R(k

11、) 0 ry 0 00rzx x T ，在 t0 时刻作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。例 7：设目标沿x 轴匀速直线运动，目标的状态可表示为 X的x观测值为z(0),在L时刻的x观测值为z(1),采样间隔为T ti tc求目标的初始状态和初始协方差解：初始状态为z(1)*(111)z(1) z(0)r P(111)r/T一 2初始协方差 r/T 2r/T其中，r为观测噪声的方差，即：W(k) N(0,r),滤波器从k 2时开始工作Kalman滤波算法状态估计的一步预测方程为X(k 1|k)(k)X7k|k)一步预测的

13、(k 1)S(k 1)KT(k 1)I K(k 1)H(k 1) P(k 1|k)转换坐标卡尔曼滤波器（非线性估计技术）。卡尔曼滤波器两个要求：1）目标的状态方程是线性的；2）观测方程是线性的。在实际的情况下，要同时满足这两个要求是困难的。通常情况下：状态方程在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。如雷达的测量是距离、方位角和高低角。从直角坐标系来看，观测方程是非线性的。假定雷达的测量为距离r、方位角和高低角，测量与目标位置的关系为y arctan xarctan 22.x y这是一个非线性关系。为解决测量方程非线性情况下的目标跟踪问题，可采用两种方法，一种是采用扩展的卡尔曼跟踪滤波器，这将在后面介绍；另一种是对测量进行坐标转换，将极坐标下的测量转化为直角坐标系下的测量，然后用标准的卡尔曼滤波器进行跟踪。对三坐标雷达，坐标转换公式为 r r cos cosxr cos sinyr sinz噪声方差的转换公式为VrVrVVxyzVcos coscos sinVxvyVzx x xr上上rz z zrr cos sin r cos cosr sin cos r sin sinsin0r cosVr Vrv

展开阅读全文