19、无迹卡尔曼滤波与无迹粒子滤波技术解析

无迹卡尔曼与粒子滤波技术解析

火锅底料102

于 2025-11-09 14:50:17 发布

阅读量21

点赞数

CC 4.0 BY-SA版权

分类专栏：卡尔曼滤波与神经网络文章标签：无迹卡尔曼滤波联合UKF 无迹粒子滤波器

本文链接：https://blog.youkuaiyun.com/spark7igniter/article/details/154627417

卡尔曼滤波与神经网络专栏收录该内容

20 篇文章 ¥499.90

订阅专栏¥69.90

会员秒杀 ¥9.9 重磅福利

超级会员免费看

无迹卡尔曼滤波与无迹粒子滤波技术解析

1. 双无迹卡尔曼滤波（UKF）与联合UKF方法

1.1 双扩展卡尔曼滤波器（EKF）与双UKF

在双扩展卡尔曼滤波器中，信号和权重采用独立的状态空间表示，同时运行两个EKF分别进行信号和权重估计。在每个时间步，信号滤波器使用当前权重估计值，权重滤波器使用当前信号状态估计值。而在双UKF算法中，状态和权重估计均使用UKF完成。

1.2 联合扩展卡尔曼滤波器与联合UKF

联合扩展卡尔曼滤波器将信号状态向量和权重向量连接成一个联合状态向量 $[x_k^T w_k^T]^T$。通过以下状态空间方程进行递归估计：
[
\begin{bmatrix}
x_{k + 1} \
w_{k + 1}
\end{bmatrix}
=
\begin{bmatrix}
F(x_k, u_k, w_k) \
I w_k
\end{bmatrix}
+
\begin{bmatrix}
B v_k \
r_k
\end{bmatrix}
\quad (7.94)
]
[
y_k = [1 \ 0 \ \cdots \ 0]
\begin{bmatrix}
x_k \
w_k
\end{bmatrix}
+ n_k
\quad (7.95)
]
然后在联合状态空间上运行EKF以同时估计状态 $x_k$ 和权重 $w$。同样，我们采用UKF替代EKF。

2. 双估计实验

2.1 含噪时间序列实验

为清晰展示UKF相对于EKF的优势，我们对两个时间序列进行实验。第一个是带有加性噪声（SNR ≈ 3 dB）的Mackey - Glass - 30混沌序列，第二个是由高斯过程噪声驱动且带有加性白高斯噪声（SNR ≈ 3 dB）的自回归神经网络生成的混沌时间序列。在Mackey - Glass问题中，所有滤波器使用具有tanh隐藏激活函数和线性输出层的标准6 - 10 - 1多层感知器（MLP）；第二个问题使用5 - 3 - 1 MLP。假设与状态相关的过程噪声和测量噪声方差已知，且仅观察到含噪时间序列，无干净参考用于训练。

不同双和联合卡尔曼估计方法的训练曲线如图7.13所示，双UKF对Mackey - Glass序列的最终估计也一并展示，UKF算法的优越性能显而易见。

实验类型	使用模型	噪声情况
Mackey - Glass - 30混沌序列	6 - 10 - 1 MLP	SNR ≈ 3 dB
自回归神经网络时间序列	5 - 3 - 1 MLP	SNR ≈ 3 dB

2.2 模式估计实验

本实验使用联合UKF估计质量 - 弹簧系统的模式。尽管系统是线性的，但同时估计固有频率 $\omega_1$ 和 $\omega_2$ 与状态是一个非线性估计问题。实验中，输入过程噪声SNR约为100 dB，测量位置 $y_1$ 和 $y_2$ 的加性噪声SNR为60 dB。使用固定创新协方差 $R_r$ 进行参数估计，采样以奈奎斯特速率（基于 $\omega_2$）进行，这突出了EKF中线性化的影响。采样速率越快，EKF和UKF的性能越接近。

2.3 F15飞行模拟实验

