反粒子滤波器：自适应非线性估计

反粒子滤波器——一种自适应非线性估计器

摘要

我们提出了反粒子滤波器（AF），这是一种新型的递归贝叶斯估计器，既不同于扩展卡尔曼滤波器（EKF）、无迹卡尔曼滤波器（UKF），也不同于粒子滤波器（PF）。我们表明，对于经典的机器人定位问题，在某些情况下，AF可以显著优于这些其他滤波器。AF将后验分布估计为辅助变量高斯分布，从而给出无需随机样本的解析公式。它能够随着不确定性的变化自适应地改变后验分布的复杂度。当不确定性较低时，AF等价于EKF；而当不确定性增加时，AF能够表示非高斯分布。在相同精度下，其计算时间可能远快于粒子滤波器。我们对两种类型的反粒子滤波器与EKF、迭代EKF、UKF、一种迭代UKF以及PF进行了仿真比较，结果表明反粒子滤波器可将误差降低至一致且精确的水平。

1 引言

非线性估计对于机器人学以及许多其他领域都至关重要。从根本上讲，人们需要基于对该‘状态’的间接且带有噪声的观测来描述其不确定性。该状态随时间演化，而这种演化会引入不确定性。当观测和演化均通过关于状态估计和白高斯噪声的线性系统方程描述时，卡尔曼滤波器是最优估计器[1]。

当系统为非线性时，这一问题变得更加困难。一种方法是在当前状态估计附近对系统方程关于状态进行线性化，如扩展卡尔曼滤波器 EKF[2, 3] 中所采用的方法。

扩展卡尔曼滤波器（EKF）已催生了许多变体。这些变体主要解决了不一致性问题。EKF 的不一致性是线性化的结果。EKF 是一种高斯估计器，这意味着后验分布由状态均值和协方差参数化。当协方差过小时，我们称该估计是过度自信的[4]。这可能导致估计发散或基于该估计产生错误推断。

为了提高滤波器稳定性，人们尝试了更好地估计协方差或直接调整协方差的方法，例如鲁棒扩展卡尔曼滤波器[5, 6]，或在[7]中用于机器人定位的方法。

通过将一组选定的点映射到非线性函数中来计算估计值，可以获得对协方差的更好估计，使得该估计能够反映多于单个点的信息。这种方法催生了无迹卡尔曼滤波器（UKF）[8–10]、线性回归卡尔曼滤波器（LRKF）[11–13]、移位瑞利滤波器 [14] 以及文献 [15] 中的滤波器。

在引入需要对估计均值进行大幅调整的观测时，常会出现发散或不稳定行为。迭代卡尔曼滤波器IEKF[16, 17]可通过重复应用扩展卡尔曼滤波器公式和线性化，直至收敛到稳定解，从而帮助解决这一问题。

上述所有估计器均使用高斯后验分布。当估计中的不确定性相对于非线性程度变得更为显著时，高斯分布将无法充分表示后验概率。此类情况可通过粒子滤波器 PF,[18–21] 来解决。粒子滤波器能够估计任意分布，且对噪声的形式无任何要求。它通过采样分布实现估计，本质上是对系统可能的演化路径进行大量仿真，即生成多个粒子，并根据观测对粒子集进行重加权。

粒子滤波器非常流行且功能强大。但它有一个缺点：估计结果依赖于在真实状态附近存在足够多的粒子。保持粒子均匀且密集地分布非常重要。然而，使用粒子滤波器时，所谓的粒子匮乏（或耗尽）问题最终难以避免。粒子匮乏是指不同粒子数量过少的状态。虽然增加粒子数量可减轻这一问题，但这会导致计算成本升高。

粒子匮乏在更高维度中更是成为一个问题。这有时可以通过Rao‐Blackwellized粒子滤波器来解决，例如快速SLAM[22, 23]。或者，文献[24]提出了一种新型的高斯粒子滤波器，该方法不需要重采样，但假设了高斯后验分布。

表示非高斯分布的其他解决方案包括在同时定位与地图构建问题中使用的高斯混合，如[25]所述；或如[13]中提出的高斯和积分滤波器。精确批量方法以及某些图算法[26–30]可通过逐次逼近求解精确系统。文献[31]提出了一种结合图信念传播、粒子滤波并使用辅助变量的方法。

