PSINS与PX4导航算法对比：从仿真代码看工程与科研的差异

PSINS与PX4导航算法对比：从仿真代码看工程与科研的差异

在导航算法的世界里，有两套代码常常被开发者们放在一起讨论：一个是西北工业大学严恭敏教授团队开发的PSINS工具箱，另一个是PX4飞控中广泛使用的EKF（扩展卡尔曼滤波）算法。如果你在实验室里做过组合导航的仿真，或者在无人机上调试过飞控，大概率对它们不会陌生。表面上看，它们都在解决同一个问题——如何融合惯性测量单元（IMU）和全球定位系统（GPS）的数据，得到更精确的姿态、速度和位置。但当你真正打开它们的源代码，从MATLAB的.m文件看到C++的.h/.cpp文件时，会感受到一种强烈的“气质”差异。这种差异，远不止是编程语言的不同，更像是两种思维模式的碰撞：一边是追求模型完备性与理论深度的科研仿真，另一边是追求实时性、鲁棒性与工程落地的嵌入式实践。今天，我们就抛开教科书式的理论对比，直接潜入代码的海洋，从滤波器最核心的“状态量”选择开始，看看这两种哲学是如何具体而微地塑造了每一行代码，并最终决定了它们各自的适用场景。

1. 状态量之争：误差量 vs. 直接量——两种滤波哲学的代码化身

所有卡尔曼滤波器的核心，都在于对“状态”的估计。而PSINS与PX4 EKF最根本的分歧，就从这里开始。

在PSINS的典型仿真demo（例如 test_SINS_GPS_153.m）中，你会看到它的状态向量被明确定义为15维的误差量。这15维具体包括：

• 3维失准角误差：数学平台系与真实导航系之间的微小角度偏差。
• 3维速度误差：惯性导航解算的速度与真实速度的差值。
• 3维位置误差：惯性导航解算的位置与真实位置的差值。
• 3维陀螺常值零偏：陀螺仪输出中不随时间变化的偏差。
• 3维加速度计常值零偏：加速度计输出中不随时间变化的偏差。

这种设计的哲学根源在于误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF）。其核心思想是：用一个“名义状态”进行高频率、高精度的惯性导航机械编排（PSINS中由 SINS 相关函数完成），这个名义状态会快速累积误差。同时，用一个独立的滤波器（ESKF）来估计这个名义状态与真实状态之间的“误差状态”。当有外部观测（如GPS）时，滤波器更新的是误差状态的估计值，然后将这个估计出的误差反馈校正到名义状态上，使其更接近真实值。

提示：误差量作为状态，其数值通常很小，动力学模型（即状态转移矩阵F）可以在一个标称工作点附近进行线性化，并且线性化误差较小。这使得滤波器更容易保持稳定，特别适合高精度、高动态的惯性导航系统分析。

让我们看一个PSINS中构建状态转移矩阵F的简化逻辑（概念性代码，非原文件）：

% 假设在东北天坐标系下，忽略一些次要项
fn = [f_E; f_N; f_U]; % 比力向量（加速度计测量值减去重力）
wnie = [0; omega_ie*cos(lat); omega_ie*sin(lat)]; % 地球自转角速度在导航系投影
wnen = ... % 导航系相对于地球的角速度（与速度、位置相关）

% F矩阵中与失准角、速度误差相关的典型块
F(1:3, 1:3) = -skew(wnie + wnen); % 失准角误差的微分方程
F(4:6, 1:3) = skew(fn); % 速度误差受失准角影响（有害加速度）
F(4:6, 4:6) = ... % 速度误差自身的项（科氏加速度等）
F(4:6, 10:12) = Cbn; % 速度误差受加速度计零偏影响（Cbn为姿态矩阵）
F(1:3, 7:9) = ... % 失准角误差受陀螺零偏影响

你可以看到，F 矩阵的每一个元素都有明确的物理意义，直接对应着惯性导航误差微分方程中的各项。这对于科研人员分析误差传播机理、评估不同误差源的影响权重是极其友好的。

截然不同的是，PX4 EKF2 选择了将直接量作为状态。 在它的 states.cpp 文件中