在F15飞机模型上进行联合估计实验，模拟包括车辆非线性动力学、发动机和传感器噪声建模以及基于查找表的大气建模。使用增益调度TECS控制器的闭环系统控制模型。模拟任务包括快速下降、短战术运行、180°转弯和上升，可能出现安定面故障。测量值为带有加性噪声（20 dB SNR）的状态，湍流约为1 m/s RMS。联合UKF在任务期间估计12个状态以及与气动力和力矩对应的参数，且估计状态未用于控制回路。结果表明，即使出现故障，UKF仍能跟踪状态和参数。由于模拟器的“黑盒”性质不利于计算EKF所需的雅可比矩阵，因此未进行EKF对比实验。

2.4 双倒立摆实验

使用联合UKF同时估计双倒立摆的状态和系统参数。观测值为六个状态的含噪测量值，估计状态用于闭环控制，参数估计用于每个时间步的控制器设计。实验开始时，状态和参数均未知，控制系统不稳定，但UKF在一次试验内实现了摆的收敛和稳定，且未出现碰撞。

3. 无迹粒子滤波器

3.1 蒙特卡罗模拟与顺序重要性采样

粒子滤波基于蒙特卡罗模拟和顺序重要性采样（SIS），目标是通过递归近似完整的后验状态密度直接实现最优贝叶斯估计。在蒙特卡罗模拟中，从后验分布中抽取一组加权粒子（样本），将积分映射为离散和。后验滤波密度可通过以下经验估计近似：
[
\hat{p}(x_k | Y_k^0) = \frac{1}{N} \sum_{i = 1}^{N} \delta(x_k - X^{(i)} k)
]
其中，随机样本 ${X^{(i)}_k; i = 1, \cdots, N}$ 从 $p(x_k | Y_k^0)$ 中抽取，$\delta(\cdot)$ 为狄拉克δ函数。任何形式的期望 $E(g(x_k))$ 可近似为：
[
E(g(x_k)) \approx \frac{1}{N} \sum {i = 1}^{N} g(X^{(i)}_k)
\quad (7.97)
]
例如，令 $g(x) = x$ 可得最优最小均方误差（MMSE）估计 $\hat{x}_k = E[x_k | Y_k^0]$。

由于直接从后验密度函数采样通常不可行，我们使用重要性采样，从已知的提议分布 $q(X_k^0 | Y_k^0)$ 采样。提议分布的选择是关键设计问题，通常选择便于采样的形式。通过重要性采样，期望 $E(g_k(X_k^0))$ 可表示为：
[
E(g_k(X_k^0)) = \frac{E_{q(\cdot | Y_k^0)}(w_k(X_k^0) g_k(X_k^0))}{E_{q(\cdot | Y_k^0)}(w_k(X_k^0))}
]
其中，未归一化重要性权重 $w_k = \frac{p(Y_k^0 | X_k^0) p(X_k^0)}{q(X_k^0 | Y_k^0)}$。重要性权重可通过以下递归更新：
[
w_k = w_{k - 1} \frac{p(y_k | x_k) p(x_k | x_{k - 1})}{q(x_k | X_{k - 1}^0, Y_k^0)}
\quad (7.99)
]

3.2 重采样和马尔可夫链蒙特卡罗（MCMC）步骤

顺序重要性采样（SIS）算法存在重要性权重方差随时间随机增加的问题，通常经过几次迭代后，一个归一化重要性权重趋近于1，其余趋近于0，大量样本因权重数值过小而被有效移除。为避免这种退化，可使用重采样或选择阶段消除低重要性权重样本，复制高重要性权重样本，之后通常进行MCMC移动步骤以增加样本多样性，同时不影响后验分布。

常见的重采样方案包括：
- 采样 - 重要性重采样（SIR） ：将狄拉克随机测度 ${X^{(i)}_k, \tilde{w}^{(i)}_k}$ 映射为等权重随机测度 ${X^{(j)}_k, N^{-1}}$，通过从离散集 ${X^{(i)}_k; i = 1, \cdots, N}$ 以概率 ${\tilde{w}^{(i)}_k; i = 1, \cdots, N}$ 均匀采样实现。
- 残差重采样 ：分两步进行，第一步使用地板函数确定每个粒子的子样本数 $N_A^i = \lfloor N \tilde{w}^{(i)}_t \rfloor$，复制粒子 $N_A^i$ 次；第二步使用SIR选择剩余样本，新权重 $w’^{(i)}_t = \tilde{N}_t^{-1} (\tilde{w}^{(i)}_t N - N_A^i)$。该方法计算成本低于纯SIR，样本方差也更低，本实验均采用残差重采样。

