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

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

代码概述

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

核心模块

1. 仿真环境与参数初始化

2. 运动模型（状态方程）

3. 观测模型（量测方程）

4. 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_kXpre​=f(Xekf,k−1​)+wk​

• 雅可比矩阵计算（线性化）：
  由于 X 轴运动方程是非线性的，代码计算了状态转移矩阵F FF（即雅可比矩阵∂ 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 + QPpre​=FPk−1​FT+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​=Ppre​HT(HPpre​HT+R)−1
  其中观测矩阵H HH为单位矩阵。

• 状态更新（Measurement Update）：
  利用观测值Z ZZ修正预测值：
  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−Kk​H)Ppre​

5. 结果可视化与评估

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

运行结果

三轴速度对比：

估计误差对比：

源代码

部分代码：

% 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);fori1=2:length(t)

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