第二章-状态估计基础

1、第二章状态估计基础状态估计的目的是对目标过去的状态进行平滑、对目标现在的运动状态进行滤波和对目标未来的运动状态进行预测。这些运动状态包括目标位置、速度、加速度等。本章讨论在多传感器跟踪系统中广泛应用的状态估计技术，这些技术包括Kalman滤波技术，滤波与滤波技术、最小二乘滤波技术和非线性系统的状态估计技术。2.1 线性系统估计卡尔曼滤波技术2.1.1 线性系统描述1.离散时间线性动态系统的状态方程线性系统采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的一般状态方程可描述为其中，是时刻目标的状态向量， 是过程噪声，它是具有均值为零、方差矩阵为的高斯噪声向量，即（：狄拉克函数，或单位冲

2、激函数），是状态转移矩阵，是过程。2. 传感器的测量（观测）方程传感器的通用观测方程为这里，是传感器在时刻的观测向量，观测噪声是具有零均值和正定协方差矩阵的高斯分布测量噪声向量，即3初始状态的描述初始状态是高斯的，具有均值和协方差，即以上描述比较抽象，下面结合具体的例子加以说明。例1：目标沿轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。解：目标的状态为用表示时间间隔，表速度噪声，则有写成矩阵形式为令有其中：均值为0，方差为的高斯噪声。例2：目标为二维空间中作匀速直线运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为二维空间作匀速直线运动的目标，目标的状态中有目

3、标的位置和目标的速度，那么目标的状态可写为用表示时间间隔，分别表示方向的加速度噪声，则有写成矩阵的形式有令， ，有假定为均值为0，方差分别为和的相互独立的高斯白噪声，则例3：目标为沿轴做匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程解：目标的状态可写为：用表示时间间隔，分别表示方向的加速度噪声，则有写成矩阵的形式有令有其中，为方向加速度噪声的方差。例4：在例3的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为其中目标的状态方程可写为其中

4、而过程噪声协方差矩阵为分别为方向，方向和方向加速度噪声方差。例5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为由题意得目标的观测方程为其中分别为和方向的观测噪声。将上式写成矩阵的形式，有令，则有假定为均值为零，方差分别为的高斯白噪声，则例6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，三维空间中作匀速直线运动的目标，其状态向量为由题意得目标的观测方程为其中分别为和方向的观测噪声。将上式写成矩阵的形式，有有假定

5、为均值为零，方差分别为的高斯白噪声，则作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。例7：设目标沿轴匀速直线运动，目标的状态可表示为，在时刻的观测值为，在时刻的观测值为，采样间隔为，求目标的初始状态和初始协方差。解：初始状态为初始协方差其中，为观测噪声的方差，即：,滤波器从时开始工作。2.1.2 Kalman滤波算法状态估计的一步预测方程为一步预测的协方差为预测的观测向量为观测向量的预测误差协方差为新息或量测残差为滤波器增益为Kalman滤波算法的状态更新方程为滤波误差协方差的更新方程为2.2转换坐标卡尔曼滤波器（非线性估计技术）。卡尔

6、曼滤波器两个要求：1) 目标的状态方程是线性的；2）观测方程是线性的。在实际的情况下，要同时满足这两个要求是困难的。通常情况下：状态方程在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。如雷达的测量是距离、方位角和高低角。从直角坐标系来看，观测方程是非线性的。假定雷达的测量为距离、方位角和高低角，测量与目标位置的关系为这是一个非线性关系。为解决测量方程非线性情况下的目标跟踪问题，可采用两种方法，一种是采用扩展的卡尔曼跟踪滤波器，这将在后面介绍；另一种是对测量进行坐标转换，将极坐标下的测量转化为直角坐标系下的测量，然后用标准的卡尔曼滤波器进行跟踪。对三坐标雷达，坐标转换公式为

