课题进度04-滤波算法对比

1. 无迹卡尔曼滤波(Unscented Kalman Filter, UKF)

定义：是一种适用于非线性系统的滤波算法，被看作是扩展卡尔曼滤波(EKF)的改进版本，解决了 EKF 在处理强非线性系统时的精度低或不稳定问题。

基本思想：

UKF 的核心思想是使用一组经过精心选择的采样点（称为 sigma 点）来统计地近似非线性系统的传播特性，而不是像 EKF 那样通过一阶泰勒展开对非线性函数进行线性化处理。
这些 sigma 点经过非线性系统的传播后，UKF 使用这些点的加权求和来估计状态和协方差，而无需显式计算雅可比矩阵。

数学框架：

（1）状态模型

UKF 适用于如下的离散非线性系统模型：

状态转移方程： $\mathbf{x}_{k} = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1}$
观测方程： $\mathbf{z}_{k} = h(\mathbf{x}_k) + \mathbf{v}_k$

其中

$\mathbf{x}_k$是状态变量

$\mathbf{u}_k$是控制输入

$\mathbf{z}_k$是观测变量

$\mathbf{w}_k, \mathbf{v}_k$分别是过程噪声和测量噪声，假设是零均值高斯白噪声，协方差为$\mathbf{Q} 和 \mathbf{R}$

$f(\cdot)$和$h(\cdot)$是非线性状态转移函数和观测函数

UKF 的关键在于：用一组精心挑选的 sigma 点来捕捉状态均值$(\mathbf{x})$和协方差$\mathbf{P}$的非线性变化。

从当前状态的均值和协方差中生成 sigma 点集：

$\mathcal{X}0 = \mathbf{x}$

$\mathcal{X}{i} = \mathbf{x} + sqrt{(n+\lambda)\mathbf{P}{k-1}}$

$\mathcal{X}{i+n} = \mathbf{x} - \sqrt{(n+\lambda)\mathbf{P}_{k-1}}$

n 是状态维度，$\lambda$是缩放参数

用这些 sigma 点通过非线性函数 $f(\cdot)$和 $h(\cdot)$ 传播，根据传播后的 sigma 点重构新的状态均值和协方差。

UKF 和 EKF 一样分为两个阶段：预测阶段和更新阶段。

（1）预测阶段：

使用 $f(\cdot)$ 和过程噪声协方差 ($\mathbf{Q}$) 来估计下一时刻的状态均值和协方差。

预测状态： $\hat{\mathbf{x}}k^- = \sum{i=0}^{2n} Wi \cdot f(\mathcal{X}{k-1}^i) $]
预测协方差： $\hat{\mathbf{P}}k^- = \mathbf{Q}_k + \sum{i=0}^{2n} Wi \left[f(\mathcal{X}{k-1}^i) - \hat{\mathbf{x}}k^-\right]\left[f(\mathcal{X}{k-1}^i) - \hat{\mathbf{x}}_k^-\right]^T$

（2）更新阶段：

使用观测函数 ($h(\cdot)$) 和观测噪声协方差 ($\mathbf{R}$) 来更新状态估计。