三维滤波，非线性系统状态与非线性观测，使用EKF和CKF进行滤波，输出滤波值曲线与误差对比

程序介绍

系统模型（状态方程与观测方程）

设三维状态向量为：

$${\mathbf{x}}_k=\left[\begin{array}{c}{x}_{1,k}\\ x_{2,k}\\ x_{3,k}\end{array}\right]$$

状态方程（运动模型）

系统非线性状态转移方程为：

$${\mathbf{x}}_k=f({\mathbf{x}}_{k-1})+{\mathbf{w}}_{k-1}$$

其中非线性函数$f(\cdot )$定义为：

$$f({\mathbf{x}}_{k-1})=\left[\begin{array}{c}{x}_{1,k-1}+\frac{2.5x_{1,k-1}}{1+x_{1,k-1}^2}+8\cos (1.2(k-1))\\ x_{2,k-1}+1\\ x_{3,k-1}\end{array}\right]$$
【强非线性系统】
过程噪声：${\mathbf{w}}_k\sim \mathcal{N}(0,\mathbf{Q})$，协方差矩阵为：

$$\mathbf{Q}=1\cdot \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

观测方程（量测模型）

观测向量由状态非线性映射得到：

$${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k$$

其中观测函数 (h(\cdot)) 为：

$$h({\mathbf{x}}_k)=\left[\begin{array}{c}\frac{x_{1,k}^2}{20}\\ x_{2,k}\\ x_{3,k}\end{array}\right]$$

观测噪声 (\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}))，协方差矩阵与过程噪声相同：

$$\mathbf{R}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

运行结果

估计的三轴状态量对比：

三轴误差曲线对比：

命令行输出的数值对比：

误差CDF曲线（越靠近左上角表示总体误差越低）：

程序源码

部分如下：

% EKF+CKF效果对比
clear;clc;close all;
%% 滤波模型初始化
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));
X_ekf=zeros(3,length(t));
X_ekf(:,1)=X(:,1);
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)

完整代码与程序详解：

https://blog.csdn.net/callmeup/article/details/136147833?spm=1011.2415.3001.5331

代码讲解、定制，可联系下方名片