【MATLAB例程】二维非线性状态估计，EKF、UKF、CKF、PF对比。非线性滤波|卡尔曼滤波。附完整代码的下载链接

最新推荐文章于 2026-07-24 15:50:48 发布

原创最新推荐文章于 2026-07-24 15:50:48 发布 · 248 阅读

5 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

收录于

卡尔曼专题免费专栏

在这里插入图片描述

代码原创，非AI生成，请勿翻卖

文章目录

程序简介
运行结果
- 真实截图
MATLAB源代码

程序简介

本程序实现了一个完整的二维非线性状态估计仿真对比实验，在同一观测模型下横向对比了四种经典滤波算法：

EKF（扩展卡尔曼滤波）：通过对非线性函数进行一阶泰勒展开实现线性化逼近；
UKF（无迹卡尔曼滤波）：利用无迹变换采样 Sigma 点，避免显式求导；
CKF（容积卡尔曼滤波）：基于球面-径向容积准则，在高维非线性场景下数值稳定性更优；
PF（粒子滤波）：基于蒙特卡洛重要性采样，适用于强非线性、非高斯系统。

仿真包含 300 步迭代，使用 400 个粒子，系统为二维状态空间模型。程序运行后将自动生成中文标注的状态估计曲线、轨迹对比图、误差曲线和误差统计柱状图，并在命令行窗口中输出未滤波、以及 EKF/UKF/CKF/PF 四种方法在 X 轴、Y 轴和总体误差上的**均方根误差（RMSE）、平均绝对误差（MAE）、最大绝对误差（MaxAE）及标准差（STD）**等全面对比指标。

代码结构清晰、注释完整，适用于导航定位与目标跟踪领域的研究与教学。

运行结果

真实截图

程序运行后将生成以下图像，命令行窗口也会输出误差统计指标。

二维状态估计曲线：
在这里插入图片描述

二维平面轨迹对比图：
在这里插入图片描述

二维状态估计误差曲线：

在这里插入图片描述

总体误差统计柱状图：
在这里插入图片描述

命令行会输出仿真步数、粒子数、观测模型说明，以及未滤波、EKF、UKF、CKF、PF 在X轴、Y轴和总体误差上的均方根误差、平均绝对误差、最大绝对误差和标准差。
在这里插入图片描述

MATLAB源代码

部分代码：

% 二维 EKF、UKF、CKF、PF 四种滤波方法对比
% 直接运行本文件即可生成中文状态曲线、中文轨迹曲线、中文误差曲线、中文误差统计柱状图和中文命令行输出。
% 作者：matlabfilter（微信），可接导航和滤波的定制与讲解
% 2024-07-13/Ver1
clear; clc; close all;
rng(0);

%% 参数设置
scene_name = '二维非线性状态估计';
main_file = 'EKFUKFCKFPF_2D.m';
script_dir = fileparts(mfilename('fullpath'));
if isempty(script_dir)
    script_dir = pwd;
end

state_dim = 2;                  % 状态维度
step_count = 300;               % 仿真步数
t = 1:step_count;               % 时间序列
Q = 0.40^2 * eye(state_dim);    % 过程噪声协方差
R = 1.00^2 * eye(state_dim);    % 测量噪声协方差
particle_count = 400;           % 粒子滤波粒子数量

x0 = [0; 1];                    % 初始真实状态
x0_est = x0 + [0.8; -0.4];      % 初始估计状态
P0 = 2 * eye(state_dim);        % 初始估计协方差

%% 生成真实状态、未滤波状态和测量值
X_true = zeros(state_dim, step_count);
X_raw = zeros(state_dim, step_count);
Z = zeros(state_dim, step_count);
chol_Q = chol(Q, 'lower');
chol_R = chol(R, 'lower');

X_true(:, 1) = x0;
X_raw(:, 1) = x0;
Z(:, 1) = measurementFunction(X_true(:, 1)) + chol_R * randn(state_dim, 1);