【MATLAB例程】三维环境下，EKF融合INS与DVL的核心程序，用于惯导和速度传感器的数据融合滤波。附下载链接

基于扩展卡尔曼滤波（EKF）的 INS 与 DVL 融合仿真系统
原创代码，非AI生成，包运行成功，请勿翻卖

程序简介

本程序基于扩展卡尔曼滤波（EKF）的 INS/DVL 速度融合仿真框架，用于三维空间中载体速度估计与轨迹重构。程序以三轴速度作为系统状态，通过构造非线性速度演化模型与线性速度观测模型，模拟惯性导航系统（$INS$）速度积分漂移与多普勒测速仪（$DVL$）速度测量噪声并存的典型导航场景，并利用$EKF$对速度状态进行实时校正。

在此基础上，通过对速度进行时间积分，进一步给出三维位移轨迹，对比分析真实轨迹、未滤波轨迹与 EKF 融合轨迹的误差特性，从而直观验证 EKF 在$INS/DVL$融合中的误差抑制能力。

程序特点与适用场景

• 结构清晰，完整覆盖 INS/DVL 融合的核心计算流程

• 直接体现 INS 漂移累积与 DVL 约束修正的物理意义

• 支持从速度层面到轨迹层面的多维度性能分析

• 可作为：

  • INS/DVL 融合算法教学示例
  • 水下导航、无人平台速度融合原型验证
  • 后续扩展至姿态耦合、自适应滤波或鲁棒滤波研究的基础框架

运行结果

三维轨迹对比，能看到EKF的轨迹更接近真值：

速度曲线对比：

速度误差曲线对比：

命令行输出的结果：

MATLAB源代码

部分代码如下：

% EKF融合INS与DVL的核心程序，三维
% 作者:matlabfilter（V同号，可接代码定制、讲解与调试）
% date: 2024-03-23
% Ver1
% 2025-12-31/Ver2：优化输出、添加轨迹
clear;clc;close all;
rng(0); %注释此行可以在每次运行时使用不同的随机数
%% 滤波模型初始化
t = 1:1:100;
Q = 0.4*diag([1,1,1]);w=sqrt(Q)*randn(size(Q,1),length(t));
R = 0.1^2*diag([1,1,1]);v=sqrt(R)*randn(size(R,1),length(t));
P0 = 1*eye(3);
X=zeros(3,length(t)); %构建滤波状态量（三轴速度）
X_ekf=zeros(3,length(t)); %构建滤波后的输出状态
X_ekf(1,1)=X(1,1);
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1);X(2,1);X(3,1)]+v(:,1); %观测量——对三轴速度进行观测

完整代码的下载链接：
https://download.youkuaiyun.com/download/callmeup/92520159

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