本文所述的代碼實現了改進的擴展卡爾曼濾波算法（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}?K?(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

完整代碼：

如需幫助，或有導航、定位濾波相關的代碼定制需求，請點擊下方卡片聯系作者