【MATLAB例程】三维非线性系统下的CKF（容积卡尔曼滤波）例程，给出滤波前后的对比，使用组合导航为背景。附MATLAB代码下载链接

代码简介

程序实现了容积卡尔曼滤波（CKF）算法在三维动态系统中的应用，专为处理复杂、非线性环境下的状态追踪问题设计。代码通过噪声抑制和趋势预测，解决了传统方法在非线性场景中的跟踪偏差问题，适用于自动驾驶导航、工业传感器融合等场景。

用于测试的系统具有强非线性特征，模拟导航中惯性导航和卫星导航的组合。

运行结果

状态值曲线（立体图）：

各维度时间-状态曲线图：

误差曲线：

CDF（累计密度函数，越靠近左上表示误差越低）图像：

matlab源代码

程序结构：

部分代码如下：

% CKF的三维滤波程序例程
% 作者:matlabfilter
% 2024-08-30/Ver1
% 2025-07-26/Ver2：添加三维图
clear;clc;close all; %清空工作区、命令行，关闭小窗口
rng(0); %固定随机种子
%% 滤波模型初始化
t = 1:1:1000;% 定义时间序列
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)); %给状态真实值申请空间
Z=zeros(3,length(t)); %定义观测值形式（申请空间）
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
%% 运动模型
X_=zeros(3,length(t)); %给未滤波的状态申请空间
X_(:,1)=X(:,1); %给未滤波的状态赋初值
for i1 = 2:length(t) %for构造循环，迭代计算状态真值、滤波值、观测值
    X(:,i1) = [X(1,i1-1) + (2.5 * X(1,i1-1) / (1 + X(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
        X(2,i1-1)+1;
        X(3,i1-1)]; %真实值的迭代
    X_(:,i1) = [X_(1,i1-1) + (2.5 * X_(1,i1-1) / (1 + X_(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
        X_(2,i1-1)+1;
        X_(3,i1-1)] + w(:,i1-1);%未滤波的值迭代
    Z(:,i1) = [X(1,i1).^2 / 20;X(2,i1);X(3,i1)] + v(i1); %迭代计算观测值，v是观测误差
end

%% CKF 滤波部分

完整代码的下载链接：
https://download.youkuaiyun.com/download/callmeup/91500173

更多滤波程序请查看相应的专栏：
卡尔曼讲解与各种典型进阶MATLAB编程（专栏目录，持续更新……）

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