原创代码，效果已调好。包运行成功

程序介绍

经典扩展卡尔曼滤波（EKF）与自适应扩展卡尔曼滤波（AEKF）的 MATLAB 对比仿真程序。程序以三维非线性离散系统为对象，通过构造真实状态、含噪声状态和观测数据，对比分析 EKF 与基于观测噪声协方差矩阵 R 自适应调节的 AEKF 在非线性状态估计中的性能差异。

构建三维非线性系统模型

程序中的系统状态为三维变量。其中第一维状态采用非线性递推形式，包含分式非线性项和余弦激励项；第二维状态按照固定增量递增；第三维状态保持不变。其目的是构建一个既包含非线性特征，又便于观察滤波效果的典型仿真系统。

真实状态更新过程中不直接加入过程噪声，用于作为理想参考轨迹；未滤波状态则在状态递推过程中加入过程噪声，用于模拟实际系统中受到扰动后的状态变化。

观测模型同样包含非线性关系。第一维观测量由第一维状态的平方项构成，第二维和第三维观测量则直接对应状态本身。这样的设置使系统同时具有非线性状态转移和非线性观测特征，适合用于测试 EKF 的线性化估计能力。

观测噪声

代码中通过以下语句人为放大第 20 至第 50 个采样点之间的观测噪声：

v(:,20:50) = 4*v(:,20:50);

这一设置用于模拟传感器短时间精度下降、外部干扰增强或观测异常等情况。在实际工程中，观测噪声往往不是恒定不变的。如果滤波器仍然使用固定的 R 矩阵，就可能导致滤波增益不合理，进而影响估计精度。

因此，该仿真通过观测噪声突变场景，对比经典 EKF 与自适应 EKF 的鲁棒性差异。

自适应的核心算法

自适应 EKF 的核心区别在于：根据观测残差大小动态调整观测噪声协方差矩阵。

运行结果

程序运行后，会依次生成三类结果图，并在 MATLAB 命令行窗口输出三维状态误差的最大值。

第一类图为三维状态估计结果对比图。图中分别绘制真实状态、经典 EKF 估计结果、未滤波状态以及自适应 EKF 估计结果。通过该图可以直观看出，未滤波状态由于受到过程噪声影响，与真实状态之间存在明显偏差；EKF 和 AEKF 均能在一定程度上改善状态估计结果，使估计曲线更加接近真实状态。

第二类图为三维状态误差对比图。分别展示未滤波误差、EKF估计误差和 AEKF 估计误差。通过误差曲线可以观察不同滤波方法在各个时刻的误差变化情况。在观测噪声被人为放大的时间段内，经典EKF由于使用固定的观测噪声协方差矩阵，易受到异常观测影响；而AEKF能根据残差大小对观测噪声协方差进行调整，从而降低异常观测对估计结果的干扰。

第三类图为绝对误差累积分布函数曲线。该图从统计角度对比了滤波前、EKF 滤波后和 AEKF 滤波后的误差分布情况。若某一方法的累积分布曲线整体更加靠左，说明该方法的绝对误差整体更小，估计性能更好。CDF的曲线越靠近左上角表示整体误差越低。

命令行截图：

MATLAB源代码

部分代码如下：

% EKF经典程序 VS  自适应R的EKF，三维非线性系统
% 作者联系VX:matlabfilter（可接代码定制、讲解）
% 2024-12-14/Ver1
% 2026-06-06/Ver2
clear; clc; close all;% 清除变量、命令行和图形窗口
rng(0); % 设置随机数种子

%% 滤波模型初始化
t = 1:1:200; %设置时间序列
Q = 1 * diag([1, 1, 1]); % 过程噪声协方差矩阵
w = sqrt(Q) * randn(size(Q, 1), length(t)); % 过程噪声
R = 1 * 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_aekf = zeros(3, length(t)); % 扩展卡尔曼滤波估计的状态
Z = zeros(3, length(t)); % 观测值形式
Z(:, 1) = [X(1, 1)^2 / 20; X(2, 1); X(3, 1)] + v(:, 1); % 设置观测量初值

完整代码：https://download.csdn.net/download/callmeup/92950761

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我