第二章-状态估计基础电子教案.doc

第二章 状态估计基础

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

其中，是时刻目标的状态向量， 是过程噪声，它是具有均值为零、方差矩阵为的高斯噪声向量，即

（：狄拉克函数，或单位冲激函数），是状态转移矩阵，是过程。
2. 传感器的测量（观测）方程
传感器的通用观测方程为

这里，是传感器在时刻的观测向量，观测噪声是具有零均值和正定协方差矩阵的高斯分布测量噪声向量，即

3．初始状态的描述
初始状态是高斯的，具有均值和协方差，即

以上描述比较抽象，下面结合具体的例子加以说明。
例1：目标沿轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。
解：目标的状态为

用表示时间间隔，表速度噪声，则有

写成矩阵形式为

令

有

其中

：均值为0，方差为的高斯噪声。
例2：目标为二维空间中作匀速直线运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。
解：由于目标为二维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为

用表示时间间隔，分别表示方向的加速度噪声，则有

写成矩阵的形式有

令，， ，
有

假定为均值为0，方差分别为和的相互独立的高斯白噪声，则

例3：目标为沿轴做匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程
解：目标的状态可写为：

用表示时间间隔，分别表示方向的加速度噪声，则有

写成矩阵的形式有

令

有

其中，

为方向加速度噪声的方差。
例4：在例3的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。
解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为

其中

目标的状态方程可写为

其中

而过程噪声协方差矩阵为

分别为方向，方向和方向加速度噪声方差。
例5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。
解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为

由题意得目标的观测方程为

其中分别为和方向的观测噪声。将上式写成矩阵的形式，有

令，则有

假定为均值为零，方差分别为的高斯白噪声，则

例6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。
解：由前面可知，三维空间中作匀速直线运动的目标，其状态向量为

由题意得目标的观测方程为

其中分别为和方向的观测噪声。将上式写成矩阵的形式，有

有

假定为均值为零，方差分别为的高斯白噪声，则

作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。

例7：设目标沿轴匀速直线运动，目标的状态可表示为，在时刻的观测值为，在时刻的观测值为，采样间隔为，求目标的初始状态和初始协方差。
解：初始状态为

初始协方差

其中，为观测噪声的方差，即：,滤波器从时开始工作。
2.1.2 Kalman滤波算法
状态估计的一步预测方程为

一步预测的协方差为

预测的观测向量为

观测向量的预测误差协方差为

新息或量测残差为

滤波器增益为

Kalman滤波算法的状态更新方程为

滤波误差协方差的更新方程为

2.2　转换坐标卡尔曼滤波器（非线性估计技术）。
卡尔曼滤波器两个要求：
1) 目标的状态方程是线性的；
2）观测方程是线性的。
在实际的情况下，要同时满足这两个要求是困难的。通常情况下：状态方程在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。如雷达的测量是距离、方位角和高低角。从直角坐标系来看，观测方程是非线性的。假定雷达的测量为距离、方位角和高低角，测量与目标位置的关系为

这是一个非线性关系。为解决测量方程非线性情况下的目标跟踪问题，可采用两种方法，一种是采用扩展的卡尔曼跟踪滤波器，这将在后面介绍；另一种是对测量进行坐标转换，将极坐标下的测量转化为直角坐标系下的测量，然后用标准的卡尔曼滤波器进行跟踪。
对三坐标雷达，坐标转换公式为

噪声方差的转换公式为

假设距离、方位角和高低角的测量噪声为相互独立的高斯白噪声，即

并且

对两坐标雷达，极坐标系下的观测为（），转换成直角坐标为

方差的转换：为求方差的转换，先求偏导

于是，得

在得到直角坐标系下的测量及测量的协方差矩阵后，可以用前面讲的方法进行目标跟踪。
这种变换存在的问题是：在直角坐标系下，测量的噪声不再是严格意义上的高斯噪声，并且噪声的分布中心并不是以直角坐标为中心，而且在直角坐标系下噪声是相互关联的。

由于噪声的分布中心并不是以直角坐标为中心，因此，噪声是有偏的，为此，有些学者提出了无偏坐标转换的方法：对转换后的直角坐标和方差进行校正。
通常情况下，传感器的测量为极坐标测量，极角坐标测量可通过无偏坐标转换变为直角坐标测量。设，则无偏转换的测量为

其中，。转换测量的协方差为

