【正文】 () 1( 1 , ) ( 1 , ) ( ) ( 1 , ) ( )HK n n F n n P n F n n n?? ? ? ? ?則： 和 第四項相消 于是有， Ricccati 差分方程 （ ） 卡爾曼濾波 ? 濾波算法 實際上，可以證明 為狀態(tài)誤差向量的相關(guān)矩陣 . ( ) { ( ) ( ) }HP n E n n???1( ) ( , 1 ) ( 1 , ) ( ) ( ) ( , 1 )P n K n n F n n G n C n K n n?? ? ? ? ?() 4）歸納起來，可得： 卡爾曼濾波算法： 初始： ^^^0( 1 ) { ( 1 ) }( 1 , 0 ) {( ( 1 ) ( 1 ) ) ( ( 1 ) ( 1 ) ) }HnX E XK E X X X X??? ? ?1 , 2 , 3 .. .n ?計 算輸入觀測向量序列 ? ? ? ? ? ?? ?1 , 2y y y n? 卡爾曼濾波 ? 濾波算法 已知參數(shù) ，觀測矩陣 C(n) ，過程噪聲相關(guān)矩陣 ? ?nnF ,1? ? ?nQ1一步預(yù)測： ^^^( 1 ) ( 1 , ) ( ) ( ) ( )( 1 ) ( 1 ) ( 1 )X n F n n X n G n ny n C n X n?? ? ? ?? ? ? ?1( ) ( 1 , ) ( , 1 ) ( ) [ ( ) ( , 1 ) ( ) ( ) ]HHG n F n n K n n C n C n K n n C n n? ?? ? ? ? ?由（ ）與（ ） ^( ) ( ) ( ) ( )n y n C n X n? ??1( 1 , ) ( 1 , ) ( ) ( 1 , ) ( )HK n n F n n P n F n n n?? ? ? ? ?1( ) ( , 1 ) ( 1 , ) ( ) ( ) ( , 1 )P n K n n F n n G n C n K n n?? ? ? ? ? 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 航天器姿態(tài)角速度瞬時估計 其角度方程一階近似表示如下 39。( ) ( 1 ) ( 1 )H H Hn n T n? ? ?? ? ? ?角速度方程 39。 39。 39。39。( ) ( 1 ) ( 1 )H H Hn n T n? ? ?? ? ? ?加速度方程 39。39。 ( ) ( ) ( )H n u n w n? ??22{ ( ) } wE w n ?? 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 設(shè)狀態(tài)向量 上面的一階近似方程均可表示為狀態(tài)方程 139。239。39。3()()( ) ( ) ( )() ()HHHnxnx n x n nxn n????????????? ????????????() A x(n)=Fx(n1)+u(n)+w(n) () 與前面的狀態(tài)方程相比多了 u(n) 狀態(tài)轉(zhuǎn)移矩陣 10010 0 0TFT???????????() 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 ( ) [ 0 , 0 , ( ) ] Tu n u n?（ ） ( ) [ 0 , 0 , ( ) ] Tw n w n? （ ） 噪聲相關(guān)矩陣 20 0 00 0 000wwQ??????????（ ） B 觀察方程 ( ) ( ) ( )Ty n h x n v n?? （ ） ()H n? 觀察矩陣 為白噪聲 方差 2v? 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 由狀態(tài)方程式 ()和觀察方程（ ）分別得到 狀態(tài)向量的兩個估計子 ( 1 ) ( 2 )? ?( ) ( )x n x n和( 1 )? ?( ) ( 1 ) ( )x n F x n u n? ? ?（ ） ( 2 )?( ) ( )Ty n h x n?（ ） 通過線形組合構(gòu)成一個統(tǒng)一估計 ( 1 ) ( 2 )? ? ?( ) [ ( ) ] ( ) ( ) ( )x n I A n x n A n x n? ? ?（ ） ( 2 ) ( 1 )? ? ?[ ( ) [ ( ) ] ( ) ( ) ( ) ]x n I A n x n A n x n? ? ?或 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 求最小均方差準(zhǔn)則下的最優(yōu)加權(quán)矩陣 A（ n ） Th令 A（ n ） =m(n) （ ） 代入 ( 1 ) ( 2 )? ? ?( ) [ ( ) ] ( ) ( ) ( )TTx n I m n h x n m n h x n? ? ?（ ） ( 2 )? ( ) ( )Th x n y n其 中 ， 即有恒等式 ( ) [ ( ) ] ( ) ( ) ( )TTx n I m n h x n m n h x n? ? ?（ ） 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 （ ） （ ）得 ( 1 )? ?( ) ( ) [ ( ) ] [ ( ) ( ) ] ( ) [ ( ) ( ) ]TTx n x n I m n h x n x n m n h x n y n? ? ? ? ? ?( ) ( ) v nTh x n y n?其 中 ， 即 （ ）vn（ ）有估計誤差的相關(guān)矩陣 ? ?( ) {[ ( ) ( ) ] [ ( ) ( ) ] }Tp n E x n x n x n x n? ? ?( 1 ) ( 1 ) 2? ?[ ( ) ] {[ ( ) ( ) ] [ ( ) ( ) ] } [ ( ) ] ( ) ( )T T T T TvI m n h E x n x n x n x n I m n h m n m n?? ? ? ? ? ? ?( 1 ) ( 1 ) 1? ?{[ ( ) ( ) ] [ ( ) ( ) ] } ( )TE x n x n x n x n p n? ? ?令21( ) [ ( ) ] ( ) [ ( ) ] ( ) ( )T T T Tvp n I m n h p n I m n h m n m n?? ? ? ? ?令 （ ） 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 其中 是第一個估計子誤差 1[ ( ) ] ( ) [ ( ) ]T T TI m n h p n I m n h? ? ?2( ) ( )Tvm n m n? 是第二個估計子誤差 m in ( )rt p w 得 211() 2 ( ) 2 ( ) ( ) 2 ( )()Trvt p n p n h m n h p n h m nmn ?? ? ? ? ??（ ） 令其為 0得 2111( ) ( ) [ ( ) ]T vm n p n h h p n h ? ???（ ） 代入（ ）得 1( ) [ ( ) ] ( )Tp n I m n h p n??（ ） 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 1 ()pn計 算( 1 ) ( 1 )1 ? ?( ) {[ ( ) ( ) ] [ ( ) ( ) ] }Tp n E x n x n x n x n? ? ?由 () 1 ? ?( ) {[ ( ( 1 ) ( 1 ) ) ( ) ] [ ( ( 1 ) ( 1 ) ) ( ) ] }Tp n E F x n x n w n F x n x n w n? ? ? ? ? ? ? ? ? ?() ( 1 ) T wFp n F Q? ? ?于是有算法步驟 初始條件 ?(1 ) 0 ,x p I??姿態(tài)角觀察值 ()yn系統(tǒng)噪聲 2() wwn ???? 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 觀察噪聲 2 vV ( )n ???? 觀察矩陣 h [1, 0 , 0] T?狀態(tài)轉(zhuǎn)移矩陣 10F 0 10 0 0TT?????????計算 n=1,2,3….. 1 ( ) ( 1 ) T wp n F p n F Q? ? ?() 2111( ) ( ) [ ( ) ]T vm n p n h h p n h ? ???() 卡爾曼濾波 ? 基于卡爾曼濾波的角速度估計 1( ) [ ( ) ] ( )Tp n I m n h p n??（ ） ( 1 )? ?( ) ( 1 ) ( )x n F x n u n? ? ?（ ） ( 1 )? ?( ) [ ( ) ] ( ) ( ) ( )Tx n I m n h x n m n y n? ? ?( ) ( ) ( )Ty n h x n v n?? —— 姿態(tài)角（ ） x(n)中第二個分量是角速度 39。()H n?