本文所述的代码实现了改进的扩展卡尔曼滤波算法（MVC-EKF），针对三维运动估计场景，与经典EKF算法进行性能对比。代码通过引入Versoria函数优化协方差更新过程，显著提升了在测量异常值干扰下的状态估计鲁棒性。

代码概述

本代码实现了一种改进的扩展卡尔曼滤波算法（MVC-EKF），针对三维运动估计场景，与经典EKF算法进行性能对比。代码通过引入Versoria函数优化协方差更新过程，显著提升了在测量异常值干扰下的状态估计鲁棒性。主要特性包括：

• 应用场景：目标跟踪、动态系统状态估计、自动驾驶定位等需抑制测量噪声的领域。
• 核心创新：在EKF框架中嵌入Versoria权重函数，动态调节卡尔曼增益，降低异常值对估计的影响。
• 实验设计：在10-30时间步注入高强度异常值，验证算法抗干扰能力。
• 性能评估：提供多维状态误差分析、统计指标（RMSE/标准差/最大值）对比及可视化。

算法原理

1. 经典EKF流程

   • 预测步：通过非线性状态转移函数 $x_{pred}=f(x_{k-1})$计算先验状态，线性化后更新协方差$P_{pred}$。
   • 更新步：根据测量残差计算卡尔曼增益$K$，修正先验状态得到后验估计$x_{est}$。

2. MVC-EKF改进

   • Versoria权重函数：定义$MVC(y,y_{pred},R)=\exp (-0.5(y-y_{pred}{)}^2/R)$，根据测量残差动态生成权重$w_{mvc}$。
   • 抗差机制：在状态更新时引入权重$x_{est}=x_{pred}+w_{mvc}\cdot K\cdot (y-y_{pred})$，当残差过大时自动降低异常测量值的修正权重。

公式对比与优化机制

步骤	传统EKF	MVC-EKF	优化原理
状态更新	直接修正所有残差	残差加权修正	抑制异常值影响
协方差更新	固定增益调节	动态权重调节协方差矩阵	提升滤波器鲁棒性
噪声处理	固定( R )	隐含噪声统计自适应（通过权重衰减）	近似实现噪声协方差自适应

运行结果

滤波后的状态曲线和真值曲线对比：

状态误差曲线对比：

程序结构：

MATLAB源代码

部分源代码如下：

% 基于MVC的EKF，含有与EKF的对比三维平面的运动估计
% 核心：目标跟踪或状态估计，通过Versoria函数优化协方差更新
% 2025-06-24/Ver1clear; clc; close all;
rng(0);
%% 系统模型定义
% 定义状态空间模型
% x(k+1) = f(x(k)) + w(k)
% y(k) = h(x(k)) + v(k)% 非线性状态转移函数
f = @(x) [x(1) + 1;  x(2) + 2; x(3)+1];
% 非线性观测函数(距离与角度)
h = @(x) [x(1)^0.5; x(2)^0.5; x(3)+x(1)];% f 和 h 的雅可比矩阵
F = @(x) [1, 0, 0;0, 1, 0;0, 0, 1]; % f 的雅可比矩阵
% H = @(x) [2 * x(1), 1];     % h 的雅可比矩阵
H = @(x) [0.5*x(1)^(-0.5),0,0;0,0.5*x(2)^(-0.5),0;0,0,0.5*x(3)^(-0.5)];     % h 的雅可比矩阵% 噪声协方差矩阵
Q = 0.01 * eye(3); % 过程噪声协方差
R = diag([1,1,1]);           % 测量噪声协方差%% 仿真参数
n = 3;      % 状态维度
N = 100;     % 时间步数
x_true = zeros(n, N); % 真实状态
x_est_ekf = zeros(n, N);  % MCC 估计状态
x_est_mvc = zeros(n, N);  % MVC 估计状态
y_meas = zeros(3, N);     % 测量值% 初始状态
x_true(:, 1) = [10; 1;1];
x_est_ekf(:, 1) = [10; 1;1];
x_est_mvc(:, 1) = [10; 1;1];% 随机生成有噪声的测量值
for k = 1:Ny_meas(:,k) = h(x_true(:, k)) + diag(sqrt(R) ).* randn;if 10<k && k<30y_meas(:,k) = h(x_true(:, k)) + diag(sqrt(R) ) * randn + 10; %特定时刻的异常值endif k < Nx_true(:, k+1) = f(x_true(:, k)) + sqrt(Q) * randn(n, 1);end
end

完整代码：

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