7、噪声方差的转换公式为 假设距离、方位角和高低角的测量噪声为相互独立的高斯白噪声，即并且对两坐标雷达，极坐标系下的观测为（），转换成直角坐标为 方差的转换：为求方差的转换，先求偏导于是，得在得到直角坐标系下的测量及测量的协方差矩阵后，可以用前面讲的方法进行目标跟踪。这种变换存在的问题是：在直角坐标系下，测量的噪声不再是严格意义上的高斯噪声，并且噪声的分布中心并不是以直角坐标为中心，而且在直角坐标系下噪声是相互关联的。 由于噪声的分布中心并不是以直角坐标为中心，因此，噪声是有偏的，为此，有些学者提出了无偏坐标转换的方法：对转换后的直角坐标和方差进行校正。通常情况下，传感器的测量为极坐标测量，极角坐

8、标测量可通过无偏坐标转换变为直角坐标测量。设，则无偏转换的测量为 其中，。转换测量的协方差为 其中在动态方程为线性、观测方程为非线性的情况下，可使用转换坐标卡尔曼滤波器，转换可采用两种方法：一种是不带校正量的转换，这是一种相对简单的方法；另一种为带校正量的方法，这是一种相对复杂的方法。2.3扩展卡尔曼滤波器(非线性估计技术)采用扩展的卡尔曼滤波器进行目标跟踪。2.3.1 系统的状态方程和测量方程状态方程：其中为状态转移矩阵。测量方程：其中2.3.2、观测方程线性化将等号后的第一项以为中心按泰勒级数展开并略去二级以上的高阶分量，有代入上式，得令，于是得到线性化的观测方程为假定目标的状态为,而，则

9、为的雅可比矩阵(Jacobian矩阵)。例：雷达的观测是在极坐标系下进行的，对于一个直角坐标为的目标，雷达所测的三个极坐标分别为观测方程为令目标的状态为，则线性化的观测矩阵为其中例：红外传感器,观测值为方位角和高低角 ，求目标的状态为时线性化的观测矩阵解：因2.3.3、扩展卡尔曼滤波方程一步预测：一步预测协方差矩阵预测的观测向量为 观测矩阵新息或量测残差为残差协方差矩阵 滤波器增益 滤波输出误差的协方差阵例题2-2：利用扩展的卡尔曼滤波算法对y轴上匀速运动的目标进行跟踪。设传感器平台在x-y平面内运动，运动方程为式中，t为时间，和为相互独立的、零均值白色高斯噪声，其方差分别为，且与过程噪声和量

10、测噪声相互独立。过程噪声为加速度噪声，其方差为，传感器测量目标的方位角，采样时间间隔，测量噪声为零均值的高斯噪声，其方差为。解：目标在y轴上运动，系统的状态方程为其中，传感器的测量方程为其中，建立目标位置估计值和方位角测量值之间的关系(不考虑测量误差)为所以, 由目标的初始状态对上式求导可得于是初始状态的方差线性化的观测矩阵2.4 无迹卡尔曼滤波器（Unscented Kalman Filter, UKF）目前，扩展卡尔曼滤波虽然被广泛用于解决非线性系统状态估计问题，但其滤波效果在很多复杂的系统中并不能令人满意。模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波器发散。另外，在许多实际

11、的应用中，模型的线性化过程比较繁杂，而且也不易得到。无迹卡尔曼滤波是由于不需要对非线性系统进行线性化，并可以很容易地应用于非线性系统的状态估计，因此，UKF方法在许多方面得到应用。2.4.1 无迹变换无迹卡尔曼滤波是在无迹变换的基础上发展起来的。无迹变换（Unscented Transformation, UT）的基本思想是由Julier等首先提出的，是用于计算经过非线性变换的随机变量统计的一种新方法。它不需要对非线性状态和测量方程模型进行线性化。假定X为一个维的随机变量，为一非线性函数，并且。的均值和协方差分别为和。无迹变换的一般步骤如下：（1） 确定()个采样点其中，为一尺度参数，可以为的

