【基于PSINS】CKF滤波，观测量为航向角、位置、速度（共7维），提供下载链接

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

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

运行结果

三维轨迹：

• 三轴位置误差曲线：

• 三轴速度误差曲线：
  

核心功能

• 多源数据融合：通过 IMU（惯性测量单元）原始数据与外部观测（GPS 位置、速度、航向角）结合，实现高精度导航解算。
• 误差抑制：利用 CKF 算法处理非线性系统噪声和观测噪声，有效抑制 IMU 累积误差。
• 实时滤波：逐帧处理 IMU 数据，动态更新导航状态（姿态、速度、位置）。

代码改进点

• 观测维度扩展：从仅速度观测（CKF153）升级为 位置+速度+航向角 多维度观测（CKF157），增强系统鲁棒性。
• 自适应滤波：通过 ckf 函数实现容积卡尔曼滤波，优化状态估计的数值稳定性。
• 误差反馈机制：kffeedback 将滤波后的误差实时反馈到惯性导航系统（INS），修正导航参数。

---

实现流程

1. 初始化设置

   • 加载轨迹数据（trjfile），设置 IMU 误差模型（imuerrset）。
   • 初始化 INS 导航系统（insinit）和 CKF 滤波器参数（kfinit）。

2. 滤波主循环

   • INS 机械编排：逐帧处理 IMU 数据，更新姿态、速度、位置（insupdate）。
   • CKF 预测与更新：
     • 预测阶段：计算状态转移矩阵（kffk）。
     • 更新阶段：融合航向角、速度、位置观测数据（ckf 函数）。
   • 状态反馈：将滤波结果反馈到 INS，修正导航误差。

3. 结果可视化

   • 轨迹对比图：显示真实轨迹与滤波后轨迹的一致性。
   • 误差分析图：绘制速度、位置误差曲线及累计概率密度（CDF），量化滤波效果。
   • 数值输出：计算 X/Y/Z 轴速度误差的平均值，评估算法性能。

关键函数说明

函数名	功能描述
psinstypedef(153)	定义导航系统类型（153 状态模型）。
imuadderr	为 IMU 数据添加预设误差（零偏、噪声等）。
kfinit	初始化卡尔曼滤波器参数（状态、噪声协方差等）。
ckf	容积卡尔曼滤波核心算法，处理非线性观测更新。

运行结果

• 轨迹精度提升：滤波后轨迹与真实轨迹高度吻合。
• 误差显著降低：X/Y/Z 轴速度误差平均值较未滤波数据明显减小（具体数值见输出）。
• 统计特性优化：累计概率密度图（CDF）显示误差分布更集中，验证算法稳定性。

程序代码

部分代码如下：

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

% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear;clc;close all;
rng(0);
glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
% imuerr = imuerrset(0.03, 100, 0.001, 5);
imuerr = imuerrset(8, 14, 0.18, 57);
imu = imuadderr(trj.imu, imuerr);  % imuplot(imu);
davp0 = avperrset([1;1;10]*60, 0, 0*[1;1;3]);

%% 速度观测UKF
ins = insinit(avpadderr(trj.avp0,davp0), ts);

完整代码下载链接：https://download.youkuaiyun.com/download/callmeup/90354209
在安装了PSINS工具箱后，直接运行即可

总结

本代码通过扩展观测维度和改进 CKF 算法，实现了高精度的 SINS/GPS 组合导航，适用于无人机、自动驾驶等场景。实验结果表明，滤波后导航误差显著降低，验证了方法的有效性。代码结构清晰，涵盖数据加载、滤波处理、结果分析全流程，可作为惯性导航算法的参考实现。

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