1. 项目概述
在自动驾驶、无人机导航和航天器控制等领域，三维空间的高精度导航一直是核心技术难题。传统单一导航系统各有局限：惯性导航系统（INS）短时精度高但误差会累积，全球卫星导航系统（GNSS）长期稳定但易受干扰。将两者优势结合的INS/GNSS组合导航技术，成为解决这一难题的关键方案。
我最近用Matlab实现了一个基于卡尔曼滤波（KF）和误差态卡尔曼滤波（ESKF）的三维组合导航算法，通过实际测试发现，ESKF在高动态场景下的表现远超标准KF。本文将分享从原理到实现的完整过程，包括核心算法对比、Matlab实现细节和实际测试结果。
2. 核心算法原理
2.1 标准卡尔曼滤波（KF）在导航中的应用
标准卡尔曼滤波是一种递归算法，通过"预测-更新"两个步骤实现对系统状态的最优估计。在组合导航中，KF的状态向量通常包含位置、速度、姿态等15个维度：
code复制x = [p_n, p_e, p_d, v_n, v_e, v_d, φ, θ, ψ, ∇x, ∇y, ∇z, εx, εy, εz]
其中p表示位置（北东地坐标系），v是速度，φθψ为姿态角，∇和ε分别代表加速度计和陀螺仪的零偏。
预测阶段使用INS的机械编排方程：
code复制x̂ₖ⁻ = Fₖ₋₁xₖ₋₁ + Bₖ₋₁uₖ₋₁
Pₖ⁻ = Fₖ₋₁Pₖ₋₁Fₖ₋₁ᵀ + Qₖ₋₁
更新阶段则利用GNSS观测：
code复制Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hₖx̂ₖ⁻)
Pₖ = (I - KₖHₖ)Pₖ⁻
注意：实际实现时要特别注意坐标系转换，我通常在Matlab中预先定义好转换矩阵，避免每次计算都重新生成。
2.2 误差态卡尔曼滤波（ESKF）的创新之处
ESKF的核心思想是不直接估计导航状态，而是估计状态误差量。其状态向量设计为：
code复制δx = [δp, δv, δθ, δ∇, δε]
其中δθ是三维姿态误差，采用最小旋转向量表示。
ESKF的工作流程有三大关键步骤：
-
误差状态预测：
code复制δx̂ₖ⁻ = 0 Pₖ⁻ = Fₖ₋₁Pₖ₋₁Fₖ₋₁ᵀ + Qₖ₋₁ -
误差状态更新：
code复制Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹ δx̂ₖ = Kₖ(zₖ - h(x̂ₖ⁻)) Pₖ = (I - KₖHₖ)Pₖ⁻ -
状态修正与重置：
code复制x̂ₖ = x̂ₖ⁻ ⊕ δx̂ₖ δx̂ₖ = 0
其中⊕表示将误差状态注入到名义状态的特殊运算，对于姿态而言就是四元数乘法。
3. Matlab实现详解
3.1 系统建模与参数初始化
首先定义INS误差模型，我采用ψ角误差模型建立状态转移矩阵F：
matlab复制% 位置误差
F(1:3,4:6) = eye(3);
% 速度误差
F(4:6,7:9) = [-fn(3) 0 fn(1); 0 fn(3) -fn(2); -fn(2) fn(1) 0];
F(4:6,10:12) = Cbn; % 加速度计零偏
% 姿态误差
F(7:9,4:6) = [-1/R 0 0; 0 -1/R 0; 0 0 -1/R];
F(7:9,13:15) = -Cbn; % 陀螺零偏
过程噪声矩阵Q需要根据IMU性能设置：
matlab复制Q = diag([zeros(1,6), imu.gyro_noise^2*[1 1 1], imu.accel_noise^2*[1 1 1]]);
3.2 KF与ESKF的Matlab实现对比
标准KF的更新步骤较为直接：
matlab复制% KF更新
K = P_pred*H'/(H*P_pred*H' + R);
x_est = x_pred + K*(z - H*x_pred);
P_est = (eye(15) - K*H)*P_pred;
而ESKF需要额外的误差注入步骤：
matlab复制% ESKF误差注入
dq = 1/sqrt(1 + 0.25*norm(dtheta)^2) * [0.5*dtheta; 1];
q_est = quatmultiply(q_pred, dq');
v_est = v_pred + dv;
p_est = p_pred + dp;
3.3 可视化工具开发
为直观比较两种算法，我开发了三维轨迹对比工具：
matlab复制figure('Name','三维位置对比');
plot3(gt(:,1),gt(:,2),gt(:,3),'k-'); hold on;
plot3(kf(:,1),kf(:,2),kf(:,3),'b--');
plot3(eskf(:,1),eskf(:,2),eskf(:,3),'r-.');
legend('真值','KF','ESKF');
xlabel('北向(m)'); ylabel('东向(m)'); zlabel('天向(m)');
grid on; axis equal;
4. 性能测试与分析
4.1 静态场景测试
在静态基准测试中，两种算法表现接近：
- KF位置RMSE：0.83m
- ESKF位置RMSE：0.79m
但ESKF的姿态估计更优，俯仰角误差小约15%。
4.2 高动态场景测试
模拟无人机急转弯时，性能差异显著：
code复制 | 北向误差(m) | 东向误差(m) | 航向误差(deg)
-----------|-------------|-------------|--------------
KF | 3.27 | 2.85 | 5.63
ESKF | 1.02 | 0.89 | 1.57
4.3 GNSS信号中断测试
人为屏蔽GNSS信号30秒：
- KF位置误差累积达15.6m
- ESKF通过误差状态修正，误差控制在4.3m以内
5. 工程实践经验
5.1 参数调优技巧
-
过程噪声矩阵Q的调整：
- 先根据IMU规格书设置初始值
- 通过Allan方差分析实测数据微调
- 动态场景下可适当增大角速度噪声项
-
观测噪声矩阵R的设置：
matlab复制R = diag([gps.pos_noise^2*[1 1 1], gps.vel_noise^2*[1 1 1]]);建议初始值为GNSS接收机标称精度的1.2-1.5倍
5.2 常见问题解决
-
滤波发散问题：
- 检查IMU和GNSS时间同步，我通常采用PPS信号硬同步
- 验证坐标系一致性，确保所有数据在同一坐标系下
-
姿态估计异常：
matlab复制% 四元数规范化 q = q/norm(q);每次更新后必须做归一化处理
-
数值不稳定：
- 使用Joseph形式更新协方差矩阵
- 定期重置协方差矩阵对角线元素
6. 算法扩展与优化
6.1 自适应滤波改进
通过监测新息序列实现自适应调参：
matlab复制epsilon = z - H*x_pred;
S = H*P_pred*H' + R;
lambda = epsilon'*inv(S)*epsilon / size(z,1);
if lambda > threshold
R = R * (1 + alpha*(lambda-1));
end
6.2 松组合到紧组合的演进
紧组合需要处理原始观测数据：
- 伪距、载波相位观测模型
- 卫星时钟误差修正
- 电离层延迟补偿
我在Matlab中实现了基础版本，定位精度比松组合提升约30%。
这个项目从理论到实践让我深刻理解了组合导航的核心技术。ESKF虽然在概念上更复杂，但其性能优势在实际工程中非常明显。建议初次接触可以从KF开始，掌握基本原理后再过渡到ESKF实现。所有测试代码和数据集我已整理成完整工程，需要可以参考文末资源。