容积卡尔曼滤波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