12、任意值。为的第i列。为状态向量X的维数。（2） 每个采样点通过非线性函数传播，得到；（3）的估计值和协方差估计如下2.4.2 无迹Kalman滤波算法根据无迹变换的特点，Kalman滤波算法可归纳如下：假定状态方程：观测方程：k时刻的状态估计值和状态协方差：和k时刻的观测向量（1） 根据采样规则，确定2+1个采样点以及相应的加权值其中，为尺度参数，可以为的任意值。为均方根的第i列。为状态向量的维数。 （2） 一步预测； ； （3） 更新状态在动态方程和观测方程同时为非线性、或者一个为线性另一个为非线性的情况下，可以使用无迹卡尔滤波器，无迹卡尔曼滤波器的缺点是计算量较大。与无迹卡尔滤波器类似的滤

13、波器还有积分卡尔滤波器（QKF）和粒子滤波器，这些滤波器均可用于非线性系统，其缺点是计算量更大。2.5 和滤波器和滤波器分别用于对匀速和匀加速目标进行跟踪。2.5.1、滤波器滤波器用于对匀速运动的目标进行跟踪。目标的状态方程为式中，目标的观测方程式中，滤波方程问题：如何求和，利用稳态滤波器的特点，即得其中，为目标的机动系数。状态估计状态估计的协方差为一步预测协方差为以上只讨论的是沿轴作直线运动情况下的滤波，对于平面或空间目标，需要分别对、及上的状态向量分别滤波。当过程噪声方差不能事先确定时，目标的机动系数无法确定，和两参数无法确定。工程上经常采用两种的方法来确定和：1、常系数法：和取固定的值一

14、般取(1) (临界阻尼法)(2) (最佳选择法)2、变系数法：这里从1开始计数。对来说时才有值，但滤波器从开始工作，前两个点用于确定目标的初始位置和速度，完成航迹起始。2.5.2 滤波器滤波器用于对匀加速运动的目标进行跟踪。目标的状态方程为式中，目标的观测方程式中，滤波方程问题：如何求、和，利用稳态滤波器的特点，即得三个非线性方程构成的方程组 其中，为目标的机动系数。解线性方程组，得、和。状态估计状态估计的协方差为以上只讨论的是沿轴作直线运动情况下的滤波，对于平面或空间目标，需要分别对、及上的状态向量分别滤波。当过程噪声方差不能事先确定时，目标的机动系数无法确定，、和三参数无法确定。工程上经常

15、采用两种的方法来确定、和1、常系数法：和取固定的值一般取2、变系数法：这里从1开始计数。对来说时才有值，对于来说时才有值，但滤波器从开始工作，前三个点用于确定目标的初始位置、速度和加速度，完成航迹起始。2.6 最小二乘估计和最小二乘滤波器2.6.1、线性最小二乘估计模型：其中， ；目标：求以使方法：得到估计：估计误差的方差：其中例：设目标作匀速直线运动，目标的状态可表示为，目标的起始状态为，目标的起始时刻为，在时刻的观测值为，在时刻的观测值为，观测噪声为相互独立的高斯白噪声，分别求和时刻目标状态的最小二乘估计。a、求时刻目标状态的估计值解：得到估计：估计误差的方差：其中b、求时刻目标状态的估计

16、值得到估计：估计误差的方差：其中2.6.2、加权最小二乘估计 当测量数据的测量精度不等时，应采用加权处理，对精度较高的测量结果赋以较大的权。目标：求以使得到估计：估计误差的方差：其中2.6.3、马尔可夫估计马尔可夫估计是一切加权最小二乘估计中能使得估计方差阵达到最小的一种。当取时，加权最小二乘估计为马尔可夫估计估计值为：其估计误差的方差为2.6.4、非线性最小二乘估计模型：目标：求以使另一种表示加权最小二乘估计目标：令，非线性加权最小二乘简化为非线性最小二乘。非线性最小二乘估计可以用Gauss-Newton算法或Levenberg-Marquart算法（这些算法在Matlab中都已编好，直接应

