【基于PSINS工具箱】以速度为观测量的SINS/GNSS组合导航，CKF滤波

基于【PSINS工具箱】，提供一个MATLAB例程，仅以速度为观测量的SINS/GNSS组合导航（滤波方式为CKF），无需下载，订阅专栏后可直接复制

文章目录

• 工具箱
• 程序简述
• • 运行结果
• 代码
• 程序讲解
• • 代码功能概述
  • 代码结构与关键步骤
  • 结论

工具箱

本程序需要在安装工具箱后使用，工具箱是开源的，链接：http://www.psins.org.cn/kydm

程序简述

原有例程的 153 153 153组合导航是 S I N S SINS SINS/ G P S GPS GPS下的位置观测或位置+速度观测，本文所述的代码是仅三轴位置观测的，使用CKF来滤波。
最后输出速度对比、速度误差、姿态对比、姿态误差、位置对比、位置误差等图片。如下：

运行结果

• 三轴AVP曲线：
  

• 三轴速度误差曲线：
  

• 滤波后 X X X轴速度累积概率分布函数：
  
  误差的特技特性输出：
  

代码

部分代码如下：

% 【PSINS】153 CKF 观测为GNSS的速度
% V：matlabfilter
% 2024-11-05/Ver1

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

rng(0); % 设置随机数种子为0，以确保结果可重复
glvs % 调用全局变量设置
psinstypedef('test_SINS_GPS_UKF_153_def'); % 设置PSINS类型

% 读取轨迹文件
trj = trjfile('trj10ms.mat');  % 读取轨迹数据（可视化注释已注释掉）

% 初始设置
[nn, ts, nts] = nnts(2, trj.ts); % 获取时间序列的参数
imuerr = imuerrset(0.03, 100, 0.001, 5); % 设置IMU误差参数
imu = imuadderr(trj.imu, imuerr);  % 对IMU数据添加误差
% imuplot(imu); % （注释掉的）绘制IMU数据

% 设置初始姿态误差
davp0 = avperrset([1; 1; 10]*60, 0.1, [1; 1; 3]); % 设置初始姿态误差
ins = insinit(avpadderr(trj.avp0, davp0), ts); % 初始化惯性导航系统

% CKF滤波器初始化
rk = [0.1, 0.1, 0.1]'; % 观测噪声权重
kf = kfinit(ins, davp0, imuerr, rk); % 初始化卡尔曼滤波器

len = length(imu); % 获取IMU数据长度
% 预分配内存以提高效率
[avp_CKF_vel, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS CKF Simulation.'); % 显示进度条

ki = 1; % 初始化计数器
for k = 1:nn:len-nn+1
    k1 = k + nn - 1;  % 计算当前时间窗口的结束索引
    wvm = imu(k:k1, 1:6);  % 获取当前时间窗口的IMU数据

完整代码下载链接：https://gf.bilibili.com/item/detail/1106602012

程序讲解

这段 MATLAB 代码实现了基于 CKF（Cubic Kalman Filter，立方卡尔曼滤波）的速度观测处理，主要用于模拟和分析惯性导航系统（INS）与全球导航卫星系统（GNSS）结合的情况。以下是对代码的详细介绍：

代码功能概述

1. 背景与应用：

   • 本代码旨在通过 CKF 方法优化速度观测，使用 GNSS 数据来提高惯性导航系统的定位精度。CKF 是一种改进的卡尔曼滤波器，适用于处理非线性系统的状态估计问题。

2. 输入数据：

   • 代码从名为 trj10ms.mat 的文件中读取轨迹数据，该数据包含了 IMU（惯性测量单元）和 GNSS 的观测信息。
   • 初始的 IMU 数据通过 imuadderr 函数添加了噪声，模拟现实环境中传感器的误差。

代码结构与关键步骤

1. 环境初始化：

   • 使用 clear; clc; close all; 清除工作空间、命令窗口和关闭所有图形窗口，以确保代码运行时环境的干净。
   • 使用 rng(0); 设置随机数生成器的种子，以便于结果的可重复性。

2. 参数设置：

   • 通过 nnts 函数提取时间序列参数，确定数据处理的步长和时间间隔。
   • 使用 imuerrset 函数初始化 IMU 的误差参数，定义加速度计和陀螺仪的误差特性。
   • davp0 设置了初始的姿态误差，这对后续的 INS 初始化至关重要。

3. INS 初始化：

   • 通过 insinit 函数结合姿态误差和时间序列，初始化惯性导航系统的状态。

4. CKF滤波器设置：

   • 初始化 CKF 的相关参数，包括观测噪声权重和状态协方差矩阵。
   • 使用 prealloc 函数为结果数据预分配内存，优化性能。

5. 主循环：

   • 使用一个循环遍历所有的 IMU 数据，进行状态更新和滤波处理：
     • 从 IMU 数据中提取当前时间窗口的观测值，调用 insupdate 更新 INS 状态。
     • 计算当前状态的 CKF 更新，并将更新后的状态与 GNSS 数据进行融合。
     • 每当时间达到整数秒时，生成带噪声的 GNSS 速度观测，并使用 CKF 更新滤波器状态。
     • 保存每次迭代的估计结果和状态协方差，用于后续分析。

6. 结果处理与绘图：

   • 使用 avpcmpplot 函数比较真实的 GNSS 姿态与 CKF 估计的速度。
   • 绘制速度误差，展示在 X、Y、Z 轴的速度估计与真实值之间的差异。
   • 最后，绘制速度误差的累积概率分布函数（CDF），直观展示滤波结果的统计特性。

结论

这段代码通过 CKF 方法有效地处理了 IMU 和 GNSS 的融合问题，展示了如何在动态环境中利用不同传感器的数据提高系统的定位精度。其结构清晰，便于理解和扩展，适合在实际应用中进行进一步的研究和优化。