EKF与UKF在四元数姿态估计中的性能对比

1. 项目背景与核心价值

在惯性导航、无人机控制和机器人姿态估计领域，四元数姿态解算一直是个经典难题。传统互补滤波虽然计算量小，但在动态环境下精度有限；而基于MEMS传感器的原始数据又存在噪声干扰和漂移问题。这时卡尔曼滤波家族就派上了大用场——特别是扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)这两种非线性滤波方法，它们对四元数姿态估计的适用性和性能差异，正是这个仿真项目要探究的核心。

我去年为一个工业级无人机项目做姿态控制器时，就深刻体会到算法选型的重要性：当时先用EKF实现了基础版本，但在高速机动时出现估计滞后；改用UKF后计算量增加了30%，但俯仰角误差从2.1°降到了0.8°。这个实战经验促使我系统性地对比这两种算法，于是有了这个仿真项目。

2. 四元数姿态表达的基础原理

2.1 为什么选择四元数

相比欧拉角，四元数没有万向锁问题；相比旋转矩阵，它只有四个参数且便于插值。其数学形式为：

code复制q = [q0, q1, q2, q3] = [cos(θ/2), u·sin(θ/2)] 

其中u是旋转轴单位向量，θ是旋转角度。在MATLAB仿真中，我通常用quaternion类来封装运算。

2.2 传感器噪声建模

MEMS陀螺的角速度噪声通常建模为：

code复制ω_meas = ω_true + β + η_v
β' = η_u (随机游走)

其中η_v是白噪声，η_u是零偏噪声。在仿真中我用Band-Limited White Noise模块生成，PSD设为0.01 deg²/s³。

3. EKF实现细节剖析

3.1 状态方程线性化

EKF的核心是对四元数微分方程进行一阶泰勒展开。状态向量取：

code复制x = [q0 q1 q2 q3 βx βy βz]'

雅可比矩阵计算时要注意四元数归一化约束，我采用的方法是在预测步骤后强制归一化：

matlab复制q_pred = q_pred / norm(q_pred);

3.2 观测模型设计

使用加速度计和磁强计作为观测：

code复制z = [a_meas; m_meas] = [R(q)·g; R(q)·m_ref] + v

其中g是重力向量，m_ref是地磁参考向量。这里有个坑：磁强计易受硬铁干扰，仿真