2 本文概述

我们将开发一类新的估计器，称之为反粒子滤波器 AF[32]。我们将描述该类中的两个成员，分别称为二次型和三角型反粒子滤波器，即 QAF 和 TAF。

我们将这些滤波器与 EKF、IEKF、UKF、IUKF1 和 PF 进行比较。

我们首先讨论自适应滤波器的后验分布。我们展示了通过引入和移除辅助变量，它如何随着不确定性的变化而自适应地改变复杂度。然后我们讨论如何通过预测和更新步骤来估计该分布。

接着是我们的仿真，展示了在具有较大不确定性的机器人定位问题中，自适应滤波器比其他类型的滤波器更准确、稳定且一致。通过增加粒子数量，可以使粒子滤波器更准确和一致。我们展示了两种情况下的粒子滤波器：一种粒子数量与我们的滤波器具有相似的计算时间，另一种则明显更慢，但仍不如自适应滤波器准确或一致。

3 辅助变量高斯分布

随着状态估计随时间演化，不确定性会在预测步骤中增加，该步骤利用动态方程将估计值向前推进。当进行观测时，不确定性会在更新步骤中被降低。

如果长时间未进行观测，不确定性可能变得过大，导致非线性效应显著。这种不确定性通常具有高度相关性，因此即使协方差矩阵的所有元素都在增长，这种增长实际上仅发生在少数几个问题方向上。我们提出将部分问题方向分解出来，并将其不确定性建模为一个nk维的辅助变量向量k。从而使后验分布具有如下形式：

$$
pðxÞ ¼ \int_{-1}^{1} Gðx - mk; PÞGðk; CÞðdkÞ^{nk} \quad (1)
$$

其中，G(k, C) 是均值为零、协方差为 C 的高斯分布。我们看到，状态估计 mk 的均值在条件上由 k 参数化。该均值 mk 是 k 的可微函数。通过假设该函数的不同形式，我们可以生成不同形状的分布以及不同类型的滤波器。我们将在后续利用这一事实：

$$
E[kk^T] - E[k]E[k^T] = C \quad (2)
$$

迭代UKF是UKF的一种迭代版本，类似于IEKF。

我们可以利用mk的泰勒级数展开推导出一些结果：

$$
mk = l + Jmk + \frac{1}{2} k^THmk + O(k^3) \quad (3)
$$

其中，μ = m0, Hm 是一个由对称k空间Hessian矩阵组成的x空间向量，而雅可比矩阵 Jm 是从k空间到x空间的矩阵。

$$
E[x] = \int_{-1}^{1} mkG(k; C)(dk)^{nk} \approx l + \frac{1}{2}\sum_{i,j} Hm_{ij} C_{ij}
$$

$$
E[xx^T] - E[x]E[x^T] \approx P + JmC(Jm)^T + \frac{1}{2}\sum_{i,j,k,l} Hm_{ij} C_{ik}C_{jl}(Hm_{kl})^T
$$

在创建和销毁辅助变量时，使用这两个方程以最小化对x的均值和协方差的影响。这一过程根据不确定性的变化自适应地进行。当不确定性较低时，可通过减少辅助维度的数量来简化分布，反之亦然，如图1所示。

摧毁辅助维度

如果我们选择k空间中的一个基，使得C = I，那么我们可以通过对剩余参数进行这些调整来消除k中的维度q：

$$
l \leftarrow l + \frac{1}{2} Hm_{qq}, \quad P_{ij} \leftarrow P_{ij} + \Delta P_{ij} = P_{ij} + J_{iq}^m J_{jq}^m + \sum_k H_{iqk}^m H_{jqk}^m - \frac{1}{2} H_{iqq}^m H_{jqq}^m
$$

通常，当上述 ΔP 的迹在某个 q 下低于阈值时，我们将通过进行上述调整并设置 kq = 0.2 来消除该维度。ΔP 在更新期间减小，在预测期间增大，因此我们通常在利用新观测完成更新后进行此项检查。

2 我们在仿真中使用阈值0.01。

创建辅助维度

