【组合导航简化代码】GNSS/SINS滤波代码例程，输出为经纬高，便于学习与理解

代码实现：GNSS/SINS组合导航系统的卡尔曼滤波仿真，用于估计载体的位置（经纬高）、速度和姿态角。

本例程通过高频 SINS 机械编排与低频 GNSS 校正相结合，实现对平台经纬度、海拔、速度和姿态的连续估计。

程序介绍

1. 初始化

   • 设定采样间隔 $\Delta t$、仿真时长、地球模型参数（长半轴 $a$、偏心率 $e$、自转角速度 ${\omega }_e$）。
   • 初始状态向量包含：
     $[\phi ,\lambda ,h,v_E,v_N,v_U,\varphi ,\theta ,\psi ,{\mathbf{b}}_g,{\mathbf{b}}_a{]}^T$，
     分别为经纬度、高程、东北天速度、横滚俯仰偏航和 IMU 偏置。

2. SINS机械编排预测

   • 姿态更新（简化积分）：

     $${\mathbf{\varphi }}_{k+1}={\mathbf{\varphi }}_k+({\mathbf{\omega }}_{\text{meas}}-{\mathbf{\omega }}_n)\Delta t.$$

   • 速度更新（导航系积分）：

     $${\mathbf{v}}_{k+1}={\mathbf{v}}_k+({\mathbf{C}}_b^n{\mathbf{f}}_{\text{meas}}+\mathbf{g})\Delta t.$$

   • 位置更新（曲率投影）：

     $${\phi }_{k+1}={\phi }_k+\frac{v_N}{R_M+h}\Delta t,{\lambda }_{k+1}={\lambda }_k+\frac{v_E}{(R_N+h)\cos \phi }\Delta t.$$

3. 卡尔曼滤波融合

   • 预测：

     $${\hat{\mathbf{x}}}_k^{-}=f\left({\hat{\mathbf{x}}}_{k-1}\right),P_k^{-}=F{P}_{k-1}{F}^T+Q.$$

   • 更新（每秒一次 GNSS）：

     $$K=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1},{\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K(z-H{\hat{\mathbf{x}}}_k^{-}).$$

运行结果

• 用圆周运动叠加高度波动模拟真实轨迹，生成对应 IMU/GNSS 观测。
• 保存并绘制经纬度、高度、速度和姿态随时间的变化，以及二维/三维轨迹。

三轴状态值曲线

角度和速度曲线

二维轨迹曲线（经度和纬度）

三维轨迹曲线

完整代码：

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者