2.3 容积Kalman滤波算法

非线性Gauss滤波的主要问题是计算非线性函数与Gauss密度函数乘积的积分。Arasaratnam[116]等使用3阶球面-相径容积规则，利用m个容积点加权求和来替代积分问题，从而在贝叶斯估计框架下提出了CKF算法。

2.3.1 容积规则

对于定理2.5中的5个Gauss积分式，可以看出，它们都可以转化成如下形式

其中，C为标量常值，f（x）是向量函数或者矩阵函数。而对于这类积分形式，CKF的提出者巧妙地将其转化成球面-相径积分，再通过容积规则进行近似。

对于式（2-145）中的积分，如果不考虑常值，令x=rz，由积分变换有

式中，U _n为n维单位球面，σ（·）为U _n上的元素，则式（2-146）中的积分就转化成一个球面积分和一个相径积分

对于式（2-147），可以用球面容积规则近似。由于容积规则的全对称性，f（rz）中的每一项单项式为。其中，d_i表示变量的阶次，当为奇数时，该项在球面上的积分为0，所以采用3阶球面容积规则近似该积分，只需考虑和两种情况，上两式在全对称容积规则近似下的球面积分为

其中表示n维单位球的表面积，。

求解上两式，得到，u₁ =1，u₂ =u₃ =…=u_n-1=0，故容积点可选为单位球面与各坐标轴的交点，即点集[1]，则有

而对于相径积分式（2-150），令，由积分变换有

式（2-152）为著名的Gauss-Laguerre积分，根据1阶Gauss-Laguerre积分规则可知，当或者时，可求得积分。

同时，由球面容积规则形成的球面-相径容积规则对所有的奇数阶项的积分都为0，故只需考虑1阶Gauss-Laguerre积分即可，此时，选取的积分点和权值分别为

将式（2-151）和式（2-155）代入式（2-146），可得到

式中，。式（2-156）即为3阶球面相径容积规则的近似策略。

对于一般意义下的Gauss积分

令，则

其中，令m=2n，则有

所以，得到非线性Gauss滤波需要近似求解的积分为

2.3.2 容积Kalman滤波算法

从定理2.3中可知，对于非线性Gauss滤波递推公式，若要转化成具体的可实现的滤波公式，则需要各种近似策略，而基于3阶球面-相径容积规则的CKF算法的实现步骤如下。

第1步：初始化

第2步：计算基本容积点和其对应的权值[116]

其中ξ_i是第i个基本容积点，m是容积点的总数，根据3阶容积积分法则，容积点的总数是系统状态维数的两倍，即m=2n，n是系统状态的维数。[1]∈Rn是完全对称点集。

假设k+1时刻的后验密度函数已知，初始状态误差方差矩阵P（k-1|k-1）正定，则对其进行因式分解有

估算容积点

估算传播容积点

第3步：计算状态预测值和误差协方差矩阵

第4步：估算预测容积点

因式分解

估算容积点

第5步：计算观测预报值和误差协方差矩阵

第6步：计算局部状态滤波和误差协方差矩阵

综上所述，图2-3给出了容积Kalman滤波器的算法流程。