第二章-状态估计基础

第二章状态估计基础状态估计的目的是对目标过去的状态进行平滑、对目标现在的运动状态进行滤波和对目标未来的运动状态进行预测。这些运动状态包括目标位置、速度、加速度等。本章讨论在多传感器跟踪系统中广泛应用的状态估计技术，这些技术包括Kalman滤波技术，滤波与滤波技术、最小二乘滤波技术和非线性系统的状态估计技术。2.1线性系统估计――卡尔曼滤波技术2.1.1线性系统描述1.离散时间线性动态系统的状态方程线性系统采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的一般状态方程可描述为)()()()()1(kVkGkXkkX其中，nRkX)(是k时刻目标的状态向量，nRkV)(是过程噪声，它是具有均值为零、方差矩阵为)(kQ的高斯噪声向量，即klTkQlVkVEkVE)()()(0)(（：狄拉克函数，或单位冲激函数），nnRk)(是状态转移矩阵，nnRkG)(是过程。2.传感器的测量（观测）方程传感器的通用观测方程为)()()()(kWkXkHkZ这里，mRkZ)(是传感器在k时刻的观测向量，观测噪声mRkW)(是具有零均值和正定协方差矩阵)(kR的高斯分布测量噪声向量，即klTkRkWkWEkWE)()()(0)(3．初始状态的描述初始状态)0(X是高斯的，具有均值)0|0(ˆX和协方差)0|0(P，即)0|0(0|0(ˆ)0(0|0(ˆ)0(PXXXXET以上描述比较抽象，下面结合具体的例子加以说明。例1：目标沿x轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。解：目标的状态为)()()(kxkxkX用T表示时间间隔，xu表速度噪声，则有)()()1()()()()1(kukxkxkTukxTkxkxxx写成矩阵形式为)(1)()(101)1()1(kuTkxkxTkxkxx令)()(,1,101)(kukVTGTkx有)()()()()1(kVkGkXkkX其中qkuEkVkVEkQxT)]([)]()([)(2)(kux：均值为0，方差为q的高斯噪声。例2：目标为二维空间中作匀速直线运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为二维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为TkykykxkxkykykxkxkX)()()()()()()()()(用T表示时间间隔，yxuu,分别表示yx,方向的加速度噪声，则有)()()1()(2)()()1()()()1()(2)()()1(22kTukykykuTkyTkykykTukxkxkuTkxTkxkxyyxx写成矩阵的形式有)()(020002)()()()(10001000010001)1()1()1()1(22kukuTTTTkykykxkxTTkykykxkxyx令)()()()()(kykykxkxkX，10001000010001)(TTk，TTTTkG020002)(22，)()()(kukukVyx有)()()()()1(kVkGkXkkX))(())()(())()(())(()()()()()()()(22kuEkukuEkukuEkuEkukukukuEkVkVEkQyyxyxxyxyxT假定)(),(kukuyx为均值为0，方差分别为xq和yq的相互独立的高斯白噪声，则yxyyxyxxqqkuEkukuEkukuEkuEkQ00))(())()(())()(())(()(22例3：目标为沿x轴做匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程解：目标的状态可写为：)()()()(1kxkxkxkX用T表示时间间隔，zyxuuu,,分别表示zyx,,方向的加速度噪声，则有)()()1()()()()1()(2)(2)()()1(22kukxkxkTukxTkxkxkuTkxTkxTkxkxxxx写成矩阵的形式有)(15.0)()()(100105.01)1()1()1(22kuTTkxkxkxTTTkxkxkxx令)()(,15.0)(,100105.01)(12121kukVTTkGTTTkx有)()()()()1(11111kVkGkXkkX其中，xxTqkuEkVkVEkQ)()()()(21xq为x方向加速度噪声的方差。例4：在例3的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为)()()()(321kXkXkXkX其中)()()()(,)()()()(,)()()()(321kzkzkzkXkykykykXkxkxkxkX目标的状态方程可写为)()()()()1(kVkGkXkkX其中)()()()(,)(000)(000)()(,)(000)(000)()(111111kukukukVkGkGkGkGkkkkzyx而过程噪声协方差矩阵为zyxqqqkQ000000)(zyxqqq,,分别为x方向，y方向和z方向加速度噪声方差。例5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为)()()()()(kykykxkxkX由题意得目标的观测方程为)()()()()()(kwkykzkwkxkzyyxx其中)(),(kwkwyx分别为x和y方向的观测噪声。将上式写成矩阵的形式，有)()()()()()(01000001)()(kwkwkykykxkxkzkzyxyx令)()()(,01000001)(,)()()(kwkwkWkHkzkzkZyxyx，则有)()()()(kWkXkHkZ)()()()()()()()()()()()()(22kwEkwkwEkwkwEkwEkwkwkwkwEkWkWEkRyyxyxxyxyxT假定)(),(kwkwyx为均值为零，方差分别为yxrr,的高斯白噪声，则yxyyxyxxrrkwEkwkwEkwkwEkwEkR00)()()()()()()(22例6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。解：由前面可知，三维空间中作匀速直线运动的目标，其状态向量为)()()()()()()(kzkzkykykxkxkX由题意得目标的观测方程为)()()()()()()()()(kwkzkzkwkykzkwkxkzzzyyxx其中)(),(),(kwkwkwzyx分别为yx,和z方向的观测噪声。将上式写成矩阵的形式，有)()()()()()()()()(010000000100000001)()()(kwkwkwkzkzkykykxkxkzkzkzzyxzyx有)()()()(kWkXkHkZ假定)(),(),(kwkwkwzyx为均值为零，方差分别为zyxrrr,,的高斯白噪声，则zyxrrrkR000000)(作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。例7：设目标沿x轴匀速直线运动，目标的状态可表示为TxxX，在0t时刻的x观测值为)0(z，在1t时刻的x观测值为)1(z，采样间隔为01ttT，求目标的初始状态和初始协方差。解：初始状态为TzzzX)0()1()1()1|1(ˆ初始协方差2/2//)1|1(TrTrTrrP其中，r为观测噪声的方差，即：),0()(rNkW,滤波器从2k时开始工作。2.1.2Kalman滤波算法状态估计的一步预测方程为)|(ˆ)()|1(kkXkkkX一步预测的协方差为)()()()()|()()|1(kGkQkGkkkPkkkPTT预测的观测向量为)|1(ˆ)1()|1(ˆkkXkHkkZ观测向量的预测误差协方差为)1()1()|1()1()1(kRkHkkPkHkST新息或量测残差为)|1(ˆ)1()1()|1(ˆ)1()1(kkXkHkZkkZkZkv滤波器增益为)1()1()|1()1(1kSkHkkPkKTKalman滤波算法的状态更新方程为)1()1()|1(ˆ)1|1(ˆkvkKkkXkkX滤波误差协方差的更新方程为)|1()1()1()1()1()1()|1()1|1(kkPkHkKIkKkSkKkkPkkPT2.2转换坐标卡尔曼滤波器（非线性估计技术）。卡尔曼滤波器两个要求：1)目标的状态方程是线性的；2）观测方程是线性的。在实际的情况下，要同时满足这两个要求是困难的。通常情况下：状态方程在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。如雷达的测量是距离、方位角和高低角。从直角坐标系来看，观测方程是非线性的。假定雷达的测量为距离r、方位角和高低角，测量与目标位置的关系为22222arctanarctanyxzxyzyxr这是一个非线性关系。为解决测量方程非线性情况下的目标跟踪问题，可采用两种方法，一种是采用扩展的卡尔曼跟踪滤波器，这将在后面介绍；另一种是对测量进行坐标转换，将极坐标下的测量转化为直角坐标系下的测量，然后用标准的卡尔曼滤波器进行跟踪。对三坐标雷达，坐标转换公式为zyxrrrrsinsincoscoscos噪声方差的转换公式为vvvAvvvrrrrrvvvzzrzyyryxxrxv