3.5 线性结构估计器

线性结构估计器是指目标状态估计与量测是线性关系的估计器，如LMMSE估计器。在目标跟踪系统中，对于线性高斯系统，卡尔曼滤波（Kalman Filter，KF）是最优的MMSE估计器。然而，对于非线性系统，KF很难直接应用。现有流行的非线性点估计器大多是基于LMMSE的估计器，其直接最小化所有线性估计中的均方误差。现有流行的LMMSE估计器主要分为两大类。一类是基于函数逼近的估计器，代表估计器有扩展卡尔曼滤波（Extended Kalman Filter，EKF）、二阶扩展卡尔曼滤波（Second-order EKF，SOEKF）和迭代扩展卡尔曼滤波（Iterated EKF，IEKF）等。另一类是基于矩逼近的估计器，代表估计器有文献 ^［6］ 提出的无迹滤波、文献 ^［7］ 提出的求积卡尔曼滤波及文献 ^［8］ 提出的容积卡尔曼滤波等，这类方法与基于函数逼近的估计方法的不同点在于计算一、二阶矩时所采用的确定性采样方法不同。

因此，3.5.1节主要针对线性高斯系统的KF进行介绍；3.5.2节主要针对基于函数逼近的LMMSE估计器进行介绍；3.5.3节主要针对基于矩逼近的LMMSE估计器进行介绍。

3.5.1 卡尔曼滤波

1.离散时间线性高斯系统描述

离散时间线性高斯系统描述如下：

式（3-122）和式（3-123）分别表示状态方程和量测方程；随机变量 x _k 表示 k 时刻的状态向量； F _{k -1} 是状态从 k -1时刻到 k 时刻的状态转移矩阵； w _{k -1} 是均值为0、方差为 Q _{k -1} 的高斯分布系统噪声，即 w _{k -1} ～ N （ 0 ， Q _{k -1} ），其描述了状态方程建模的不确定性； z _k 是带噪声的量测； H _k 是量测矩阵； v _k 是均值为0、方差为 R _k 的高斯分布量测噪声，即 v _k ～ N （ 0 ， R _k ）。

在该线性系统中， F _{k -1} 、 H _k 、 Q _{k -1} 和 R _k 是已知且可以提前获取的，它们可以是常数，也可以随时间变化。 w _k 和 v _k 不相关，即对于所有的 k 和 j ，都有

式中， δ _kj 是克罗内克函数，即

定义 Z ^k ={ z ₁ ， z ₂ ，…， z _k }是直到 k 时刻所有的量测，假设被估计量为 x _j ，则当 j ＜ k 时为平滑，当 j = k 时为滤波，当 j ＞ k 时为预测。本书仅关注滤波方法。

2.卡尔曼滤波推导

1）一步预测与新息

假设基于量测 Z ^{k -1} 已有估计值 ，则根据状态方程（3-122）来预测 k 时刻的状态值。直观的想法是，因为 w _{k -1} 的均值为零，所以定义 为根据 Z ^{k -1} 所得的估计值 的一步预测合理数值，即

考虑到 v _k 的均值为零，因此量测的期望为 是合适的。基于以上两点，可根据 k 时刻的量测数据 z _k 来估计 x _k 的递推形式，即

式中， 反映了第 k 个量测对状态估计提供的新息，卡尔曼滤波利用新息对状态估计进行在线修正； K _k 是一个待定的校正增益矩阵，是 k 时刻对新息的加权，反映了状态估计过程中对新息的重视程度，其目标是使估计误差的方差最小。为此，先推导误差方差公式。

2）估计误差的方差

定义预测的误差和估计的误差分别为

式（3-132）和式（3-133）的含义分别为接收到 z _k 之前和接收到 z _k 之后对 x _k 的估计误差。根据式（3-133），考虑到式（3-131），将式（3-123）和式（3-132）代入式（3-133）可得

估计误差的方差矩阵为

定义一步预测误差的方差为

由模型的基本统计性质可知

而且，预测误差与量测噪声互不相关：

将式（3-136）～式（3-138）代入式（3-135），得

要完成递推，还需分析从 P _{k -1} 到 P _{k | k -1} 的递推公式，式（3-130）两边同时减去 x _k ，可得