其中

　　在动态方程为线性、观测方程为非线性的情况下，可使用转换坐标卡尔曼滤波器，转换可采用两种方法：一种是不带校正量的转换，这是一种相对简单的方法；另一种为带校正量的方法，这是一种相对复杂的方法。

2.3　扩展卡尔曼滤波器(非线性估计技术)
采用扩展的卡尔曼滤波器进行目标跟踪。
2.3.1 系统的状态方程和测量方程
状态方程：

其中

为状态转移矩阵。
测量方程：

其中

2.3.2、观测方程线性化
将等号后的第一项以为中心按泰勒级数展开并略去二级以上的高阶分量，有

代入上式，得

令，，，于是得到线性化的观测方程为

假定目标的状态为,而，则

为的雅可比矩阵(Jacobian矩阵)。
例：雷达的观测是在极坐标系下进行的，对于一个直角坐标为的目标，雷达所测的三个极坐标分别为

观测方程为

令目标的状态为，则线性化的观测矩阵为

其中
例：红外传感器,观测值为方位角和高低角 ，求目标的状态为时线性化的观测矩阵
解：因
2.3.3、扩展卡尔曼滤波方程
一步预测：

一步预测协方差矩阵

预测的观测向量为

观测矩阵

新息或量测残差为

残差协方差矩阵

滤波器增益

滤波输出

误差的协方差阵

例题2-2：利用扩展的卡尔曼滤波算法对y轴上匀速运动的目标进行跟踪。
设传感器平台在x-y平面内运动，运动方程为

式中，t为时间，和为相互独立的、零均值白色高斯噪声，其方差分别为，且与过程噪声和量测噪声相互独立。过程噪声为加速度噪声，其方差为，传感器测量目标的方位角，采样时间间隔，测量噪声为零均值的高斯噪声，其方差为。
解：目标在y轴上运动，系统的状态方程为

其中，

传感器的测量方程为

其中，　
建立目标位置估计值和方位角测量值之间的关系(不考虑测量误差)为

　所以
　　　　　　, 由
目标的初始状态

对上式求导可得

于是

初始状态的方差

线性化的观测矩阵

仅供学习与交流，如有侵权请联系网站删除 谢谢44
2.4 无迹卡尔曼滤波器（Unscented Kalman Filter, UKF）
目前，扩展卡尔曼滤波虽然被广泛用于解决非线性系统状态估计问题，但其滤波效果在很多复杂的系统中并不能令人满意。模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波器发散。另外，在许多实际的应用中，模型的线性化过程比较繁杂，而且也不易得到。
无迹卡尔曼滤波是由于不需要对非线性系统进行线性化，并可以很容易地应用于非线性系统的状态估计，因此，UKF方法在许多方面得到应用。
2.4.1 无迹变换
无迹卡尔曼滤波是在无迹变换的基础上发展起来的。无迹变换（Unscented Transformation, UT）的基本思想是由Julier等首先提出的，是用于计算经过非线性变换的随机变量统计的一种新方法。它不需要对非线性状态和测量方程模型进行线性化。
假定X为一个维的随机变量，为一非线性函数，并且。的均值和协方差分别为和。无迹变换的一般步骤如下：
（1） 确定()个采样点

其中，为一尺度参数，可以为的任意值。为的第i列。为状态向量X的维数。
（2） 每个采样点通过非线性函数传播，得到
；　
（3）　的估计值和协方差估计如下

2.4.2 无迹Kalman滤波算法
根据无迹变换的特点，Kalman滤波算法可归纳如下：
假定
状态方程：
观测方程：
k时刻的状态估计值和状态协方差：和
k时刻的观测向量
（1） 根据采样规则，确定2+1个采样点以及相应的加权值

　　　其中，为尺度参数，可以为的任意值。为均方根的第i列。为状态向量的维数。
（2） 一步预测
；

；

（3） 更新状态

在动态方程和观测方程同时为非线性、或者一个为线性另一个为非线性的情况下，可以使用无迹卡尔滤波器，无迹卡尔曼滤波器的缺点是计算量较大。与无迹卡尔滤波器类似的滤波器还有积分卡尔滤波器（QKF）和粒子滤波器，这些滤波器均可用于非线性系统，其缺点是计算量更大。
2.5 和滤波器
和滤波器分别用于对匀速和匀加速目标进行跟踪。
2.5.1、滤波器
滤波器用于对匀速运动的目标进行跟踪。
目标的状态方程为

