【基于PSINS】CKF滤波，观测量为航向角、位置、速度（共7维），附完整代码

本代码基于 PSINS 工具箱 实现了一个 15 维状态的容积卡尔曼滤波（CKF）算法，用于 SINS/GPS 组合导航系统。该算法在原有仅速度观测的 CKF153 模型基础上改进，新增 位置、航向角 作为观测输入，提升了导航精度。

以下是代码的核心功能与实现流程：

运行结果

三维轨迹：

• 三轴位置误差曲线：

• 三轴速度误差曲线：
  

完整代码

完整代码如下，在安装了PSINS工具箱后，直接运行即可：

% 【PSINS】位置、速度、航向角为观测的157，CKF。从速度观测的CKF153改进而来
% 2025-02-07/Ver1

% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear;