【MATLAB代码介绍】EKF+CKF对比程序

文章目录

程序介绍
运行结果
程序源码

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

程序介绍

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

设三维状态向量为：

x k = $x 1 , k x 2 , k x 3 , k$ \mathbf{x}k = \begin{bmatrix} x{1,k} \\ x_{2,k} \\ x_{3,k} \end{bmatrix} xk= x1,kx2,kx3,k

状态方程（运动模型）

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

x k = f ( x k − 1 ) + w k − 1 \mathbf{x}k = f(\mathbf{x}{k-1}) + \mathbf{w}_{k-1} xk=f(xk−1)+wk−1

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

f ( x k − 1 ) = $x 1 , k - 1 + 2.5 x 1 , k - 1 1 + x 1 , k - 1 2 + 8 cos ( 1.2 ( k - 1 ) ) x 2 , k - 1 + 1 x 3 , k - 1$ f(\mathbf{x}{k-1}) = \begin{bmatrix} x{1,k-1} + \dfrac{2.5\, x_{1,k-1}}{1 + x_{1,k-1}^2} + 8 \cos\big(1.2\,(k-1)\big) \\ x_{2,k-1} + 1 \\ x_{3,k-1} \end{bmatrix} f(xk−1)= x1,k−1+1+x1,k−122.5x1,k−1+8cos(1.2(k−1))x2,k−1+1x3,k−1
【强非线性系统】

过程噪声： w k ∼ N ( 0 , Q ) \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) wk∼N(0,Q)，协方差矩阵为：

Q = 1 ⋅ $1 0 0 0 1 0 0 0 1$ \mathbf{Q} = 1 \cdot \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} Q=1⋅ 100010001

观测方程（量测模型）

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

z k = h ( x k ) + v k \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k zk=h(xk)+vk

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

h ( x k ) = $x 1 , k 2 20 x 2 , k x 3 , k$ h(\mathbf{x}k) = \begin{bmatrix} \dfrac{x{1,k}^2}{20} \\ x_{2,k} \\ x_{3,k} \end{bmatrix} h(xk)= 20x1,k2x2,kx3,k

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

R = $1 0 0 0 1 0 0 0 1$ \mathbf{R} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} R= 100010001

运行结果

估计的三轴状态量对比：

三轴误差曲线对比：

命令行输出的数值对比：

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

程序源码

部分如下：

matlab 复制代码

% 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

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