我们可以通过将单位方向u上的部分不确定性从P转移到一个新的k维q中，从而创建新的辅助维度。为此，我们必须选择新的参数以满足Cqq= 1和Hqk= 0的这些约束条件：

$$
Jmq = r \sqrt{\frac{1 - d}{u}}, \quad P = P - u(1 - d)r^2u^T,
$$

$$
P^{-1} = P^{-1} + P^{-1}u(1 - d)r^2(P^{-1}u)^T / d
$$

其中 $ r^{-2} = u^T P^{-1}u $，d是一个用于保持P正定的小数值。在实际应用中，我们使用P的最大特征值作为r²，对应的特征向量作为u。如果该特征值超过某个阈值3，则创建一个新的维度q。

分布参数化—反粒子

我们将假设向量函数 mk 可以通过两种方式进行参数化。一种是通过某种规范参数化 p，该参数化可用于直接计算任意 lambda 下的 mk。另一种是在一组 k = ui 处的 mk 的取值，由此可以推导出规范参数。这组取值记为 xi = mui。

这些 ui 按照均值 k = 0 值周围分布的方式，利用 C 进行选择，类似于在线性回归卡尔曼滤波器（无迹卡尔曼滤波器）中选择回归（sigma）点的方法。

我们将集合 {xi} 称为反粒子，因为它们在粒子滤波器中的作用类似于粒子，但并非随机样本。我们假设可以根据需要在规范参数化与反粒子之间相互转换。

$$
{xi} \leftrightarrow {p} \quad (4)
$$

反粒子将足以进行预测，并且是更新的结果。我们会发现有必要使用规范参数进行更新。在创建和销毁辅助维度时也会使用它们。

4 非线性过程的估计

所有估计器的起点都是过程模型：

$$
x_k = f(x_{k-1}; u_k) + x_k, \quad x_k \sim G(0, Q_k) \quad (5)
$$

$$
z_k = h(x_k) + v_k, \quad v_k \sim G(0, R_k) \quad (6)
$$

这里，xk是状态，zk是测量值，uk是控制信号，Q/R 是白噪声协方差。我们将使用下标 k − 1|k − 1和 k|k − 1分别表示第k次预测之前和之后的参数。

3 我们为此阈值使用1.0。

预测

预测通过在 xk−1 上进行边缘化来完成：

$$
\int p(x_k|x_{k-1}; u_k)p_{k-1|k-1}(x_{k-1})dx_{k-1} = p_{k|k-1}(x_k) \quad (7)
$$

对于上述过程以及我们对分布所假设的形式，这变为

$$
\int G(x_k - m_{k|k-1}, p_{k|k-1})G(k; C_{k|k-1})(dk)^{nk} \approx/
\int\int G(x_k - f(x_{k-1}; u_k); Q_k)G(x_{k-1} - m_{k-1|k-1}; P_{k-1|k-1})G(k; C_{k-1|k-1})dx_{k-1}(dk)^{nk}
$$

我们像在扩展卡尔曼滤波器中一样，在估计状态 $ x_k = m_{k-1|k-1} $ 附近进行线性化。

$$
f(x_{k-1}; u_k) \approx x_k + Jf_k(x_{k-1} - m_{k-1|k-1}), \quad Jf_k \equiv \frac{\partial f(x_{k-1};u_k)}{\partial x_{k-1}} | {x {k-1}=m_{k-1|k-1}}
$$

$$
x_k \equiv f(m_{k-1|k-1}; u_k), \quad P_k \equiv Q_k + Jf_k P_{k-1|k-1}(Jf_k)^T
$$

使用这些替换，我们可以进行上述边缘化并得到：

$$
\int G(x_k - m_{k|k-1}, P_k)G(k; C_{k-1|k-1})(dk)^{nk}
$$

根据该方程，我们制定以下预测规则：

$$
P_{k|k-1} = P_k| {k=0}, \quad C {k|k-1} = C_{k-1|k-1}, \quad (8)
$$

$$
m_{k|k-1} = x_k \Rightarrow {x_{i|k-1}} = {f(x_{i|k-1})} \quad (9)
$$

