CDKF（中心差分卡尔曼滤波）的MATLAB代码（附下载链接）

另有关于EKF和CDKF的对比程序：EKF+CDKF两个滤波的MATLAB程序，估计三轴位置，带中文注释——https://blog.youkuaiyun.com/callmeup/article/details/136610153。此文章所在的专栏还有更多好用的例程，欢迎订阅。

CDKF介绍

中心差分卡尔曼滤波（Central Difference Kalman Filter，CDKF）是卡尔曼滤波器的一种变体，通过将非线性系统模型线性化来处理非线性问题。
CDKF的工作原理如下：首先，使用非线性系统模型和当前的状态估计来预测系统的状态。然后，通过将状态预测输入到非线性观测模型中，得到观测的预测值。接下来，使用观测的预测值和实际观测值之间的差异来计算卡尔曼增益。最后，使用卡尔曼增益来更新状态估计，并得到最终的估计结果。

CDKF相对于传统的卡尔曼滤波器具有更好的非线性逼近能力，因为它使用了更准确的状态变化估计。然而，由于需要线性化非线性系统模型，CDKF可能在极端情况下失效，例如系统存在较大的非线性或噪声。

运行结果

滤波值与真值的对比：

滤波前后误差的对比，计算公式：
$$误差=滤波值/未滤波值-真值$$

误差的累计概率密度（CDF）图像：

代码下载链接

如需代码，可点击下列链接下载（需付费，请谨慎下载）：
https://download.youkuaiyun.com/download/callmeup/89708882

部分代码如下

% CDKF三维滤波
% 2024-6-28/Ver1
clear;clc;close all;
rng(0);
%% initial
T = 0.1; %采样率
t = T:T:100;
Q = 0.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));
P = 0.1*diag([1,1,1]);
P_num = zeros(length(t),size(P,1),size(P,2)); %存放每次迭代的P
P_num(1,:,:) = P;
X=zeros(3,length(t));
X(:,1) = [1,1,1]'; %初值设置，这里为全1，可以删除，删除则表示初值为全零
X_cdkf=zeros(3,length(t));
X_cdkf(:,1)=X(:,1);
Z=zeros(3,length(t)); %定义观测值形式
%% model
X_=zeros(3,length(t));
X_(:,1)=X(:,1);