《随机系统的滤波与控制》研究生课程教学课件（讲稿）非线性系统滤波算法——容积卡尔曼滤波算法及其改进

兴容积卡尔曼滤波

容积卡尔曼滤波

容积卡尔曼滤波|CaiYuan-Li容积卡尔曼滤波算法及其改进西安交通大学蔡远利1/74

容积卡尔曼滤波 | Cai Yuan-Li 容积卡尔曼滤波算法及其改进 西安交通大学 蔡远利 1 / 74

容积卡尔曼滤波CaiYuan-Li1贝叶斯递推滤波框架4..82容积数值积分3容积卡尔曼滤波算法.17..244平方根容积卡尔曼滤波算法.305迭代容积卡尔曼滤波算法...366递推后验克拉美罗下限，517再入弹道目标状态估计仿真，..728小结9一些参考文献，.732/74

容积卡尔曼滤波 | Cai Yuan-Li 1 贝叶斯递推滤波框架. 4 2 容积数值积分 . 8 3 容积卡尔曼滤波算法.17 4 平方根容积卡尔曼滤波算法.24 5 迭代容积卡尔曼滤波算法.30 6 递推后验克拉美罗下限.36 7 再入弹道目标状态估计仿真.51 8 小结 .72 9 一些参考文献 .73 2 / 74

容积卡尔曼滤波|CaiYuan-Li考虑如下离散时间非线性系统：X, =f(Xk-1)+Wk-I(1)Z=h(x)+Vk(2)式中：f()：R"→R"，h()：R"→R"。X是维数为n的状态向量，Zk是维数为n.的量测值。过程噪声W-和量测噪声v相互独立，且3/74

容积卡尔曼滤波 | Cai Yuan-Li 考虑如下离散时间非线性系统： 1 1 ( ) kk k = + − − x fx w (1) ( ) z hx v k kk = + (2) 式中：f( ) ： x x   n n → ，h( ) ： x y n n   → 。 k x 是维数为 x n 的状态向量， k z 是维数为 z n 的量测值。过程噪声 wk−1 和量测噪声 k v 相互独立，且 3 / 74

容积卡尔曼滤波|CaiYuan-LiWk-I1~N(0,Qk-I).V~N(0,R,)。1贝叶斯递推滤波框架在k-1时刻，假设状态xk-,服从高斯分布，即xk-1～N(xk-1;x-1,Pk-1)。4/74

容积卡尔曼滤波 | Cai Yuan-Li 1 1 (, ) w 0Q k k − −   , (, ) k k v 0R   。 1 贝叶斯递推滤波框架 在 k −1时刻，假设状态 k−1 x 服从高斯分布，即 1 111 (;, ) ˆ k kkk − −−− x xxP   。 4 / 74

容积卡尔曼滤波ICaiYuan-Li1.1时间更新X, = E[x/Yk-]= E[f(xk-)+ Wk-/Y-]= E[f(xk-)/Y- ](3)= f(xx-1)p(x-Yx-1)dxx-= [ f(xx-1)N(xx-1;xx-1, P-1)dxt-- (R"R"P, = E[(X -X)(X-X,)Y]= [ f(xk--)f(xx-1)N(xk-1;xk-1, Px--)dxx-1 -X,x +Qk-1(4)Ra5/74

容积卡尔曼滤波 | Cai Yuan-Li 1.1 时间更新 1 1 11 1 1 1 11 1 1 111 1 [ ] [( ) ] [( ) ] ( )( ) ( ) ( ; , ) ˆ n n x x k kk k kk k k k kk k k kkk k E E E p d d − − −− − − − −− − − −−− − = =+ = = = ∫ ∫ x xY fx w Y fx Y fx x Y x fx x x P x    (3) 1: 1 1 111 1 1 [( )( ) ] ( )( ) ( ; , ) ˆ nx T k k kk k k T T k k k k k k kk k E d − − −−− − − =− − = − + ∫ P x xx x Y fx fx x x P x xx Q   (4) 5 / 74

容积卡尔曼滤波CaiYuan-Li1.2量测更新假设x~（x,x,P），可求出量测预测和新息方差=[ h(xx-1)W(x;x,P,)dxPo(5)Pgu= J h(x-)h(x-) N(x;x,P)d -I+RkR"r(6)6/74

容积卡尔曼滤波 | Cai Yuan-Li 1.2 量测更新 假设 (;, ) k kkk x xxP   ，可求出量测预测和新息方差： 1 ( ) (;, ) nx k k kkk k d = ∫ − y hx x x P x   (5) , 11 ( )( ) ( ; , ) nx T T yy k k k k k k k k k k d = − − − + ∫ P hx hx x x P x yy R   (6) 6 / 74

容积卡尔曼滤波|CaiYuan-Li似然密度函数服从高斯分布p(yY）=N(yk;k,Pw）。xh"(x)W(x;x,P)dx-xyPay,=(7)RSW,=PouPsik(8)X,=x+W(yk-y)(9)7/74

容积卡尔曼滤波 | Cai Yuan-Li 似然密度函数服从高斯分布 1: , ( ) (;, ) k k k k yy k p yY yyP =  。 , () (;, ) nx T xy k k k k k k k k k = d − ∫ P xh x x x P x xy   (7) 1 k xy k yy k , , − W PP = (8) ˆ ( ) x x Wy y k k kk k =+ − (9) 7 / 74

容积卡尔曼滤波|CaiYuan-LiP,=P.-W,PwAW!(10)k时刻的后验概率密度p(x/Y）=N(x;xP）。2容积数值积分在贝叶斯框架下，状态估计最终化为求解式(3)一（7)中的多维积分，即将非线性滤波归结为非线性函数和高斯概率密度乘积的积分问题。考虑下面非线性函数积分问题：8/74

容积卡尔曼滤波 | Cai Yuan-Li , T P P WP W k k k yy k k = − (10) k 时刻的后验概率密度 ( ) (;, ) ˆ kk kkk p xY xxP =  。 2 容积数值积分 在贝叶斯框架下，状态估计最终化为求解式(3)—(7)中的多维积分，即将非线 性滤波归结为非线性函数和高斯概率密度乘积的积分问题。 考虑下面非线性函数积分问题： 8 / 74

容积卡尔曼滤波CaiYuan-LiI(f)= Jg, f(x)e-*dx(11)式中：I(f)为所求积分，x为滤波估计状态向量，xeR"，R"为积分域，f(x)为非线性函数。式(11)的积分可使用基于容积原则的数值积分方法进行求解。首先，令x=rs且ss=1则xx=r2,re[0,)，将式(11)变换为球形-径向容积形式，则I(f)= { [ f(rs)r"-l exp(-r)do(s)dr(12)19/74

容积卡尔曼滤波 | Cai Yuan-Li () () T nx I ed − = ∫ x x f fx x  (11) 式中：I( )f 为所求积分，x 为滤波估计状态向量， x n x∈ ， x n  为积分域，f x( ) 为非线性函数。 式(11)的积分可使用基于容积原则的数值积分方法进行求解。首先，令x s = r 且 1 T s s = 则 2 , [0, ) T x x = ∈∞ r r ，将式(11)变换为球形-径向容积形式，则 1 2 0 ( ) ( ) exp( ) ( ) n n I r r r d dr σ ∞ − = − ∫ ∫ U f fs s (12) 9 / 74