将式（3-122）和式（3-132）代入式（3-140），得

由于估计误差与过程噪声互不相关，即

因此

求得估计误差方差的递推公式之后，讨论 K _k 的选择，使估计误差的方差 P _k 最小。

3）增益矩阵 K _k

要使估计误差的方差矩阵 P _k 取最小值，等价于使误差方差矩阵的迹最小，可将估计误差方差矩阵 P _k 的迹对 K _k 求偏导并令偏导数为零，即

对式（3-139）取迹，有

从而应用矩阵迹的求导公式得

考虑到 P _{k | k -1} 、 R _k 为对称矩阵，可得

令式（3-148）为零，得KF增益为

进而，估计误差的方差矩阵可简化为

3.卡尔曼滤波算法

根据前述推导过程，对于式（3-122）和式（3-123）描述的线性高斯系统模型，总结离散时间条件下的KF算法的一步循环如下。

1）初始化

步骤1：给定滤波初始条件 、 P ₀ 、 Q ₀ 、 R ₀ ，对于时刻 k =1，2，3，…循环执行步骤2～步骤4。

2）预测

步骤2：计算状态估计和估计误差的协方差为

3）更新

步骤3：计算量测预测、新息协方差矩阵和卡尔曼增益为

步骤4：计算状态估计和估计误差的协方差为

4.卡尔曼滤波的性质

1）递推性

离散时间KF是一种递推的滤波方法，每个时刻通过计算被估计量预测的一、二阶矩和更新的一、二阶矩完成递推过程。KF不要求保存过去的量测信息，获得新的量测信息后，根据新数据和前一时刻的估计值，利用状态方程和递推公式即可获得新的估计值。因此，K F比较适合应用在动态量测的场景中，如卫星定位和惯性导航、目标动态定位与跟踪、卫星轨道参数估计等。

2）无偏性

KF的状态估计值 是状态被估计量 x _k 的MMSE估计，因此KF估计 是无偏估计，即 = E ［ x _k ］。

3）最优性

如果初始状态及初始状态误差、系统噪声及量测噪声都满足高斯假设，则KF是最优的MMSE估计器，此时的误差方差矩阵 P _k 是基于量测 Z ^k 的所有估计中的最小均方误差矩阵；如果不满足高斯假设，则KF是最优的LMMSE估计器，此时的误差矩阵 P _k 是基于量测 Z ^k 的所有线性估计中的最小均方误差矩阵。

3.5.2 基于函数逼近的LMMSE估计器

对于非线性估计系统，KF难以直接使用，LMMSE估计器是常用的点估计器，具体介绍请参阅3.4.3节。

给定以下离散时间动态非线性系统：

式中， x _k 表示 k 时刻的状态向量； z _k 表示 k 时刻的量测； f _k （·）为状态转移函数； h _k （·）为量测函数， w _k ～ N （ 0 ， Q _k ）和 v _k ～ N （ 0 ， R _k ）分别为服从高斯分布的系统过程噪声和量测噪声， Q _k 和 R _k 分别为对应的方差矩阵； w _k 和 v _k 写在括号内，表示噪声可能为乘性噪声，也可能为加性噪声。

基于函数逼近的LMMSE估计的典型方法是EKF，其通过对非线性的状态转移函数和量测函数进行泰勒展开而近似原始函数，泰勒展开的阶数不同，对应的EKF精度也不同。对于简单的非线性滤波场景，一阶EKF计算复杂度小，精度也可满足需求，因此本书仅针对一阶EKF进行介绍，其他类型的EKF可以参考文献 ^［4］ 。

1.非线性系统泰勒展开

对于任意两个独立的随机变量 x 和 q ，满足

式中， x ∈ℝ ⁿ ， q ∈ℝ ^l ， y ∈ℝ ^m 。令 ξ =［ x ^T ， q ^T ］ ^T ， P ^a =diag（ P ， Q ），其中diag（·）表示由括号内的向量或矩阵构成的对角矩阵或对角分块矩阵，则通过非线性函数 g （·）变换后的随机变量 y 的概率密度表示为