3.3 粒子滤波算法

通用粒子滤波算法的伪代码如下：

1. 初始化: k = 0
    - 对于 i = 1, ..., N，从先验 p(x0) 中抽取状态 X^(i)_0。
2. 对于 k = 1, 2, ...
    (a) 重要性采样步骤
        - 对于 i = 1, ..., N，采样 X^(i)_k ~ q(xk | x^(i)_0:k - 1, Yk^0)。
        - 对于 i = 1, ..., N，计算重要性权重（未归一化）：
            w^(i)_k = w^(i)_k - 1 * (p(yk | X^(i)_k) * p(X^(i)_k | X^(i)_k - 1)) / q(X^(i)_k | X^(i)_0:k - 1, Yk^0)。
        - 对于 i = 1, ..., N，归一化重要性权重：
            w~^(i)_k = w^(i)_k / (Σ(j = 1 to N) w^(j)_k)。
    (b) 选择步骤（重采样）
        - 复制/抑制具有高/低重要性权重 w~^(i)_k 的样本 X^(i)_k，以获得 N 个近似服从 p(x^(i)_k | Yk^0) 分布的随机样本 X^(i)_k。
        - 对于 i = 1, ..., N，设置 w^(i)_k = w~^(i)_k = N - 1。
    (c) MCMC 移动步骤（可选）
    (d) 输出: 算法输出一组样本，用于近似后验分布：
        p^(x_k | Y_k^0) = (1 / N) * Σ(i = 1 to N) δ(x_k - X^(i)_k)。
        最优 MMSE 估计器为：
        x^_k = E(x_k | Y_k^0) ≈ (1 / N) * Σ(i = 1 to N) X^(i)_k。
        类似地，函数 g(x_k) 的期望也可作为样本平均值计算。

3.4 EKF和UKF粒子滤波器

为改进提议分布的选择并解决样本耗尽问题，可使用EKF生成的高斯近似作为最优提议分布，即 $q(x_k | X_{k - 1}^0, Y_k^0) \triangleq q_n(x_k | Y_k^0)$，通过为每个粒子使用单独的EKF生成和传播高斯提议分布 $q_n(x^{(i)}_k | Y_k^0) = n(\hat{x}^{(i)}_k, P^{(i)}_k)$。

将EKF替换为UKF可更准确地传播状态分布的高斯近似的均值和协方差，UKF生成的分布与真实后验分布的支持重叠更大。基于UKF生成提议分布的粒子滤波器称为无迹粒子滤波器（UPF），在通用粒子滤波算法的重要性采样步骤中，将“对于 i = 1, …, N，采样 $X^{(i)} k \sim q(x_k | x^{(i)} {0:k - 1}, Y_k^0)$”替换为以下UKF更新步骤：
- 更新每个粒子的先验（k - 1）分布 ：
- 计算sigma点 ：
[
X^{(i)a} {k - 1} = [\hat{X}^{(i)a} {k - 1} \ \hat{X}^{(i)a} {k - 1} + \gamma \sqrt{P^{(i)a} {k - 1}} \ \hat{X}^{(i)a} {k - 1} - \gamma \sqrt{P^{(i)a} {k - 1}}]
\quad (7.109)
]
- 粒子未来传播（时间更新） ：
[
X^{(i)x} {k|k - 1} = F(X^{(i)x} {k - 1}, u_k, X^{(i)v} {k - 1})
]
[
\hat{X}^{(i)} {k|k - 1} = \sum_{j = 0}^{2L} W^{(m)} j X^{(i)x} {j,k|k - 1}
\quad (7.110)
]
[
P^{(i)} {k|k - 1} = \sum {j = 0}^{2L} W^{(c)} j (X^{(i)x} {j,k|k - 1} - \hat{X}^{(i)} {k|k - 1}) (X^{(i)x} {j,k|k - 1} - \hat{X}^{(i)} {k|k - 1})^T
\quad (7.111)
]
[
Y^{(i)} {k|k - 1} = H(X^{(i)x} {k|k - 1}, X^{(i)n} {k - 1})
]
[
\hat{y}^{(i)} {k|k - 1} = \sum {j = 0}^{2L} W^{(m)} j Y^{(i)} {j,k|k - 1}
\quad (7.112)
]
- 合并新观测（测量更新） ：
[
P_{\tilde{y} k \tilde{y}_k} = \sum {j = 0}^{2L} W^{(c)} j (Y^{(i)} {j,k|k - 1} - \hat{y}^{(i)} {k|k - 1}) (Y^{(i)} {j,k|k - 1} - \hat{y}^{(i)} {k|k - 1})^T
\quad (7.113)
]
[
P {x_k y_k} = \sum_{J = 0}^{2L} W^{(c)} j (X^{(i)} {j,k|k - 1} - \hat{X}^{(i)} {k|k - 1}) (Y^{(i)} {j,k|k - 1} - \hat{y}^{(i)} {k|k - 1})^T
\quad (7.114)
]
[
K_k = P {x_k y_k} P_{\tilde{y} k \tilde{y}_k}^{-1}
]
[
\hat{X}^{(i)}_k = \hat{X}^{(i)} {k|k - 1} + K_k (y_k - \hat{y}^{(i)} {k|k - 1})
\quad (7.115)
]
[
P^{(i)}_k = P^{(i)} {k|k - 1} - K_k P_{\tilde{y}_k \tilde{y}_k} K_k^T
\quad (7.116)
]
- 从 $q(x^{(i)} k | x^{(i)} {0:k - 1}, Y_k^0) \approx n(\hat{X}^{(i)}_k, P^{(i)}_k)$ 中采样 $X^{(i)}_k$ 。

