【PSINS工具箱】MATLAB例程，平面上的组合导航，观测量为位置、速度、航向角，共5维。状态量为经典的15维

PSINS工具箱里面无论153还是156、159，都是三维空间上的滤波，这里给出平面上的滤波配置，使用EKF，观测为位置、速度、航向角（共2+2+1=5维）

程序功能

本文介绍的代码为二维平面下的位置、速度和航向角观测融合的 EKF 滤波仿真。功能如下：

• 构建一个具有加速、匀速、转弯等运动特性的二维运动轨迹；
• 给$IMU$数据（陀螺和加速度）添加噪声与系统误差；
• 使用$EKF$滤波器融合 航向角 + 速度 + 位置观测信息；
• 对比滤波前后结果，评估滤波对速度和位置估计精度的改善效果；
• 输出轨迹对比图、误差曲线、误差统计指标、热力图和性能评估表。

主要步骤

1. 初始化与轨迹生成

   • 设置采样周期 ts=0.1s；
   • 构造运动轨迹（匀速、加速、减速、转弯等）；
   • 生成理想的惯性数据 trj。

2. IMU误差建模

   • 使用 imuerrset 设置陀螺零偏、随机游走、加速度误差等；
   • 在理想IMU数据上添加误差，得到含噪数据 imu。

3. 滤波器初始化

   • 设置初始姿态、速度和位置误差；
   • 构建 15维状态扩展卡尔曼滤波器（含姿态误差、速度误差、位置误差、陀螺零偏、加速度计零偏）；
   • 设置过程噪声矩阵 Q、观测噪声矩阵 R、初始协方差矩阵 P。

4. 滤波循环

   • 利用惯导更新 insupdate；
   • 每隔1秒引入观测（航向角、速度、位置）；
   • 调用 kfupdate 更新滤波器状态并反馈至惯导解算；
   • 存储滤波结果用于后续分析。

5. 结果分析与可视化

   • 绘制速度、位置误差曲线；
   • 绘制滤波后轨迹与真实轨迹对比图；
   • 绘制位置误差热力图；
   • 计算误差统计指标（MAE、RMSE、最大误差、标准差、95%分位数）；
   • 生成滤波性能评估表格和报告。

运行结果

轨迹示意图：

误差图像：

输出的误差特性：

MATLAB代码

部分代码如下：

% 【PSINS】二维平面上的位置、速度、航向角为观测的155，滤波方法为EKF
% 作者：matlabfilter
% 2025-08-20/Ver1
% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear; clc; close all;
rng(0); % 设置随机数种子为0，以确保结果可重复
glvs % 调用全局变量设置
psinstypedef(153); % 设置PSINS类型

ts = 0.1;       % sampling interval
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % 初始化avp
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, '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);


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

% 设置初始姿态误差
davp0 = avperrset(0.1*[1; 1; 1], 0, [1; 1; 3]);

%% 速度观测EKF
ins = insinit(avpadderr(trj.avp0, davp0), ts); % 初始化惯性导航系统

完整代码：
https://download.youkuaiyun.com/download/callmeup/91717545

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