式中， 表示矩阵的行列式； g ^-1 （ y ）表示反函数。

令 ξ = ，其中 δ ξ ～ N （ 0 ， P ^a ），可得函数 g （·）的泰勒级数展开为

式中， e _i =［0…0 1…0］ ^T 是坐标轴 i 方向上的单位向量，只有位置 i 的取值为1，其余位置的取值均为0； 为 g （·）的雅可比矩阵，可通过下式计算得到：

是海森矩阵（Hessian Matrix），可通过下式计算得到：

式中，［·］ _{j ， j ′} 表示矩阵中位于（ j ， j ′）的元素。

取泰勒级数的前两项对非线性函数进行线性化近似，即

计算式（3-161）在 点的期望为

协方差为

随机变量 ξ 与 y 的互协方差矩阵为

2.扩展卡尔曼滤波算法

考虑由式（3-154）和式（3-155）构成的非线性系统，EKF算法的步骤如下。

1）初始化

步骤1：给定滤波器初始条件 、 P ₀ 、 Q ₀ 、 R ₀ ，对于时刻 k =1，2，3，…循环执行步骤2～步骤6。

2）预测

步骤2：计算动态系统状态方程的雅可比矩阵为

步骤3：计算状态预测及预测误差协方差为

3）更新

步骤4：计算量测方程的雅可比矩阵为

步骤5：计算量测预测、新息协方差矩阵和卡尔曼增益为

步骤6：计算状态估计及估计误差协方差为

特别地，当系统噪声和量测噪声为加性噪声时，式（3-166）和式（3-170）中的矩阵 F _{k ， w} 和 H _{k ， v} 为单位矩阵，此时不需要再对它们进行实时计算，状态预测的误差矩阵和量测预测的误差矩阵分别通过以下两个式子获得：

EKF算法的优点在于使用较为简便的算法即可获得较好的滤波效果，而且在工程实践中，线性化近似是解决非线性系统问题的常用手段，易于理解和使用，因此可以广泛应用于大多数实际问题。其缺点在于，EKF算法基于局部线性化方法，因而在非线性度较高的系统中无法获得满意的滤波效果，模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波的发散。此外，EKF算法要求动态模型和量测模型函数是可微的，由于在实际问题中无法轻易获得所需的雅可比矩阵，因此EKF算法无法实现。同时，复杂的非线性系统的一阶的泰勒近似误差较大，甚至即使获得了雅可比矩阵，也会积累较大的计算误差，导致程序难以调试，因此越来越多的研究开始寻求更多新的非线性滤波算法。

3.5.3 基于矩逼近的LMMSE估计器

1.基于矩逼近的LMMSE估计

给定由式（3-154）和式（3-155）构成的非线性系统，基于LMMSE估计器，可得到被估计量 x _k 的LMMSE估计为

式中，相关的一、二阶矩可通过以下式子计算得到：

由式（3-180）～式（3-184）可以看出，估计的 的相关一、二阶矩的计算方法可统一由以下函数表示：

式中， N 为采样点的个数； x _i 为采样点； w _i 为采样点对应的权值。式（3-185）表明，在已知分布 和函数 q （ x ）时， 可通过对 x 进行采样得到。不同的基于矩逼近的LMMSE估计器所采用的确定性采样方法不同，即获得 x _i 的方法不同。下面将介绍无迹变换、容积规则和高斯-厄米特求积规则3种确定性采样方法。在此之前，先介绍基于确定性采样的LMMSE估计的算法流程。

1）初始化

步骤1：给定滤波器初始条件 、 P ₀ 、 Q ₀ 、 R ₀ ，对于时刻 k =1，2，3，…循环执行下述步骤2～步骤11。

2）预测

步骤2：考虑到系统噪声可能为非加性噪声，将 k -1时刻的估计 和系统噪声的均值、估计的协方差矩阵 P _{k -1} 和 Q _{k -1} 分别扩维，得到

步骤3：根据式（3-186）和式（3-187），利用确定性采样方法获得关于 的确定性采样点 及权值 w ₁ ， w ₂ ，…， w _N ，其中， N 为采样点个数。

步骤4：利用动态模型传播采样点，得到预测的状态采样点为

