【MATLAB代码介绍】使用EKF融合惯导和DVL（速度）的MATLAB仿真例程

基于 MATLAB 编写的扩展卡尔曼滤波（EKF）仿真程序。该代码旨在模拟和验证 EKF 算法在处理非线性系统时的状态估计性能，具体场景设定为惯性导航系统（INS）与多普勒测速仪（DVL）的数据融合。
如有程序答疑等需求，可通过文末的卡片联系作者

文章目录

代码概述
- 核心模块
运行结果
源代码

代码概述

该程序构建了一个三轴速度估计的仿真环境。它通过生成带有噪声的"真实"运动轨迹和观测数据，分别对比了"未滤波（仅惯性推算）"与"EKF 滤波"两种情况下的估计精度，直观展示了多传感器融合在抑制误差发散方面的优势。

核心模块

仿真环境与参数初始化
运动模型（状态方程）
观测模型（量测方程）
EKF 核心算法

这是代码的主体部分，实现了标准的扩展卡尔曼滤波流程：

状态预测（Time Update）：

利用非线性运动方程预测下一时刻的状态 X p r e X_{pre} Xpre。
X p r e = f ( X e k f , k − 1 ) + w k X_{pre} = f(X_{ekf, k-1}) + w_k Xpre=f(Xekf,k−1)+wk
雅可比矩阵计算（线性化）：

由于 X 轴运动方程是非线性的，代码计算了状态转移矩阵 F F F（即雅可比矩阵 ∂ f ∂ x \frac{\partial f}{\partial x} ∂x∂f）：
F 1 , 1 = 1 + 2.5 ( 1 − v x 2 ) ( 1 + v x 2 ) 2 F_{1,1} = 1 + \frac{2.5(1 - v_x^2)}{(1 + v_x^2)^2} F1,1=1+(1+vx2)22.5(1−vx2)

这是 EKF 处理非线性问题的关键步骤。
协方差预测：
P p r e = F P k − 1 F T + Q P_{pre} = F P_{k-1} F^T + Q Ppre=FPk−1FT+Q
卡尔曼增益计算：
K k = P p r e H T ( H P p r e H T + R ) − 1 K_k = P_{pre} H^T (H P_{pre} H^T + R)^{-1} Kk=PpreHT(HPpreHT+R)−1

其中观测矩阵 H H H 为单位矩阵。
状态更新（Measurement Update）：

利用观测值 Z Z Z 修正预测值：
X e k f , k = X p r e + K k ( Z k − Z ^ ) X_{ekf, k} = X_{pre} + K_k (Z_k - \hat{Z}) Xekf,k=Xpre+Kk(Zk−Z^)
P k = ( I − K k H ) P p r e P_k = (I - K_k H) P_{pre} Pk=(I−KkH)Ppre

结果可视化与评估

轨迹对比图： 绘制了"真实值"、"EKF 估计值"和"未滤波值"的对比曲线，直观展示 EKF 如何紧贴真实轨迹。
误差分析图： 绘制了未滤波误差与 EKF 滤波误差的对比，显示 EKF 显著降低了误差幅值。
定量输出： 在命令行窗口打印 X、Y、Z 三轴速度误差的最大绝对值，量化滤波效果。

运行结果

三轴速度对比：

估计误差对比：

源代码

部分代码：

matlab 复制代码

% EKF融合INS与DVL的核心程序
clear;clc;close all;
rng(0); %注释此行可以在每次运行时使用不同的随机数
%% 滤波模型初始化
t = 1:1:100;
Q = 1*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); %观测量------对三轴速度进行观测
%% 运动模型
X_=zeros(3,length(t));
X_(:,1)=X(:,1);
for i1 = 2:length(t)

完整代码：
https://blog.csdn.net/callmeup/article/details/136966605?spm=1011.2415.3001.5331