式中，
　　，，，
目标的观测方程

式中，
　　　　　，
滤波方程

问题：如何求和，利用稳态滤波器的特点，即

得

其中，，为目标的机动系数。
状态估计

状态估计的协方差为

一步预测协方差为

以上只讨论的是沿轴作直线运动情况下的滤波，对于平面或空间目标，需要分别对、及上的状态向量分别滤波。当过程噪声方差不能事先确定时，目标的机动系数无法确定，和两参数无法确定。工程上经常采用两种的方法来确定和：
　1、常系数法：和取固定的值
　　一般取
(1) (临界阻尼法)
(2) (最佳选择法)
　2、变系数法：

这里从1开始计数。对来说时才有值，但滤波器从开始工作，前两个点用于确定目标的初始位置和速度，完成航迹起始。
2.5.2 滤波器
滤波器用于对匀加速运动的目标进行跟踪。
目标的状态方程为

式中，
　　，，，
目标的观测方程

式中，
　　　　　，
滤波方程

问题：如何求、和，利用稳态滤波器的特点，即

得三个非线性方程构成的方程组

其中，，为目标的机动系数。
解线性方程组，得、和。状态估计

状态估计的协方差为

以上只讨论的是沿轴作直线运动情况下的滤波，对于平面或空间目标，需要分别对、及上的状态向量分别滤波。当过程噪声方差不能事先确定时，目标的机动系数无法确定，、和三参数无法确定。工程上经常采用两种的方法来确定、和
　1、常系数法：和取固定的值
　　一般取

2、变系数法：

这里从1开始计数。对来说时才有值，对于来说时才有值，但滤波器从开始工作，前三个点用于确定目标的初始位置、速度和加速度，完成航迹起始。

2.6 最小二乘估计和最小二乘滤波器
2.6.1、线性最小二乘估计
模型：

其中， ；；
目标：求以使
方法：
得到估计：
估计误差的方差：
其中
例：设目标作匀速直线运动，目标的状态可表示为，目标的起始状态为，目标的起始时刻为，在时刻的观测值为，在时刻的观测值为，观测噪声为相互独立的高斯白噪声，，分别求和时刻目标状态的最小二乘估计。
a、求时刻目标状态的估计值
解：

得到估计：
估计误差的方差：
其中

b、求时刻目标状态的估计值

得到估计：
估计误差的方差：
其中

2.6.2、加权最小二乘估计
　当测量数据的测量精度不等时，应采用加权处理，对精度较高的测量结果赋以较大的权。
目标：求以使
得到估计：
估计误差的方差：
其中
2.6.3、马尔可夫估计
　马尔可夫估计是一切加权最小二乘估计中能使得估计方差阵达到最小的一种。
　　当取时，加权最小二乘估计为马尔可夫估计
估计值为：
其估计误差的方差为

2.6.4、非线性最小二乘估计
模型：

目标：求以使
另一种表示

加权最小二乘估计
目标：
令，非线性加权最小二乘简化为非线性最小二乘。
非线性最小二乘估计可以用Gauss-Newton算法或Levenberg-Marquart算法（这些算法在Matlab中都已编好，直接应用就行，如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(k<kmax)
k:=k+1;
repeat
Solve (A+I)*=g
if ()
stop:=true;
else
pnew:=p+;

if
p:=pnew;
A:=JT*J;Ep:=Y-F(p);g=:JT*Ep;
Stop:=() or ;
;v:=2;
else

endif
endif
until()or(stop)
endwhile
X:=p;
非线性最小二乘估计也是一种最大似然估计，是一种最优估计，估计误差的方差能达到CRLB，因此估计误差的方差可取为CRLB。估计误差的方差为

例：匀速运动目标，目标的状态为，时刻的测量为，时刻的测量为，传感器测距误差标准差为=10m,测角误差标准差为=0.001rad, 求时刻目标的状态。
设时刻为，则时刻目标的状态为

时刻的理论观测值为

时刻的理论观测值为

加权非线性最小二乘估计的目标函数为
时刻目标状态的估计为