式中， 为 的前 n _x 项， 为 的后 n _w 项，其中， n _x 为状态 x _k 的维度， n _w 为系统噪声 w _k 的维度。

步骤5：计算状态预测及其协方差矩阵为

步骤6：考虑到量测噪声可能为非加性噪声，因此将预测的状态 和量测噪声的均值、状态预测的协方差矩阵和量测噪声协方差矩阵分别扩维，得到

步骤7：根据式（3-191）和式（3-192），利用确定性采样方法获得关于 的确定性采样点 及权值 w ₁ ， w ₂ ，…， ，其中， N ₂ 为采样点个数。

步骤8：利用量测模型传播采样点，得到预测的量测采样点为

式中， 为 的前 n _x 项， 为 的后 n _v 项，其中， n _v 为量测噪声 v _k 的维度。

步骤9：计算量测预测及其协方差矩阵为

步骤10：计算状态预测与量测预测的互协方差矩阵为

3）更新

步骤11：计算 k 时刻的状态估计及其协方差矩阵为

步骤1～步骤11给出了完整的基于确定性采样方法的LMMSE估计流程，可以看出，其与卡尔曼滤波的形式相同，因此LMMSE估计器也称卡尔曼类滤波器。接下来分别介绍3种确定性采样方法。

2.无迹变换

无迹变换（Unscented Transform，UT）是用于计算经非线性变换后的随机变量的统计特性的方法，可直接求出目标分布的均值和协方差，避免了对非线性函数的近似。

无迹变换的思想是利用初始分布的均值和协方差生成一系列确定的sigma点，这些sigma点通过非线性函数传播，得到估计的均值和协方差。

考虑 n 维随机变量 和 m 维随机变量 ，有

式中， g （·）为非线性变换函数。

无迹变换的目的是根据 x 的统计特性获得非线性函数传播后的 y 的统计特性，为此需要选取2 n +1个sigma点 χ ⁱ ，sigma点及其权值的选取规则为

式中， λ = α ² （ n + κ ） -n ，决定sigma点与均值 的距离；参数 α 通常设为一个较小的正数（10 ^-4 ≤ α ＜1）； κ 通常取为0或3 -n ；对于高斯分布， β =2是最优的，如果状态变量是单变量，则最佳的选择是 β =0。

应用上述确定性采样方法得到的LMMSE估计器称为无迹滤波（Unscented Filter，UF）。UF将状态随机变量以一系列采样点的形式表示，这些采样点能获取随机变量的均值和协方差。UF的优点是不近似非线性动态模型和量测模型，而是直接利用原系统模型。此外，由于不要求系统可微，也不需要计算复杂的雅可比矩阵，因此基于无迹变换的UF算法更具有实际应用价值。

UF自提出以来，在工程上得到了广泛应用，但处理维数（维数 n ≥4）时，UF中的自由调节参数 κ ＜0，使某些sigma点的权值 w ＜0，从而使滤波过程中的协方差非正定，导致滤波数值不稳定，甚至可能发散，因此UF在高维数中存在应用困难的情况。

3.容积规则

对于高斯分布下的非线性滤波问题，可以将其归结为一个积分计算问题，其中被积函数表示为非线性函数与高斯密度的乘积，考虑以下标准高斯加权积分：

容积规则利用球面径向变换、球面容积规则和径向积分规则求解该高斯积分问题，有关球面径向变换、球面容积规则和径向积分规则的推导可参考文献 ^［8］ 。下面直接介绍式（3-202）的求解方法。

对于三阶的容积规则，共需要2 n 个容积点，标准高斯加权积分如下：

式中，

^［1］ ∈ℝ ⁿ 表示 n 维空间的点集，有

进而，若 x ～ ，令 P = SS ^T ，得

基于容积规则的LMMSE估计器称为容积卡尔曼滤波（Cubature KF，CKF）。相比UF，CKF采用偶数并具有相同权值的点集，UF选用奇数及不同权值的点集；在高维数系统中，UF的sigma点权值容易出现负值，而CKF权值永远为正值，因而高维情况下其数值稳定性和滤波精度优于UF。由于具有不容易发散且计算量小的优点，CKF一经提出就被广泛应用于各领域的估计问题。

