容积KF(CKF)

0 前言

为了克服UKF在高维情况下出现滤波精度低的问题，Arasaratnam 和Haykin基于Caubature求积分变换，提出了容积卡尔曼滤波CKF方法。后来众多学者又基于CKF，提出了很多改进版本，如平方根CKF。

对于高斯分布下的非线性滤波问题，实际上就求后验期望的积分。由于被积分函数表现为非线性后验分布与高斯概率密度的乘积，因此一般很难得到解析解。这也是线性系统下该积分可以得到解析解，即著名的卡尔曼滤波算法。
$$E[x|z]={\int }_{R}f(x)\exp (-x^{\text{T}}x)dx$$
因此针对该非线性函数的积分问题，产生了众多基于数值积分的滤波算法。如UKF通过确定性采样来传播分布的一二阶矩（均值和方差）。而CKF作为看另一种求积分近似方法，利用球面径向规则。

CKF和UKF比较：

当取$\kappa =0$时， CKF 和 UKF 的估计精度相同，但鉴于 CKF 采样点少，实时性比 UKF 好，故应选用 CKF 滤波算法；

当$n\le 2$时,即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波算法；

当$n=2$时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更好，应选用 CKF 滤波算法；

当$n\ge 3$时,即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。

1 问题描述（离散时间非线性系统描述)

考虑带加性噪声的一般非线性系统模型，
$$\begin{gathered} x_k=f(x_{k-1})+w_{k-1} \\ {z}_k=h(x_k)+v_k \end{gathered}$$
其中$x_k$为$k$时刻的目标状态向量。$z_k$为$k$时刻观测向量（传感器数据）。这里不考虑控制器$u_k$。$w_k$和$v_k$分别是过程噪声序列和量测噪声序列，并假设$w_k$和$v_k$为零均值高斯白噪声，其方差分别为$Q_k$和$R_k$的高斯白噪声，即$w_k\sim N(0,Q_k)$,$v_k\sim N(0,R_k)$，且满足如下关系(线性高斯假设)为：
$$\begin{array}{rl}E[w_i{v}_j^{\text{T}}]& =0\\ E[w_i{w}_j^{\text{T}}]& =0i\ne j\\ E[v_i{v}_j^{\text{T}}]& =0i\ne j\end{array}$$

2 带加性噪声的容积卡尔曼滤波算法CKF

（1） 初始化

步骤一：给定$k-1$时刻的状态估计和协方差矩阵
$${\hat{x}}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}$$
当为$k=0$时刻时，假设$x_0\sim N({\bar{x}}_0,P_0),Q_0,R_0$,则滤波器最优初始化为
$$\begin{gathered} {\hat{x}}_{0|0}=E(x_0)={\bar{x}}_0 \\ {P}_{0|0}=E(x_0-{\hat{x}}_{0|0})(x_0-{\hat{x}}_{0|0}{)}^{\text{T}}=P_0 \end{gathered}$$

（2） 状态预测

步骤二：根据$k-1$时刻的估计 ${\hat{x}}_{k-1|k-1}$和方差$P_{k-1|k-1}$,产生容积点
$$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-|k-1}^{\text{T}}$$

$${\xi }_i=\sqrt{\frac{m}{2}}[1{]}_i,i=1,2,\cdots ,m=2n$$

$[1]$表示$n$（$n$维状态） 维空间的点集，即$[1]=[I_{n\times n}-I_{n\times n}]$.
$$\begin{array}{rl}& S_{k-1|k-1}=\sqrt{P_{k-1|k-1}}\\ & {\mathcal{X}}_{k-1|k-1}^i={\hat{x}}_{k-1|k-1}+S_{k-1|k-1}{\xi }_i\end{array}$$
步骤三： 根据非线性模型进行容积点的非线性传播
$${\mathcal{X}}_{k|k-1}^i=f({\mathcal{X}}_{k-1|k-1}^i)，i=1,2,\cdots ,m$$
步骤四： 状态一步预测和预测方差
$$\begin{array}{rl}& {\hat{x}}_{k|k-1}=\frac{1}{m}\sum _{i=1}^m{\mathcal{X}}_{k|k-1}^i\\ & P_{k|k-1}=\frac{1}{m}\sum _{i=0}^m({\mathcal{X}}_{k|k-1}^i-{\hat{x}}_{k|k-1})({\mathcal{X}}_{k|k-1}^i-{\hat{x}}_{k|k-1}{)}^{\text{T}}+Q_k\end{array}$$

（3） 测量更新

步骤五： 根据$k$时刻的估计${\hat{x}}_{k|k-1}$和方差$P_{k|k-1}$，计算容积点
$$\begin{array}{rl}& S_{k|k-1}=\sqrt{P_{k|k-1}}\\ & {\zeta }_{k|k-1}^i={\hat{x}}_{k|k-1}+S_{k|k-1}{\xi }_i\end{array}$$
步骤六： 将 ${\zeta }_{k|k-1}^i$点通过量测方程传播，
$${\mathcal{Z}}_{k|k-1}^i=h({\zeta }_{k|k-1}^i)，i=1,2,\cdots ,m$$
步骤七： 观测的预测，观测的预测误差方差（新息方差），状态与量测互协方差，增益
$$\begin{array}{rl}& {\hat{z}}_{k|k-1}=\frac{1}{m}\sum _{i=1}^m{\mathcal{Z}}_{k|k-1}^i\\ & {\mathcal{S}}_k=\frac{1}{m}\sum _{i=1}^m({\mathcal{Z}}_{k|k-1}^i-{\hat{z}}_{k|k-1})({\mathcal{Z}}_{k|k-1}^i-{\hat{z}}_{k|k-1}{)}^{\text{T}}+R_k\\ & C_k=\frac{1}{m}\sum _{i=1}^m({\mathcal{X}}_{k|k-1}^i-{\hat{x}}_{k|k-1})({\mathcal{Z}}_{k|k-1}^i-{\hat{z}}_{k|k-1}{)}^{\text{T}}\\ & K_k=C_k{\mathcal{S}}_k^{-1}\end{array}$$
步骤八：
$$\begin{array}{rl}{\hat{x}}_{k|k}& =E\left[x_k\mid Z^k\right]={\hat{x}}_{k|k-1}+K_z\left(z_k-{\hat{z}}_{k|k-1}\right)\\ P_{k\mid k}& =\text{cov}\left({\tilde{x}}_{k\mid k}\right)=P_{k\mid k-1}-K_k{\mathcal{S}}_k{K}_k^{\text{T}}\end{array}$$
这里${\tilde{x}}_{k\mid k}=x_k-{\hat{x}}_{k|k}$为估计误差，$Z^k$表示前$k$时刻的所有观测，即 { z k , z k − 1 , ⋯   , z 1 } \{z_k,z_{k-1},\cdots,z_1 \} {zk​,zk−1​,⋯,z1​}

参考：从贝叶斯滤波理论到容积卡尔曼滤波算法（CKF）详细推导及编程实现常转弯率模型估计

下面这个更清楚