容积卡尔曼滤波CaiYuan-LiN(x;μ,).对应的非线性积分为(-py z(x-) dxf(x)N(x,μ, E)dx f(x)e12元(20)分解=V//"，作变量变换x=Vzy+μ，则式(20)的积分变换为15/74
容积卡尔曼滤波 | Cai Yuan-Li (; , ) x μ Σ ，对应的非线性积分为 1 1 () () 2 1 () (; , ) ( ) 2 T n n x x d ed π − −− − = ∫ ∫ x μ Σ x μ fx x μ Σ x f x x Σ (20) 分解 T Σ = Σ Σ ，作变量变换 x = + Σy μ，则式(20)的积分变换为 15 / 74
容积卡尔曼滤波ICaiYuan-Li1 f(VEy+p)/edxf(x)W (x;μ, E)dx/2元2J r(/Zy+μ)e"dy12元(21)[ f(Ey+μ)W(y;0,I)dyR"利用式(18)，可得到式(20)积分的3阶球形-径向数值积分的近似解[.. f(x)N(x;μ, E)dx ~Zo,f(/Z5, +μ)i=(22)16/74
容积卡尔曼滤波 | Cai Yuan-Li 1 2 1 2 1 () (; , ) ( ) 2 1 ( ) (2 ) ( ) (;,) T nx nx T nx nx n d e d e d d π π − − = + = + = + ∫ ∫ ∫ ∫ y y y y fx x μ Σ x f Σy μ Σ x Σ f Σy μ y f Σy μ y0I y (21) 利用式(18)，可得到式(20)积分的 3 阶球形-径向数值积分的近似解 1 () (; , ) ( ) nx m i i i d ω = ∫ fx x μ Σ x f ≈ + ∑ Σξ μ (22) 16 / 74
容积卡尔曼滤波|CaiYuan-Li3容积卡尔曼滤波算法基于容积原则的卡尔曼滤波方法，即容积卡尔曼滤波方法（CubatureKalmanFilterCKF)。若k-1时刻的后验概率为p(xk-zk-1)~N(xk-1x-1,Pk-），容积卡尔曼滤波算法步骤如下：17/74
容积卡尔曼滤波 | Cai Yuan-Li 3 容积卡尔曼滤波算法 基于容积原则的卡尔曼滤波方法，即容积卡尔曼滤波方法（Cubature Kalman Filter ，CKF）。 若 k −1时刻的后验概率为 1 1: 1 1 1 1 ( ) (;, ) ˆ k k kkk p − − −−− xz xxP ，容积卡尔曼 滤波算法步骤如下： 17 / 74
容积卡尔曼滤波CaiYuan-Li3.1时间更新3.1.1计算方差的平方根和容积点Sk-I = chol(Pk-1)(23)X,k-1 =Sk15, +Xk-I(24)式中：chol表示矩阵的Cholesky分解。18/74
容积卡尔曼滤波 | Cai Yuan-Li 3.1 时间更新 3.1.1 计算方差的平方根和容积点 1 1 chol{ } S P k k − − = (23) ,1 1 1 ˆ X S jk k j k −− − = + ξ x (24) 式中：chol 表示矩阵的 Cholesky 分解。 18 / 74
容积卡尔曼滤波CaiYuan-Li3.1.2计算通过非线性状态方程传播的容积点X, = f(X ju-1)(25)3.1.3计算状态预测和方差ox'X=j.j=l(26)19/74
容积卡尔曼滤波 | Cai Yuan-Li 3.1.2 计算通过非线性状态方程传播的容积点 * , ,1 ( ) X fX jk jk = − (25) 3.1.3 计算状态预测和方差 * , 1 m k j jk j ω = x X = ∑ (26) 19 / 74