近似条件是Pk不随k变化，并且通过将反粒子映射到非线性f中，m_{k|k-1}将在所有k下遵循 x_k。

我们最终得到一个简单的预测公式，该公式使用扩展卡尔曼滤波器的预测方法处理P，保持C不变，并通过动力学方程以与扩展卡尔曼滤波器处理均值相同的方式映射反粒子，如图1所示。如果不需要进行更新，则可以直接将反粒子输入到下一个预测步骤中，而无需转换为规范形式。

更新

在更新过程中，利用观测测量来形成后验概率分布。更新最重要的部分是找到 xk 和 k 的最大似然估计（MLE）。为此使用了规范参数化。一旦找到该最大似然估计，就会在其周围生成一组新的反粒子。这些反粒子随后即可用于下一步的预测步骤。

设 Dz(xk) = zk − h(xk) 并应用贝叶斯规则，可得后验概率：

$$
G(x_k - m_{k|k}, P_{k|k})G(k; C_{k|k}) \propto G(x_k - m_{k|k-1}, P_{k|k-1})G(Dz(x_k); R)G(k; C_{k|k-1})
$$

对上述右侧取指数，我们看到

$$
(x_{MLE}, k_{MLE}) = \arg \min_{x_k,k} g(x_k, k)
\quad (10)
$$

其中 g 由以下公式给出：

$$
g(x_k; k) = [(x_k - m_{k|k-1})^T P^{-1} {k|k-1}(x_k - m {k|k-1}) + Dz(x_k)^T R^{-1} Dz(x_k) + k^T C^{-1}_{k|k-1}k]/2
$$

我们将反复使用函数h的线性化：

$$
Dz(x_k) = z_k - h(x_k) \approx z_k - h(x) + J_h(x_k - x) \quad (11)
$$

我们进行三阶段更新，如图 2 所示。<第一阶段>仅改变 k，同时保持 xk − m_{k|k-1} 不变。这将沿 mk 曲线移动以最小化式(10)。<第二阶段>同时改变 xk 和 k。<第三阶段>保持 k= ui 不变，改变 xk 以寻找更新后的反粒子集。

更新阶段1—接近最大似然估计

我们首先将反粒子集转换为规范参数化。我们寻求m_{k|k-1}曲线上的一个点k，以最好地解释这些测量值。

$$
k_0 = \arg \min_k g(m_{k|k-1}; k) \quad (12)
$$

我们使用高斯‐牛顿法，从k0= 0开始，

$$
\nabla g = (C^{-1} {k|k-1}k_0 - (J_h J_m^{k|k-1})^T R^{-1}_k Dz)| {x=m_{k_0|k-1}}
\quad (13)
$$

$$
X \equiv C^{-1} {k|k-1} + (J_h J_m^{k|k-1})^T R^{-1}_k J_h J_m^{k|k-1}| {x=m_{k_0|k-1}}
\quad (14)
$$

$$
\Delta k_0 = -X^{-1} \nabla g \quad (15)
$$

其中 Jm^{k|k-1} 表示 m_{k|k-1} 关于 k 的雅可比矩阵。我们将使用此 Δk0 迭代地逼近方程(12)的解。

更新阶段2—寻找最大似然估计

在第二阶段，我们通过改变xk和k来求解方程(10)。我们采用高斯‐牛顿法，起始点为xk= m_{k_0} 和k= k_0。

$$
\nabla_x = (J_m^{k|k-1})^T P^{-1} {k|k-1}(x_k - m {k|k-1}) - C^{-1}_{k|k-1}k \quad (16)
$$

$$
w_x = (J_h)^T R^{-1} k Dz(x_k) - P^{-1} {k|k-1}(x_k - m_{k|k-1}) \quad (17)
$$

$$
W_{kk} = (J_m^{k|k-1})^T P^{-1} {k|k-1} J_m^{k|k-1} + C^{-1} {k|k-1} \quad (18)
$$

$$
W_{xk} = P^{-1}_{k|k-1} J_m^{k|k-1} \quad (19)
$$

$$
W_{xx} = (J_h)^T R^{-1} k J_h + P^{-1} {k|k-1} \quad (20)
$$

