【MATLAB例程】CKF（容积卡尔曼滤波）对二维非线性系统的滤波例程，非线性的状态与观测，附代码下载链接

状态方程和观测方程，都是二维、非线性的。有中文注释，可根据需要在代码上自行修改。

程序简介

程序功能概述
程序演示了：二维非线性状态的$CKF$（Cubature Kalman Filter，容积卡尔曼滤波）估计流程。通过构造非线性系统的真实轨迹、加入过程/观测噪声生成观测数据，利用 CKF 进行估计，并与未滤波结果进行对比，最后给出误差曲线、误差分布（$CDF$，累计密度函数）和数值统计（最大值、均值、标准差）。

模型与数据

• 状态为二维，含非线性驱动项与常速增长项；
• 观测为两个非线性量：其中一个维度是平方项、另一维是立方项；
• 过程噪声和观测噪声均为零均值高斯，协方差可由 Q、R 调节；
• 程序固定随机种子保证可复现。

绘图与输出

• 绘制二维轨迹对比图（真实/未滤波/CKF）；
• 绘制各维度的状态曲线与误差曲线；
• 绘制误差的 CDF 图，直观比较滤波效果；
• 在命令行输出各维度误差的最大值、均值与标准差示例。

参数与使用建议

• 调大 Q：滤波对模型不确定性更敏感，跟踪突变更快但噪声更大；
• 调大 R：更不信任量测，结果更平滑但可能滞后；
• 可将时间长度、非线性强度、噪声水平作为实验变量，观察 CKF 的稳健性与收敛行为。

适用与扩展

• 适合演示“非线性+高斯噪声”场景下的无导数滤波思想；
• 可平滑替换状态/观测函数以适配目标跟踪、导航等任务；
• 易扩展到更高维度、不同噪声水平和不同非线性强度的对比实验。

运行结果

两个轴分别为X和Y绘制的图形，包括滤波前后的对比图像：

各轴的状态曲线：

累计密度函数图像对比：

MATLAB源代码

部分代码如下：

% CKF的二维滤波程序例程
% 作者:matlabfilter
% 2025-08-28/Ver1
clear;clc;close all; %清空工作区、命令行，关闭小窗口
rng(0); %固定随机种子
%% 滤波模型初始化
t = 1:1:100;% 定义时间序列
Q = 0.1*diag([1,1]);% 设置过程噪声协方差矩阵
w = sqrt(Q)*randn(size(Q,1),length(t)); %生成
% 观测噪声协方差矩阵和观测噪声
R = 1*diag([1,1]);
v = sqrt(R)*randn(size(R,1),length(t));
% 初始状态估计协方差矩阵
P0 = 1*eye(2);
% 初始化状态向量
X = zeros(2,length(t)); %给状态真实值申请空间
Z=zeros(2,length(t)); %定义观测值形式（申请空间）
Z(:,1)=[X(1,1)^2/20;X(2,1)^3]+v(:,1); %观测量
%% 运动模型
X_=zeros(2,length(t)); %给未滤波的状态申请空间
X_(:,1)=X(:,1); %给未滤波的状态赋初值

完整代码的下载链接：
https://download.csdn.net/download/callmeup/91802029

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