【MATLAB例程】二维环境，INS与DVL的融合，使用EKF（扩展卡尔曼滤波），基于非线性测速系统，附MATLAB例程的下载链接-优快云博客

在这里插入图片描述

原创程序，请勿翻卖

文章目录

程序简介
运行结果
MATLAB源代码

程序简介

程序实现INS 与 DVL 速度信息融合的二维扩展卡尔曼滤波（EKF）示例。以二维速度作为状态量，通过构造非线性运动模型与非线性观测模型，对含噪 INS 速度进行滤波修正，并通过时间积分得到二维位置轨迹，从而对比未滤波与 EKF 滤波条件下的速度误差和轨迹漂移情况。

重点如下：

INS：提供连续但随时间漂移的速度信息（过程噪声主导）
DVL：提供带噪但相对稳定的速度观测（观测噪声主导）
EKF：在非线性条件下实现二者的最优融合

运行结果

轨迹对比：
在这里插入图片描述

速度曲线与速度误差曲线：
在这里插入图片描述

命令行输出的结果：

MATLAB源代码

部分代码如下：

% EKF融合INS与DVL的核心程序，二维
% 作者:matlabfilter（V同号，可接代码定制、讲解与调试）
% 2025-12-30/Ver1

clear;clc;close all;
rng(0); %注释此行可以在每次运行时使用不同的随机数
%% 滤波模型初始化
t = 1:1:100;
Q = 0.01*diag([1,1]);w=sqrt(Q)*randn(size(Q,1),length(t));
R = 1^2*diag([1,1]);v=sqrt(R)*randn(size(R,1),length(t));
P0 = 1*eye(2);
X=zeros(2,length(t)); %构建滤波状态量(二维速度)
X_ekf=zeros(2,length(t)); %构建滤波后的输出状态
X_ekf(1,1)=X(1,1);
Z=zeros(2,length(t)); %定义观测值形式
Z(:,1)=[X(1,1);X(2,1).^2]+v(:,1); %观测量——对二维速度进行观测
%% 运动模型
...