$$
\begin{bmatrix}
\Delta k \
\Delta x_k
\end{bmatrix}
= W^{-1} w =
\begin{bmatrix}
W_{kk} & W_{xk}^T \
W_{xk} & W_{xx}
\end{bmatrix}^{-1}
\begin{bmatrix}
\nabla_x \
w_x
\end{bmatrix}
\quad (21)
$$

更新 C 和 P

我们像IEKF一样设置P_{k|k}=(W_{xx})^{-1}| {MLE}，这给出了在最大似然估计附近的分布的正确形状。对于C更新，我们注意到C {k|k}是后验分布中k的协方差，即公式(2)。通过在最大似然估计附近进行展开来估计此值，我们得到：

$$
C_{k|k} \approx \int_{-1}^{1} (k - k_{MLE})^2 G((k - k_{MLE}, x - x_{MLE}); W^{-1})(dx)^n(dk)^{nk} \quad (22)
$$

$$
C^{-1} {k|k} = [W {kk} - W_{xk}^T(W_{xx})^{-1}W_{xk}]|_{x_k, k = MLE} \quad (23)
$$

$$
= C^{-1} {k|k-1} + (J_h J_m)^T (J_h P {k|k-1}(J_h)^T + R)^{-1} J_h J_m \quad (24)
$$

更新第三阶段—创建新的反粒子集

在第三阶段，我们需要创建一个反映测量值的新反粒子集。利用k_{MLE}和C来生成ui的新值，然后通过在约束条件k= ui下最小化g来产生新的反粒子集。

$$
x_{i|k} = \arg \min_{x_k} g(x_k; u_i) \quad (25)
$$

$$
\Delta x_{i|k} = (W_{xx})^{-1} w_x | {x_k = x {i|k}, k=u_i} \quad (26)
$$

我们再次使用高斯‐牛顿法，这次从 $ x_i = m_{u_i|k-1} $:

高斯‐牛顿迭代

在三个更新阶段中的每一个阶段，我们都会计算一个Δ增量。通常情况下，该Δ增量的方向和大小都不正确，尽管它大致上是准确的。为了弥补方向上的偏差，我们采用迭代方法，并在g的变化不再显著时停止。为了弥补大小上的偏差，我们在Δ的方向上进行线搜索，以找到沿该直线g值的最小值。

具体做法是计算从0到2Δ之间直线上N个点处的g值，然后利用二次拟合对最小值及其两侧的点进行插值，从而得到最小值。4

5 QAF和TAF

二次反粒子滤波器 QAF 使用这种参数化：

$$
mk = l + Kk + \frac{1}{2} k^T Ck \quad (27)
$$

其中 {p} = {l + K；C}。5 我们选择 C 为对角矩阵，则：

$$
u_0 = 0, \quad u_i = (0,…, \sqrt{C_{ii}},…, 0)^T, \quad u_{-i} = -u_i,
$$

$$
u_{i;j} = (u_i + u_j)/\sqrt{2}, \quad i, j \in {1, 2,…n_k}, j < i:
$$

这导致高斯分布中的线性相关被推广为抛物线相关。抛物线无法对较大的圆形非线性进行建模，这促使我们尝试圆形参数化。三角反粒子滤波器TAF采用这种参数化方法：

$$
m_k^h = l_h + \sum_j a_{hj} k_j \quad (28)
$$

$$
m_k^i = l_i + q_i \sin \sum_j a_{ij} k_j + \phi_i (\cos \sum_j b_{ij} k_j - 1), \quad i \neq h \quad (29)
$$

{p} = {l；q；a；φ；b}。6 对于TAF，我们使用额外的2nk个值u±2i = u±i/2；其中 i = 1；2；…；nk。

4 我们使用了 N = 21. 5K 是来自 k → x 空间的矩阵，且 x 向量 C 是关于 k 的对称矩阵。6a 和 b 是来自 k → x 空间的矩阵。

反粒子滤波器——一种自适应非线性估计器

6 实验

我们对机器人在二维空间中的运动进行了仿真。