其余步骤保持不变。

综上所述，UKF在多种实验中展现出优于EKF的性能，而无迹粒子滤波器（UPF）通过结合UKF改进了粒子滤波的提议分布选择，进一步提升了滤波效果。这些方法在含噪信号处理、系统状态估计等领域具有重要应用价值。

graph TD;
    A[初始化] --> B[重要性采样步骤];
    B --> C[选择步骤（重采样）];
    C --> D{MCMC移动步骤};
    D -- 可选 --> E[输出];
    D -- 不选 --> E;

该流程图展示了通用粒子滤波算法的主要步骤，从初始化开始，经过重要性采样、重采样，可选择进行MCMC移动步骤，最终输出结果。

graph TD;
    A[计算sigma点] --> B[粒子未来传播（时间更新）];
    B --> C[合并新观测（测量更新）];
    C --> D[采样];

此流程图展示了无迹粒子滤波器（UPF）中UKF更新步骤的主要流程，包括计算sigma点、时间更新、测量更新和采样。

4. 技术对比与优势总结

4.1 UKF与EKF对比

在多个实验中，UKF相较于EKF展现出明显的优势。在含噪时间序列实验里，不同双和联合卡尔曼估计方法的训练曲线直观地显示出UKF算法的优越性能。在模式估计实验中，EKF无法收敛到 $\omega_2$ 的真实值，而UKF能较好地完成估计任务。这是因为EKF在处理非线性问题时，采用线性化近似，会引入一定误差，尤其在采样速率较慢时，线性化的影响更为突出；而UKF通过无迹变换，更准确地描述状态分布的均值和协方差，能更好地适应非线性系统。

4.2 UPF与传统粒子滤波器对比

传统粒子滤波器在选择提议分布时，常使用过渡先验，可能导致与真实后验分布的重叠不足，出现样本耗尽问题。而无迹粒子滤波器（UPF）使用UKF生成提议分布，能更准确地传播状态分布的高斯近似的均值和协方差，UKF生成的分布与真实后验分布的支持重叠更大，从而有效解决样本耗尽问题，提高滤波效果。

4.3 优势总结

UKF优势 ：能更准确地处理非线性系统，避免EKF线性化带来的误差，在多种非线性估计问题中表现出色。
UPF优势 ：改进了粒子滤波的提议分布选择，结合UKF的优势，提升了粒子滤波器的性能，能更好地适应复杂的非线性、非高斯系统。

5. 应用场景分析

5.1 工业控制领域

