1. 先说清楚CKF到底是个什么滤波器卡尔曼滤波家族里EKF是资历最老的老大哥通过雅可比矩阵把非线性系统硬生生“掰直”成线性来近似UKF是后来的改良派靠UT变换选一组Sigma点用统计近似去逼近非线性函数的概率分布。这两个算法在很多工程场景里都已经用了十几年效果也基本够用。但你如果往深了做比如高维状态估计、强非线性系统、或者说对数值稳定性要求极高的场景EKF和UKF都会暴露出各自的短板——EKF的线性化误差在强非线性下容易被放大UKF在高维情况下会面临Sigma点的权重出现负值的问题导致协方差矩阵失去正定性数值上极其不稳定。CKFCubature Kalman Filter容积卡尔曼滤波就是在这个背景下出现的。它的核心不是拍脑袋选点而是从数学上严格推导出一组容积点Cubature Points来近似高斯加权积分。这组点有明确的数学依据球面-径向容积准则权重恒为正数点数固定为状态维数的2倍既不像EKF那样需要求导也不像UKF那样需要调参调到头秃。我最早接触CKF是做一个组合导航的项目状态维数拉到15维以后UKF的Sigma点权重出现了负值协方差矩阵直接不正经了后来换成CKF才把数值稳定性救回来。这篇就沿着CKF的数学思路走一遍它解决什么问题、容积点是怎么来的、为什么权重一定为正、实际用的时候有哪些坑。适合刚把卡尔曼滤波基础过完、想进一步理解非线性滤波本质的读者也适合在工程项目里被UKF高维数值问题折磨过的工程师。2. 贝叶斯滤波框架所有卡尔曼滤波的共同底盘2.1 状态空间模型和两个基本假设任何卡尔曼滤波类算法本质上都是在求解贝叶斯滤波的递推问题。设系统的状态方程为x_k f(x_{k-1}) w_k z_k h(x_k) v_k其中w_k是过程噪声v_k是量测噪声。卡尔曼滤波能成立的前提是这两个噪声都是零均值高斯白噪声且互不相关。这个假设非常关键——正因为都是高斯分布整个滤波过程才能被均值和协方差两个统计量完全描述否则你就要去面对粒子滤波那种用一堆点去逼近任意分布的重体力活了。贝叶斯滤波做的事情可以用一句话概括已知上一时刻的状态后验分布通过状态方程预测当前时刻的先验分布再用当前时刻的量测去修正得到当前时刻的后验分布。写成递推公式就是p(x_k | z_{1:k}) ∝ p(z_k | x_k) ∫ p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) dx_{k-1}前一半是时间更新预测后一半是量测更新修正两个步骤交替进行滤波器就能一直滚下去。2.2 高斯假设让问题变得“可算”如果状态和噪声都是高斯分布且状态方程和量测方程都是线性的那上面的积分有闭式解这就是经典卡尔曼滤波。问题出在非线性f和h不是线性函数的时候p(x_k | z_{1:k})就不再是高斯分布了积分也没法解析求解。这时候各家算法就开始走不同的近似路线EKF对非线性函数做一阶泰勒展开把问题强行拉回线性框架但展开点附近的局部线性化在强非线性下误差很大。UKF选取一系列Sigma点通过这些点的非线性映射来近似变换后的均值和协方差。这个方法不再需要求导精度能到三阶但高维时权重会出现负值。CKF用球面-径向容积准则选取容积点本质上也是用一组加权点去近似高斯加权积分但数学上更干净。CKF的切入点和UKF不太一样。UKF是先定Sigma点的形状比例对称或最小偏度单形再去凑权重CKF则是从高斯加权积分本身出发先做球面-径向变换再推导数值积分规则最后自然得到一组容积点。这个顺序上的区别决定了CKF的权重恒正、点数固定为2n不需要任何调节参数。3. 容积点从哪来球面-径向变换的完整推导3.1 高斯加权积分的基本形式先看最核心的数学问题。假设有一个标准高斯分布密度函数是N(x; 0, I) (2π)^(-n/2) · exp(-x^T x / 2)要计算一个非线性函数g(x)关于这个分布的期望也就是积分I(g) ∫ g(x) N(x; 0, I) dx卡尔曼滤波的时间更新和量测更新里那些看起来让人头疼的积分全部都可以归化成这种形式——本质都是求非线性函数在高斯分布下的统计期望。所以问题就变成了怎么精确、高效地做这种高斯加权积分。3.2 关键变换球面坐标与径向积分的分离这里有个漂亮的数学技巧。把x写成x r · s其中r √(x^T x)是径向半径s是单位球面上的点满足s^T s 1。这个变换把n维空间的积分拆成了两部分在单位球面上的面积分以及沿着径向r的一维积分。代入高斯加权积分I(g) ∫_0^∞ ∫_{S^n} g(r·s) (2π)^(-n/2) · exp(-r^2 / 2) · r^(n-1) dσ(s) dr注意看exp(-r^2/2)和r^(n-1)只跟径向有关所以可以先算球面上的积分再算径向积分。这个分解的价值在于我们把一个高维积分变成了一个低维球面积分和一个一维径向积分。球面上的积分用一组等权重的点来近似径向积分则用高斯-拉盖尔求积公式来精确处理。3.3 三阶容积准则的推导结果接下来的推导需要一些计算细节但结论非常干净。为了保证对三阶以下多项式精确成立球面-径向容积准则最终给出的容积点和权重是ξ_i √(n) · e_i, i 1, 2, ..., n ξ_(ni) -√(n) · e_i, i 1, 2, ..., n ω_i 1 / (2n), i 1, 2, ..., 2n其中e_i是n维空间里的第i个标准基向量只有第i位是1其他位都是0。也就是说2n个容积点分别分布在n个坐标轴的正负方向、距离原点√n的位置上每个点的权重都是1/(2n)。这个结果有几个直接推论权重全部为正。这一点在数值上是巨大的优势——任何一步计算中协方差矩阵都不会因为负权重而失去正定性。点数固定为2n。不需要像UKF那样选α、β、κ这些比例参数更没有“调参玄学”。点的位置完全由维数决定。状态维数越高点的半径√n越大但点的分布依然对称。3.4 从标准高斯到一般高斯上面推导出来的容积点适用于标准高斯分布均值为0、方差为单位阵。实际滤波中状态分布是任意的N(m, P)。这时候不能直接用标准容积点要做一步线性变换x m S · r其中S是协方差矩阵P的Cholesky分解下三角矩阵满足P S S^T。这一步的含义可以理解成先把一般高斯分布旋转、拉伸回标准高斯分布在标准空间里用体积点做数值积分再变换回原空间。对应的积分转化关系是∫ g(x) N(x; m, P) dx ∫ g(m S·r) N(r; 0, I) dr ≈ Σ_{i1}^{2n} ω_i · g(m S·ξ_i)实际操作中每次预测和更新都要对协方差矩阵做一次Cholesky分解。这个分解虽然有点计算量但非常稳定也是CKF在工程上可靠的重要原因之一。4. CKF滤波流程预测和更新到底怎么走4.1 时间更新预测CKF的时间更新和卡尔曼滤波家族的逻辑一致用上一时刻的后验分布去推当前时刻的先验分布。具体步骤是第一步对上一时刻的协方差矩阵P_(k-1|k-1)做Cholesky分解P_(k-1|k-1) S_(k-1|k-1) S_(k-1|k-1)^T第二步计算容积点X_(i, k-1|k-1) m_(k-1|k-1) S_(k-1|k-1) · ξ_i其中ξ_i就是上一节推导出的标准容积点。第三步把每个容积点通过状态方程传播X*_(i, k|k-1) f(X_(i, k-1|k-1))第四步加权求预测均值和预测协方差m_(k|k-1) Σ_{i1}^{2n} ω_i · X*_(i, k|k-1) P_(k|k-1) Σ_{i1}^{2n} ω_i · (X*_(i, k|k-1) - m_(k|k-1)) · (X*_(i, k|k-1) - m_(k|k-1))^T Q这里的Q是过程噪声协方差矩阵。需要注意的是每个容积点传播后要减去预测均值再求外积这个操作是协方差近似的标准做法。4.2 量测更新修正量测更新同样生成一组容积点但用的是预测分布。先对预测协方差做Cholesky分解P_(k|k-1) S_(k|k-1) S_(k|k-1)^T生成容积点并经过量测方程传播X_(i, k|k-1) m_(k|k-1) S_(k|k-1) · ξ_i Z_(i, k|k-1) h(X_(i, k|k-1))然后计算量测预测均值、新息协方差和互协方差ẑ Σ_{i1}^{2n} ω_i · Z_(i, k|k-1) P_zz Σ_{i1}^{2n} ω_i · (Z_(i, k|k-1) - ẑ)(Z_(i, k|k-1) - ẑ)^T R P_xz Σ_{i1}^{2n} ω_i · (X_(i, k|k-1) - m_(k|k-1))(Z_(i, k|k-1) - ẑ)^T这里的R是量测噪声协方差矩阵。最后算卡尔曼增益并更新状态K_k P_xz · P_zz^(-1) m_(k|k) m_(k|k-1) K_k · (z_k - ẑ) P_(k|k) P_(k|k-1) - K_k · P_zz · K_k^T整体流程和UKF非常像区别只在于点数固定为2n而非2n1权重全部相等且为正不需要调比例参数。这两个区别让CKF在高维场景下更稳。5. 一张表格看清CKF、UKF和EKF的差异维度EKFUKFCKF核心思路一阶泰勒线性化UT变换选Sigma点球面-径向容积准则是否需要求导需要雅可比矩阵不需要不需要容积/Sigma点数量无2n12n权重符号无不是点方法高维下可能出现负权重恒为正调节参数无α、β、κ需要整定无数值稳定性中等高维下容易失稳较高强非线性精度较低二阶到三阶三阶典型场景弱非线性、模型简单中低维中等非线性高维、强非线性、组合导航这里要特别解释一下UKF和CKF的区别。很多人以为UKF和CKF就是“换了一组点”其实不是。UKF的Sigma点是通过对协方差矩阵做Cholesky分解然后加减√((nλ))倍的列向量得到的点的位置直接与调参比例λ相关CKF的容积点则是从高斯加权积分数值近似的角度严格推导的点的位置是√n不带任何可调参数。从数学哲学上说UKF是“凑点去匹配矩”CKF是“推导积分规则再自然得到点”后者在理论上更自洽。6. 实操中的关键细节与避坑经验6.1 Cholesky分解失败怎么办CKF每步都要做Cholesky分解这是整个算法里最脆弱的一环。如果P矩阵在数值计算中失去正定性分解直接就会报错。处理方法我试过几种最常用的招是在分解前加一个小的正则化项P P ε·Iε取1e-9到1e-12量级既能保证分解不炸也不会明显影响滤波精度。另外可以在分解前检查对角线元素是否有负值或NaN及时发现问题才能对症下药。工程上我还习惯把滤波器的状态变量做归一化处理避免因为量纲差异过大导致协方差矩阵的条件数太大。6.2 噪声矩阵的初始化不能瞎拍脑袋Q和R是滤波器的两个“信任旋钮”Q越大表示越信任量测R越大表示越信任模型。实际项目里很多人上来就用单位阵结果滤波曲线飘得没法看。我的做法是先用离线数据粗略估算量测噪声的方差作为R的初值Q则从一个小量比如1e-6的对角阵开始逐步调到滤波曲线既不发散也不过度抖动的平衡点。这一步没有理论上的“最优解”只有工程上的“够用就行”。6.3 容积点传播后的数值陷阱容积点经过状态方程传播后如果原始f函数里带有强非线性项比如三角函数嵌套高次项计算出来的点之间差值可能非常大。这时候求协方差时容易出现“大数减小数”的灾难性抵消。我踩过这个坑之后能归一化的变量尽量在状态向量里就用归一化表示实在无法避免的在计算协方差时用两步法先算各点的均值再从每个点里减去均值后再求外积。这个操作看着不起眼但对数值稳定性帮助很大。7. 从标准CKF到平方根CKF进一步压榨数值稳定性如果状态维数非常高比如20维以上或者精度要求极其苛刻标准CKF的Cholesky分解仍然可能因为协方差矩阵退化而失败。这时候可以考虑平方根CKFSquare-Root Cubature Kalman Filter。平方根CKF的核心思想是直接传递协方差矩阵的Cholesky因子而不是传递协方差矩阵本身。预测和更新步骤中所有跟协方差相关的运算都在平方根域完成相当于用平方根因子的数值稳定性换掉了P P^T本身的条件数劣势。代价是实现复杂度高不少但换来的是整个滤波过程几乎不会因为数值问题发散。我个人的经验是15维以下用标准CKF足够稳20维以上上平方根CKF。如果你做得更精细还可以考虑与自适应机制结合——比如基于新息序列的协方差匹配方法来在线调整Q和R让滤波器在环境变化时自适应地调整信任权重。8. CKF最适合哪些场景我的选型建议CKF的出现不是为了干掉UKF而是在UKF不适用的场景里补齐短板。从我实践过的项目来看这几类场景最适合CKF第一类是高维状态估计。比如组合导航里状态向量动辄15维到30维UKF在高维下Sigma点权重出现负值的概率大增CKF的恒正权重优势非常明显。第二类是强非线性系统的滤波。比如目标跟踪里目标做高机动转弯运动量测方程又涉及雷达测距测角的极坐标转换这种情况下EKF的一阶线性化误差已经不可忽略CKF的三阶精度更有保障。第三类是对可重复性要求极高的项目。UKF的三个调节参数在不同维数、不同噪声下都要重新整定没有明确规则CKF没有任何参数同样的代码换个系统直接跑不会因为参数没调好而出诡异结果。当然如果你的系统状态维数就2、3维非线性也不强那EKF够用且计算量最小如果维数在5维以下且不追求极致稳定UKF也没毛病。工具没有好坏只有合不合适——CKF是给你多一个更稳的选项不是要你无脑替换。9. 我的一点实践心得当初从UKF切到CKF的时候我其实挺意外——数学推导那么多工程实现却比UKF更简单。没有参数要调点是对称的权重的和为1每一步都干净利落。后来再回头看那套球面-径向容积准则的推导我才真正理解一个道理很多看起来复杂的算法改进本质上是把“凑”变成了“推”把工程上的修修补补升级成数学上自洽的框架。CKF没有引入什么惊天动地的全新思想它只是把高斯加权积分这个老问题用更漂亮的方式重新做了一遍。如果你正在被高维滤波的数值稳定性问题折磨或者被UKF的参数整定搞到头大我建议你花一晚上把CKF的推导过一遍再用代码实现一版对比看看。数学的美往往就在这种“推着推着答案自己浮现出来”的过程里。