$$
f(x; u) =
\begin{bmatrix}
x \
y \
h
\end{bmatrix}
+
\begin{bmatrix}
\cos h & 0 \
\sin h & 0 \
0 & 1
\end{bmatrix}
\begin{bmatrix}
\Delta s \
\Delta h
\end{bmatrix}
\quad (30)
$$

其中 $ Q_k = J_u Q J_u^T + 10^{-10}I $，$ J_u $ 是 $ f $ 关于 $ u $ 的雅可比矩阵，$ Q $ 为 $ 2 \times 2 $ 对角矩阵。观测值为到已知数据关联的点特征的距离和方位。我们的无迹卡尔曼滤波器和粒子滤波器7实现方式与[33]中相同，而迭代无迹卡尔曼滤波器是其迭代版本。

第一次实验中，我们针对Q的3个取值（小、中、大）分别进行了800次仿真。机器人从特征圆的中心出发，以里程计读数稳定增加（0.2, 0, 0）的方式移动，其真实路径则根据Q添加了相应的噪声。该实验设计为机器人先观测到一个提供部分位姿信息的第一个特征，随后再观测到一个提供精确位姿的第二个特征。一致性和准确性在第一次更新前、更新时、第二次特征更新前、第二次特征更新时以及第二次观测到特征后的某些时刻进行了分析。

一次运行的示例如图3所示。可以看出，自适应滤波器能够近似建模新月形分布。图4显示了首次更新后均方根xy误差的变化情况。我们看到 QAF和TAF比其他方法收敛更快。注意，粒子滤波器 tends to gets stuck at a fixed error depending on the density of particles while the other methods improve the pose more rapidly. 图4还显示了在(x, y, h) 误差中超出 ±1, ±1, ±0.1 区域的运行次数。9这表明存在发散估计，因为两个特征的测量应将位姿带入该区域。为了评估一致性，我们基于马氏距离计算了一个检验统计量

$$
d^2 = (x - x_{true})^T (Cov_x)^{-1} (x - x_{true}) \quad (31)
$$

累积 $ \chi^2_3 $ 分布 $ d^2 $ 在每个时刻都被记录下来。如果误差服从高斯分布且估计是一致的，则该分布将是均匀分布。通过对这些值排序并绘制图形，我们可以将经验分布与图5左上角的理想直线进行比较。fi无法展示所有这些图，但我们可以通过绘制所有图表随时间相对于更新的柯尔莫哥洛夫‐斯米尔诺夫检验统计量（图5）。该统计量是经验曲线与理想曲线之间的最大垂直距离。通过此处显示的粒子滤波器极差的一致性，可以看出粒子耗尽的影响。TAF 的值总体较低，并且比其他方法更早达到“一致”的高斯后验分布。‘一致’高斯后验分布。

表1总结了实验1的平均计算时间。由于这取决于MATLAB实现细节和仿真的难度，因此该数据参考价值有限，但可以大致了解时间上的权衡。我们可以看出当非线性较强时（Q最大时可达13个辅助维度），TAF似乎比QAF快2倍。这很可能是因为TAF的参数化更容易向最大似然估计移动。对于具有中等非线性的自适应滤波器，其耗时因子约为高斯估计器的2–5倍。

然后我们观察了在一系列每次均未提供足够信息以完全定位机器人的情况下，误差在更新后的表现。我们进行了使用方形四特征图和依次观测每个特征的真实路径的仿真。从图6中可以看出，自适应滤波器对围绕方形路径累积的误差较为不敏感。

表1 实验1中各种滤波器的平均计算时间（单位：秒）
Q(2,2)
—
1E-5
1E-4
1E-3

7 结论

我们研究了一种全新的非线性滤波器——自适应滤波器。自适应滤波器方法在精度上可显著优于其他流行的递归非线性估计器。在此特定问题上，TAF 在某些方面略优于为其量身定制的 QAF。QAF 是一种更为通用的滤波器。我们还表明，在二维机器人定位问题中，新估计器相较于 EKF、IEKF、UKF、IUKF 和 PF 显著更加一致。通常情况下，PF 能够表示比自适应滤波器更多的分布形态，因此它是一种更通用的解决方案，特别是能够估计多模态分布。对于可以近似用自适应滤波器建模的问题，自适应滤波器具备优势，使其成为一种可行的估计器选择。