在工业控制系统中，如双倒立摆系统，系统状态和参数往往是未知的，且存在噪声干扰。UKF和UPF可用于实时估计系统状态和参数，为控制器设计提供准确信息，实现系统的稳定控制。通过不断更新状态和参数估计值，调整控制器的参数，使系统能够快速适应外界环境的变化，提高控制精度和稳定性。

5.2 航空航天领域

在F15飞行模拟实验中，涉及车辆非线性动力学、发动机和传感器噪声等复杂因素。UKF和UPF可用于估计飞机的状态和参数，如位置、速度、姿态等，即使在出现故障的情况下，也能准确跟踪状态变化，为飞行控制和决策提供可靠依据。在航空航天的其他场景中，如卫星轨道估计、飞行器导航等，也可发挥重要作用。

5.3 信号处理领域

在含噪时间序列处理中，如Mackey - Glass混沌序列和自回归神经网络生成的时间序列，UKF和UPF可用于去除噪声，恢复真实信号。通过对信号状态和权重的估计，提高信号处理的质量，在通信、雷达等领域有广泛的应用前景。

6. 操作步骤总结

6.1 双UKF和联合UKF操作步骤

双UKF ：
1. 初始化信号和权重的状态。
2. 在每个时间步，使用UKF同时进行信号和权重的估计。
3. 将当前权重估计值用于信号滤波器，当前信号状态估计值用于权重滤波器。
联合UKF ：
1. 将信号状态向量和权重向量连接成联合状态向量。
2. 建立联合状态的状态空间方程。
3. 使用UKF对联合状态进行递归估计。

6.2 粒子滤波操作步骤

通用粒子滤波 ：
1. 初始化：从先验分布中抽取初始状态样本。
2. 重要性采样步骤：从提议分布中采样新状态，计算重要性权重并归一化。
3. 选择步骤（重采样）：复制高重要性权重样本，抑制低重要性权重样本。
4. 可选的MCMC移动步骤：增加样本多样性。
5. 输出：使用样本近似后验分布，计算最优MMSE估计器。
无迹粒子滤波器（UPF） ：
1. 执行通用粒子滤波的初始化步骤。
2. 在重要性采样步骤中，使用UKF更新每个粒子的状态：
  - 计算sigma点。
  - 进行粒子未来传播（时间更新）。
  - 合并新观测（测量更新）。
  - 从更新后的分布中采样新状态。
3. 执行通用粒子滤波的后续步骤。

7. 总结与展望

UKF和无迹粒子滤波器（UPF）在非线性估计和滤波问题中展现出卓越的性能。UKF通过无迹变换克服了EKF线性化的局限性，在多种实验中表现优于EKF；UPF结合UKF改进了粒子滤波的提议分布选择，解决了样本耗尽问题，进一步提升了滤波效果。这些方法在工业控制、航空航天、信号处理等领域具有重要的应用价值。

未来，随着科技的不断发展，这些技术有望在更多复杂场景中得到应用。例如，在人工智能和机器学习领域，可用于处理复杂的非线性模型和数据；在物联网和传感器网络中，可用于对大量含噪数据的处理和分析。同时，还可以进一步研究如何优化这些算法，提高计算效率和精度，以适应更实时、更复杂的应用需求。

graph TD;
    A[工业控制领域] --> B[实时估计系统状态和参数];
    B --> C[调整控制器参数];
    C --> D[提高控制精度和稳定性];
    E[航空航天领域] --> F[估计飞机状态和参数];
    F --> G[为飞行控制和决策提供依据];
    H[信号处理领域] --> I[去除噪声恢复真实信号];
    I --> J[提高信号处理质量];

该流程图展示了UKF和UPF在不同应用领域的主要作用流程，在工业控制中用于系统状态估计和控制，在航空航天中用于飞行状态估计和决策，在信号处理中用于信号恢复和质量提升。

技术方法	优势	应用场景
UKF	更准确处理非线性系统，避免EKF线性化误差	含噪时间序列处理、模式估计、F15飞行模拟、双倒立摆控制等
UPF	改进粒子滤波提议分布，解决样本耗尽问题	复杂非线性、非高斯系统的状态估计

此表格总结了UKF和UPF的优势及主要应用场景，便于读者对比和理解。

通过以上对UKF和UPF的详细解析，希望能帮助读者深入理解这些技术的原理、优势和应用，为实际应用提供参考。