基于 PSINS工具箱【需提前安装，工具箱开源】的运动建模与滤波，实现 IMU 与雷达观测融合的扩展卡尔曼滤波（EKF）示例。利用PSINS的轨迹生成模块构造三维机动轨迹，并按设定的加速度、转弯、爬升/下降段生成真实 AVP（姿态－速度－位置）序列。随后给 IMU 数据加入安装误差、随机漂移和噪声，形成带误差的惯导数据，并通过 insupdate 完成惯导解算。
人工撰写，拒绝AI，包运行成功
文章目录
程序简介
本程序利用 P S I N S PSINS PSINS工具箱构建一个三维机动轨迹，并模拟 I M U IMU IMU与雷达观测数据，用于演示扩展卡尔曼滤波（EKF）在惯导与外部传感器融合中的具体实现流程。
程序首先通过多段轨迹段（匀速、加速、转弯、爬升、下降等）生成一条空间运动轨迹，并得到对应的姿态、速度、位置及理想 IMU 数据。随后为 IMU 引入噪声、随机漂移和安装误差，得到更贴近真实环境的带误差惯导信息，并使用惯导解算模块进行连续更新。
雷达传感器被放置在固定位置，能量测目标的距离、俯仰角和方位角。程序构造带噪声的雷达观测，并将其转换成三维位置，以此作为 E K F EKF EKF的外部观测量。滤波器根据惯导预测结果与雷达观测之间的差异，估计位置误差、速度误差以及陀螺仪和加速度计的偏置，并通过反馈机制实时修正惯导解算，使结果逐步逼近真实轨迹。
代码最后绘制真实轨迹、雷达反算轨迹、滤波后轨迹，并提供三轴状态对比、误差对比以及统计信息，包含最大误差、平均误差和标准差等。整体流程清晰地展示了 IMU 与雷达融合在三维定位中的效果，也是使用 PSINS 进行多传感器融合的一个完整示例。
运行结果
运动轨迹、雷达位置绘图：
滤波误差绘图：
三轴误差分开显示：
命令行输出的结果：
MATLAB源代码
程序结构：
部分代码如下：
matlab
% 基于PSINS工具箱的IMU数据和雷达观测对目标滤波，EKF
% 作者：matlabfilter
% 2025-11-25/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]]; % 初始化avp
traj_ = [];
%% 轨迹设置
seg = trjsegment(traj_, 'init', 0);
seg = trjsegment(seg, 'uniform', 100);
%% = 滤波结果误差统计 ==
fprintf('========== 滤波结果误差统计 ==========\n');
% X轴（纬度）
fprintf('滤波X轴（纬度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_filter_x)));
fprintf(' 最小值：%.4f\n', min(abs(error_filter_x)));
fprintf(' 平均值：%.4f\n', mean(abs(error_filter_x)));
fprintf(' 标准差：%.4f\n', std(error_filter_x));
% Y轴（经度）
fprintf('滤波Y轴（经度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_filter_y)));
fprintf(' 最小值：%.4f\n', min(abs(error_filter_y)));
fprintf(' 平均值：%.4f\n', mean(abs(error_filter_y)));
fprintf(' 标准差：%.4f\n', std(error_filter_y));
% Z轴（高度）
fprintf('滤波Z轴（高度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_filter_z)));
fprintf(' 最小值：%.4f\n', min(abs(error_filter_z)));
fprintf(' 平均值：%.4f\n', mean(abs(error_filter_z)));
fprintf(' 标准差：%.4f\n', std(error_filter_z));
% ========== 雷达观测误差统计 ==========
fprintf('\n========== 雷达观测误差统计 ==========\n');
% X轴（纬度）
fprintf('雷达X轴（纬度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_radar_x)));
fprintf(' 最小值：%.4f\n', min(abs(error_radar_x)));
fprintf(' 平均值：%.4f\n', mean(abs(error_radar_x)));
fprintf(' 标准差：%.4f\n', std(error_radar_x));
% Y轴（经度）
fprintf('雷达Y轴（经度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_radar_y)));
fprintf(' 最小值：%.4f\n', min(abs(error_radar_y)));
fprintf(' 平均值：%.4f\n', mean(abs(error_radar_y)));
fprintf(' 标准差：%.4f\n', std(error_radar_y));
% Z轴（高度）
fprintf('雷达Z轴（高度）误差：\n');
fprintf(' 最大值：%.4f\n', max(abs(error_radar_z)));
fprintf(' 最小值：%.4f\n', min(abs(error_radar_z)));
fprintf(' 平均值：%.4f\n', mean(abs(error_radar_z)));
fprintf(' 标准差：%.4f\n', std(error_radar_z));
% ========== 整体误差统计（可选） ==========
error_filter_all = sqrt(error_filter_x.^2 + error_filter_y.^2 + error_filter_z.^2); % 滤波三维位置误差
error_radar_all = sqrt(error_radar_x.^2 + error_radar_y.^2 + error_radar_z.^2); % 雷达三维位置误差
fprintf('\n========== 三维位置误差统计 ==========\n');
fprintf('雷达（滤波前）三维位置误差：\n');
fprintf(' 最大值：%.4f\n', max(error_radar_all));
fprintf(' 最小值：%.4f\n', min(error_radar_all));
fprintf(' 平均值：%.4f\n', mean(error_radar_all));
fprintf(' 标准差：%.4f\n', std(error_radar_all));
fprintf('滤波后，三维位置误差：\n');
fprintf(' 最大值：%.4f\n', max(error_filter_all));
fprintf(' 最小值：%.4f\n', min(error_filter_all));
fprintf(' 平均值：%.4f\n', mean(error_filter_all));
fprintf(' 标准差：%.4f\n', std(error_filter_all));完整代码可直接运行，下载链接：
https://download.csdn.net/download/callmeup/92413556
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者