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

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

@[toc]

程序简介

运行结果

真实截图

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

二维状态估计曲线：

二维平面轨迹对比图：

二维状态估计误差曲线：

总体误差统计柱状图：

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

MATLAB源代码

部分代码：

% 二维 EKF、UKF、CKF、PF 四种滤波方法对比
% 直接运行本文件即可生成中文状态曲线、中文轨迹曲线、中文误差曲线、中文误差统计柱状图和中文命令行输出。
% 作者：matlabfilter（V同号），可接导航和滤波的定制与讲解
% 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);

如需帮助，或有导航、定位滤波相关的代码定制需求，可联系我