当前位置：首页 > web >正文

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

web 2025/8/24 10:42:59

在这里插入图片描述

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

文章目录

程序功能
- 主要步骤
运行结果
MATLAB代码

程序功能

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

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

主要步骤

初始化与轨迹生成
- 设置采样周期 ts=0.1s；
- 构造运动轨迹（匀速、加速、减速、转弯等）；
- 生成理想的惯性数据 trj。
IMU误差建模
- 使用 imuerrset 设置陀螺零偏、随机游走、加速度误差等；
- 在理想IMU数据上添加误差，得到含噪数据 imu。
滤波器初始化
- 设置初始姿态、速度和位置误差；
- 构建 15维状态扩展卡尔曼滤波器（含姿态误差、速度误差、位置误差、陀螺零偏、加速度计零偏）；
- 设置过程噪声矩阵 Q、观测噪声矩阵 R、初始协方差矩阵 P。
滤波循环
- 利用惯导更新 insupdate；
- 每隔1秒引入观测（航向角、速度、位置）；
- 调用 kfupdate 更新滤波器状态并反馈至惯导解算；
- 存储滤波结果用于后续分析。
结果分析与可视化
- 绘制速度、位置误差曲线；
- 绘制滤波后轨迹与真实轨迹对比图；
- 绘制位置误差热力图；
- 计算误差统计指标（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); % 初始化惯性导航系统