Matlab求Jacobian矩阵的方法
syms x xv y yv dt std_r std_b r1 b1 r2 b2
y1=(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(x^2+y^2))/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弧度。在时，观测值为[49999.7, 1.591196]，在时，观测值为[51111.3, 1.561589]，在时，观测值为[52043.8, 1.532488]，分为用线性最小二乘和非线性最小二乘法估计目标的初始状态和状态估计的协方差。

2.6.5、最大似然和非线性最小二乘的转化
例：匀速运动目标，目标的状态为，时刻的测量为，时刻的测量为，传感器测距误差标准差为=10m,测角误差标准差为=0.005rad, 求时刻目标的状态的最大似然估计。
正态分布概率密度函数

假定观测为相互独立的0均值高斯白噪声。测量为的概率密度函数为

的概率密度函数为

测量为和测量同时出现的联合概率密度函数（似然函数）为
对数似然

2.6.6、最小二乘滤波器
模型：
状态方程

观测方程

求
按最小二乘估计的方法，先确定与间的关系

得到最小二乘估计

其中， ；；；
取加权矩阵，作用是使过老数据的作用逐渐消失。取

则加权最小二乘估计为

其中

现在假设知道　，求，于是就形成了最小二乘滤波器。
加权最小二乘滤波器的递推公式为

其中，。

例：设目标为匀速运动模型，即状态向量为

观测为目标在直角坐标下的位置，写出最小二乘滤波器中的状态转移矩阵及观测矩阵。
解：转移矩阵为
　　　　　　其中
观测矩阵为

时刻的观测

例2-4 目标为二维空间中作匀速直线运动的目标，其初始状态为
, 过程噪声的方差q=1。传感器为2维雷达，测量目标的距离和方位角，传感器位置为，其测距误差标准差为100 m,测角误差标准差为0.001弧度，采样时间间隔为10 s。试：
　　1、根据已知条件产生仿真的观测数据。
　　2、利用仿真的观测数据分别用变系数滤波器、转换坐标Kalman滤波器、最小二乘滤波器和扩展Kalman滤波器进行目标跟踪，并在同一图上绘制出跟踪结果。

2.7 空间配准和坐标变换
在对坐标位置的不同传感器所获得的数据进行处理时，首先要作的是将测量数据转换到公共的坐标系下，然后才能对数据进行处理。通常的坐标系有以下4种：
（1） 以本传感器位置为中心的直角坐标系（传感器局部坐标系）
（2） 以地球中心为中心的直角坐标系（地心坐标系）
（3） 以经度、纬度、高度表示的大地坐标系（地理坐标系）
假设在局部坐标系中
:正东为正,
:正北为正,
z:向上为正；
方位角：正东为0，逆时针为正，
高低角：水平为0，逆时针为正。
主要坐标变换有：
（1）地理坐标到地球固定坐标的转换关系为

其中，分别表示地理的经度、纬度和高度，为地球的长轴半径，为地球短轴半径， 为地球的偏心率，。
（2）从地理坐标到传感器局部坐标的变换关系为

其中，为传感器的地理坐标，而

（3） 从传感器局部坐标到地球固定坐标的变换关系为

（4）地球固定坐标到地理坐标的变换
从地球固定坐标到地理坐标的变换较为复杂，求纬度时用到迭代。
下面给出的是一种求取地理坐标的算法。

图1 参考的椭球及地理参数

图2　椭圆和地理纬度
假设:
地球的长轴半经: ;
地球短轴半径为;
;
：地理经度
：地理纬度
：高度
点的直角坐标：
P点的经度可以直接求出

考虑地球椭圆切面（如图2），有

在OXZ坐标系下，P点的坐标为
　　　　　　　　，
椭圆方程的极坐标形式（或的坐标）为
　　　　　，　
点处椭圆切线的法向量为（）

与点处椭圆切线垂直并通过和点的垂线为

,则上述方程可写为

其中
　　　　　，，
上述方程为一元4次方程，可以通过复杂的推导得到其解析解。方程的根也可以通过（NR）方法近似求取。这里，我们采用NR方法求取

其中，。现在的问题是如何求取。因为

（由于未知，而已知，与线的斜率近似相同）
取，并令，得

解方程，得

其中，是符号函数

由通过迭代可求出，并取，再计算出纬度和高度。

因为

于是

假定为迭代精度，为最大迭代次数。
（1） 计算和系数

（2） 如果

转入（8）
（3） 初始化和

（4）

（5） 如果，转入（7）
（6） 如果，转入（4）
（7）

（8） 算法结束。