4.高斯-厄米特求积规则

考虑下面的单变量函数求积分问题：

式中， χ _j 为第 j 个求积分点； w （ x ）为权值函数； w _j 为该积分点对应的权值。当给定积分点时，即可求解积分。在高斯-厄米特积分中，当权函数 w （ x ）= N （ x ；0，1）时，有

权值和积分点可以通过求解一个多项式的根得到。当使用第 m 阶厄米特多项式 H _m （ x ）时，该积分规则对于直到2 m -1阶的多项式都是准确的。

m 阶厄米特多项式定义为

前几阶厄米特多项式为

更高阶的厄米特多项式可以通过以下递归的方法得到：

通过求解以上方程的根，即可得到积分点χ _j ，其对应的权值为

注意，上述采样方法仅对标量进行采样，对于向量形式的高斯-厄米特求积问题，可以通过将其转化为对每一维进行积分获得。

式中， s =［ s ₁ ， s ₂ ，…， ］ ^T ； N = 为采样点数量， m 为厄米特多项式的阶数，也是 s 的每一维采样点的数量， n _x 为 x 的维度；向量 s _i 表示第 i 个采样点，其由每一维 s _i 的采样点构成。

利用高斯-厄米特求积规则获得 s 的每一维的采样点后，进行排列组合获得采样点 s _i ，利用式（3-214）即可获得式（3-188）～式（3-196）中所需的一、二阶矩。基于高斯-厄米特求积规则的LMMSE估计器称为求积卡尔曼滤波（Quadrature KF，QKF）。与UF和CKF相比，QKF计算量大，但是结果更加精确，不易发散。

3.5.4 仿真结果与分析

本节采用双站纯角度跟踪场景对EKF、UF、CKF和QKF进行验证与比较。

仿真场景具体设置如下。

目标在平面内做近似匀速运动，其状态传播方程为

式中， ，［ x _k ， y _k ］ ^T 和 分别表示目标位置和速度； w _{k -1} ～ N （ 0 ， Q _{k -1} ）为系统过程噪声， Q _{k -1} =20 ² I ₂ ， I ₂ 为2×2维单位阵； F _{k -1} 和 G _{k -1} 由以下两个式子分别给出：

式中，diag（·）表示由括号内的矩阵构成的分块对角矩阵；采样周期 T 为1s。目标初始状态由 产生，其中， =［-20000m，250m/s，15000m，-100m/s］ ^T ， P ₀ =diag［10000m ² ，5000m ² /s ² ，10000m ² ，5000m ² /s ² ］。

传感器位置为［ x ₁ ， y ₁ ］ ^T =［0，0］ ^T m，［ x ₂ ， y ₂ ］ ^T =［0，4000］ ^T m，量测为由这两个传感器得到的两个方位角，即

式中， 为量测噪声； σ _θ 为0.3rad。

采用均方根误差（Root Mean Square Error，RMSE）对目标跟踪的位置和速度进行评估，对4种算法进行比较。位置和速度RMSE的定义由以下两个式子分别给出：

式中， 和 分别表示 k 时刻目标的位置RMSE和速度RMSE； 和 分别表示第 i 次蒙特卡罗 k 时刻目标的位置和速度估计结果； 和 分别表示第 i 次蒙特卡罗 k 时刻目标的真实位置和真实速度； N 为蒙特卡罗次数。

仿真时间长度为100个时间步长，蒙特卡罗次数为300，QKF每个维度采用3个求积点，仿真结果如图3-1和图3-2所示。

结果表明，在高度非线性情况下，QKF算法的位置估计和速度估计优于其他几种估计器，主要因为在计算一、二阶矩的过程中，QKF算法的采样点多于UF和CKF。虽然CKF算法的采样点比UF少1个，但是在本场景中，状态维度为4，UF算法的采样点权值部分为负值，易导致发散。随着时间的推移，EKF算法的误差累积越来越大，因此EKF算法不适用于高度非线性场景。 b046Jach1znFWBP59UOrz9x8EJrQPzhxM/ZeTQiZFYdKKEQWTgYsVRKhlt+o4oXR