【PSINS】EKF、UKF、CKF三个滤波下的组合导航（松组合）对比

该 MATLAB 代码实现了扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和无迹卡尔曼滤波的变体（CKF）的对比，主要用于导航与定位领域，通过处理惯性测量单元（IMU）和GPS数据来估计运动轨迹。

说明：基于PSINS工具箱的EKF、UKF、CKF对比

该 MATLAB 代码实现了扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和无迹卡尔曼滤波的变体（CKF）的对比，主要用于导航与定位领域，通过处理惯性测量单元（IMU）和GPS数据来估计运动轨迹。

主要功能

1. 参数初始化：

   • 清理工作空间并设置随机数生成器。
   • 定义采样时间间隔和初始的速度、姿态和位置（AVP）信息。

2. 轨迹设置：

   • 通过一系列轨迹段（如匀速、加速、转弯和爬升）来模拟运动轨迹。
   • 使用trjsimu函数生成包含IMU数据的轨迹。

3. 初始化滤波器：

   • 设置IMU误差模型和初始状态误差。
   • 初始化INS（惯性导航系统）和卡尔曼滤波器（KF）。

4. 扩展卡尔曼滤波（EKF）：

   • 在每个时间步中更新INS状态并进行KF更新。
   • 通过模拟GPS位置获取带噪声的观测值，进行观测更新。
   • 记录滤波结果并进行结果展示。

5. 无迹卡尔曼滤波（UKF）：

   • 类似于EKF的过程，但使用UKF算法进行状态更新。
   • 处理IMU数据并用GPS数据进行观测更新，记录滤波结果。

6. 无迹卡尔曼滤波变体（CKF）：

   • 重复UKF的流程，但使用CKF算法进行状态估计。
   • 记录估计的状态和协方差。

7. 结果可视化：

   • 绘制真实轨迹、EKF、UKF和CKF的估计轨迹。
   • 误差比较：绘制不同滤波器在X、Y、Z轴上的位置误差及其累积分布函数（CDF）。

8. 性能评估：

   • 输出各个滤波器在X、Y、Z轴上的最大误差，以量化不同滤波器的性能。

运行结果

轨迹曲线：

误差曲线：

代码

部分代码：

% 基于PSINS工具箱的EKF、UKF、CKF对比
% 2024-7-14/Ver1
clear;clc;close all;
rng(0);
glvs
psinstypedef(153);
ts = 0.1;       % sampling interval
%% 轨迹设置
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % init avp
% trajectory segment setting
traj_ = [];
seg = trjsegment(traj_, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, traj_, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'climb',        10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'descent',      10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  traj_, 2); %2
seg = trjsegment(seg, 'uniform',      100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);
% trjfile('trj10ms.mat', trj);
%% 初始化
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);
% KF filter
rk = poserrset([1;1;3]);
kf = kfinit(ins, davp0, imuerr, rk);
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp_ekf, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, 'KF'); 
ki = 1;

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