卡尔曼滤波与扩展卡尔曼滤波在导航系统中的应用

1. 卡尔曼滤波与扩展卡尔曼滤波在导航系统中的核心原理

在工程实践中，我们常常需要从带有噪声的观测数据中估计系统的真实状态。1960年，R.E.Kalman提出的卡尔曼滤波器（Kalman Filter, KF）为解决这一问题提供了优雅的数学框架。作为线性高斯系统的最优状态估计器，KF通过"预测-更新"的迭代机制，实现了对系统状态的最小方差估计。

1.1 卡尔曼滤波的基本方程

KF的核心在于两个交替进行的阶段：时间更新（预测）和量测更新（校正）。其数学表述如下：

状态预测方程：
x̂ₖ⁻ = Fₖx̂ₖ₋₁ + Bₖuₖ
Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ

量测更新方程：
Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hₖx̂ₖ⁻)
Pₖ = (I - KₖHₖ)Pₖ⁻

其中，x̂表示状态估计，P为误差协方差矩阵，F是状态转移矩阵，H为观测矩阵，Q和R分别代表过程噪声和观测噪声的协方差矩阵，K则是著名的卡尔曼增益。

实际工程中，Q和R的取值对滤波性能影响极大。通常需要通过系统辨识或经验调参来确定这些参数。一个实用的技巧是：Q反映你对系统模型的信任程度，R则体现对传感器的信任度。

1.2 扩展卡尔曼滤波的非线性处理

当系统存在非线性时，标准的KF不再适用。扩展卡尔曼滤波（Extended Kalman Filter, EKF）通过局部线性化解决了这一问题。具体做法是对非线性函数进行一阶泰勒展开：

非线性状态方程：
xₖ = f(xₖ₋₁, uₖ) + wₖ
zₖ = h(xₖ) + vₖ

线性化后的雅可比矩阵：
Fₖ ≈ ∂f/∂x|x̂ₖ₋₁
Hₖ ≈ ∂h/∂x|x̂ₖ⁻

这种线性化处理使得KF的框架得以延续，但也引入了线性化误差。在强非线性系统中，这种误差可能导致滤波发散。我在实际项目中就曾遇到过无人机剧烈机动时EKF估计失准的情况，后来通过限制最大角速度解决了问题。

2. 惯性导航系统中的滤波应用实践

2.1 INS误差模型构建

惯性导航系统（INS）的误差主要来源于惯性测量单元（IMU）的零偏、刻度因子误差和随机游走噪声。一个完整的INS误差状态通常包括：

• 姿态误差：3维
• 速度误差：3维
• 位置误差：3维
• 陀螺零偏：3维
• 加速度计零偏：3维

这样，一个基础的INS误差状态就有15维。在Matlab中，我们可以这样建模：

matlab复制% INS误差状态方程示例
function dx = insErrorModel(t, x, imu)
    % 姿态误差
    Cbn = quat2dcm(imu.attitude);
    dx(1:3) = -imu.angVel × x(1:3) - Cbn * x(7:9) + imu.gyroNoise;
    
    % 速度误差
    dx(4:6) = Cbn * diag(imu.acc) * x(10:12) + ...
              (cross(imu.acc, x(1:3)) * Cbn)' + imu.accNoise;