卡尔曼滤波与粒子滤波在状态估计中的工程实践

1. 状态估计算法概述与选型思考

在工程实践中，我们常常需要从带有噪声的观测数据中推断系统的真实状态。2001年NASA火星探测器着陆过程中，正是依靠状态估计算法在传感器数据不完整的情况下准确判断了飞行器姿态。这类算法主要分为三类：卡尔曼滤波系列（KF/EKF/UKF）、粒子滤波（PF）以及新兴的深度学习方法。

我从2015年开始在无人机飞控系统中应用这些算法，发现每种方法都有其独特的适用场景。传统卡尔曼滤波（KF）要求系统必须是线性的，这在实际工程中几乎不存在。于是工程师们发展出两大分支：EKF通过泰勒展开局部线性化，而UKF则采用确定性采样逼近非线性分布。粒子滤波走的是另一条路——用蒙特卡洛方法近似后验概率分布。

重要提示：选择算法时首先要明确系统非线性程度。弱非线性系统用EKF足够，强非线性或非高斯噪声建议UKF或PF。

2. 扩展卡尔曼滤波(EKF)实现细节

2.1 EKF核心公式推导

EKF的核心思想是在当前估计点对非线性函数进行一阶泰勒展开。以无人机姿态估计为例，其状态转移函数f和观测函数h通常是非线性的：

code复制x_k = f(x_{k-1}, u_k) + w_k  
z_k = h(x_k) + v_k

线性化过程需要计算雅可比矩阵。在Matlab中我们可以用符号计算工具箱自动求导：

matlab复制syms x y z
f = [x*cos(y); z*exp(-x)];
F_jac = jacobian(f, [x y z]);

2.2 典型实现误区

我在2017年调试四轴飞行器时曾犯过一个经典错误——没有及时更新雅可比矩阵。EKF要求在每次预测和更新后立即重新计算雅可比矩阵，但为节省计算量，我固定使用了初始点的雅可比矩阵，导致姿态估计在大角度机动时完全失效。

正确的实现流程应该是：

1. 预测状态均值
2. 计算当前点的状态转移雅可比F
3. 预测协方差
4. 计算观测雅可比H
5. 更新卡尔曼增益
6. 更新状态估计
7. 更新协方差

3. 无迹卡尔曼滤波(UKF)技术解析

3.1 Sigma点采样策略

UKF通过精心选择的Sigma点捕获状态分布的统计特性。对于n维状态向量，通常选取2n+1个Sigma点。以车辆定位问题为例（状态维度为5），采