文章目录
三维滤波，非线性系统状态与非线性观测，使用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
代码讲解、定制，可联系下方名片