17、用就行，如lsqnonlin，当然也可自行编写）求解。这里给出一种自我实现的方法。令则Levenberg-Marquart算法求解非线性最小二乘的迭代公式为 令一种伪语言描述的LM算法如下：;e1:=10-15;e2:=10-15; e3:=10-15;kmax=100;k:=0;v:=2;p=X0;A:=JT*J;Ep:=Y-F(p);g:=JT*Ep; Stop:=(); ;while(not stop)and(kkmax)k:=k+1; repeat Solve (A+I)*=gif () stop:=true;elsepnew:=p+;if p:=pnew;A:=JT*J;Ep:=Y-

18、F(p);g=:JT*Ep; Stop:=() or ;v:=2;elseendifendifuntil()or(stop)endwhileX:=p;非线性最小二乘估计也是一种最大似然估计，是一种最优估计，估计误差的方差能达到CRLB，因此估计误差的方差可取为CRLB。估计误差的方差为例：匀速运动目标，目标的状态为，时刻的测量为，时刻的测量为，传感器测距误差标准差为=10m,测角误差标准差为=0.001rad, 求时刻目标的状态。设时刻为，则时刻目标的状态为时刻的理论观测值为时刻的理论观测值为加权非线性最小二乘估计的目标函数为时刻目标状态的估计为Matlab求Jacobian矩阵的方法syms

19、 x xv y yv dt std_r std_b r1 b1 r2 b2y1=(r1-sqrt(x+dt*xv)2+(y+dt*yv)2)/std_r;y2=(b1-atan(y+dt*yv)/(x+dt*xv)/std_b;y3=(r2-sqrt(x2+y2)/std_r;y4=(b2-atan(y/x)/std_b;J=jacobian(y1;y2;y3;y4,x xv y yv);J=simplify(J)例2-3 目标为二维空间中作匀速直线运动的目标，传感器为2维雷达，测量目标的距离和方位角，传感器位置为，其测距误差标准差为100 m,测角误差标准差为0.001弧度。在时，观测值为4

20、9999.7, 1.，在时，观测值为51111.3, 1.，在时，观测值为52043.8, 1.，分为用线性最小二乘和非线性最小二乘法估计目标的初始状态和状态估计的协方差。2.6.5、最大似然和非线性最小二乘的转化例：匀速运动目标，目标的状态为，时刻的测量为，时刻的测量为，传感器测距误差标准差为=10m,测角误差标准差为=0.005rad, 求时刻目标的状态的最大似然估计。正态分布概率密度函数假定观测为相互独立的0均值高斯白噪声。测量为的概率密度函数为的概率密度函数为测量为和测量同时出现的联合概率密度函数（似然函数）为对数似然2.6.6、最小二乘滤波器模型：状态方程观测方程求按最小二乘估计的方

21、法，先确定与间的关系得到最小二乘估计其中， ；取加权矩阵，作用是使过老数据的作用逐渐消失。取则加权最小二乘估计为其中现在假设知道，求，于是就形成了最小二乘滤波器。加权最小二乘滤波器的递推公式为 其中，。例：设目标为匀速运动模型，即状态向量为观测为目标在直角坐标下的位置，写出最小二乘滤波器中的状态转移矩阵及观测矩阵。解：转移矩阵为其中观测矩阵为时刻的观测例2-4 目标为二维空间中作匀速直线运动的目标，其初始状态为, 过程噪声的方差q=1。传感器为2维雷达，测量目标的距离和方位角，传感器位置为，其测距误差标准差为100 m,测角误差标准差为0.001弧度，采样时间间隔为10 s。试：1、根据已知条件产生仿真的观测数据。2、利用仿真的观测数据分别用变系数滤波器、转换坐标Kalman滤波器、最小二乘滤波器和扩展Kalman滤波器进行目标跟踪，并在同一图上绘制出跟踪结果。2.7 空间配准和坐标变换在对坐标位置的不同传感器所获得的数据进行处理时，首先要作的是将测量数据转换到公共的坐标系下，然后才能对数据进行处理。通常的坐标系有以下4种：（1） 以本传感器位置为中