【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轴上的最大误差,以量化不同滤波器的性能。

运行结果

轨迹曲线:

误差曲线:

代码

部分代码:

matlab 复制代码
% 基于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;

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

相关推荐
forest_097 小时前
《AI 编程 Harness 实战手册》04|审批这道门:点了一百次“允许”,还剩多少作用?
人工智能·算法·prompt·agi
H.莓飛8 小时前
【C++】【STL】stack_OJ题
服务器·c语言·开发语言·数据结构·c++·算法·visual studio
不会武功不懂江湖10 小时前
RISC-V链接器与链接脚本
汇编·算法·risc-v
Evand J10 小时前
【MATLAB例程】基于IMM-EKF与AOA/测距融合的二维移动目标跟踪定位
matlab·代码·定位·1024程序员节·toa·aoa
Zwawa11 小时前
卡尔曼到底在算什么:从一维到它的变式以及工程版卡尔曼滤波算法C语言实现
c语言·算法·嵌入式
电科_银尘11 小时前
【大模型算法与工程 14】Tokenizer与Embedding:文本如何变成模型输入
算法·embedding
饮水桶_T12 小时前
Python 面向对象(十七)|继承、封装、多态 笔记
数据结构·笔记·算法
地平线开发者15 小时前
让 AI Agent 和 征程6 台架高效、稳定、安全地协作
算法·安全·自动驾驶
kyle~16 小时前
SHA-256 哈希去重
算法·哈希算法