【正文】 T TT )()()()1( 00 ?????? ?? υBuxx AAA x(k+1)=Fx(k)+Gu(k+1)+νd(k) y(k)=Cx(k)+ωd(k) 卡爾曼濾波 根據(jù)給定條件，求取噪聲序列的統(tǒng)計參數(shù) E[ν d(k)]=0， E[ν d(k)ν dT(k)]=Vδ kj E[ω d(k)]=0， E[ω d(k)ω dT(k)]=Wδ kj δ kj=1， k=j； δ kj=0， k≠j 為了得到對系統(tǒng)狀態(tài) (隨機序列 ) 較為準(zhǔn)確的估計，應(yīng)將隨機噪聲帶來的干擾降到最低 —— 卡爾曼濾波。 )(?)()(~ kkk xxx ??由于狀態(tài)誤差也是一個隨機量 求取其統(tǒng)計參數(shù) —— 協(xié)方差矩陣 )](~)(~[)( kkEkTxxP ?如果能使協(xié)方差矩陣變得最小，就可使?fàn)顟B(tài)的估計最準(zhǔn)。 換言之 ]),2(),1(),(|)([)(? ???? kkkkEk yyyxx最優(yōu)估計應(yīng)是在前 k次輸出 y(k)的條件下，狀態(tài)的期望值。 卡爾曼濾波 如果記 根據(jù) k時刻即 k時刻以前的量測值，估計 j時刻的狀態(tài)。 如果 kj：根據(jù) k時刻即 k時刻以前的量測值，估計以前的狀態(tài) —— 平滑或內(nèi)插。 如果 kj：根據(jù) k時刻即 k時刻以前的量測值，估計未來的狀態(tài) —— 預(yù)報或外推。 如果 k=j：根據(jù) k時刻即 k時刻以前的量測值，估計當(dāng)前(現(xiàn)時刻 )的狀態(tài) —— 濾波。 ]),2(),1(),(|)([)|)((? ???? kkkjEkjk yyyxx卡爾曼濾波 引入說明符號 )]1(|)1[(?)1(? ???? ? kkk xx 為 k1時刻的狀態(tài)估計 )1(?)1()1(~ ????? kkk xxx 為 k1時刻的狀態(tài)估計誤差 )]1(~)1(~[)1( ???? kkEk TxxP為 k1時刻的狀態(tài)估計誤差的協(xié)方差矩陣 )1|(? ?kkx 為下一步的預(yù)報估計 )1|(?)()(~ ??? kkkk xxx為下一步的預(yù)報估計誤差 卡爾曼濾波 )]1|(~)1|(~[)1|( ???? kkkkEkk TxxP為下一步的預(yù)報估計誤差的協(xié)方差矩陣 )|(?)(? kkk xx ??為 k時刻的狀態(tài)估計 )(?)()(~ kkk xxx ?? 為 k時刻的狀態(tài)估計誤差 )](~)(~[)( kkEk TxxP ?為下一步的預(yù)報估計誤差的協(xié)方差矩陣 卡爾曼濾波 狀態(tài)預(yù)報 根據(jù)狀態(tài)方程 x(k+1)=Fx(k)+Gu(k+1)+ν d(k) y(k)=Cx(k)+ω d(k) ]),2(),1(),(|)1([ ]),2(),1(),(|)1([ ]),2(),1(),(|)1([ ]}),2(),1(),(|)]1()1()1({[ ]),2(),1(),(|)([)1|(??????????????????????????????kkkkEkkkkEkkkkEkkkkkkEkkkkEkkyyyνyyyGuyyyFxyyyνGuFxyyyxxdd卡爾曼濾波 因為 E[ν d(k)]=0且 因此預(yù)報方程為 預(yù)報誤差為 ]),2(),1(),(|)([)(? ???? kkkkEk yyyxx)1()1()1|(? ????? kkkk GuFxx)1()1(~)]1()1(?[)]1()1()1([)1(?)1()1|(~???????????????????kkkkkkkkkkkddνxFGuxFνGuFxxxx卡爾曼濾波 預(yù)報誤差的協(xié)方差 VFFPννFxxFνFxνxFxxPTTddTTTdTTdT?????????????????????)1|(0)]1()1([)]1(~)1(~[)]}1()1(~)][1()1(~{[)]1|(~)1|(~[)1|(kkkkEkkEkkkkEkkkkEkk設(shè) x(k)的估計具有以下形式 )]1|(?)()[()1|(?)(? ????? kkkkkkk xCyKxx卡爾曼濾波 當(dāng)前狀態(tài)的預(yù)測值＝上一步對狀態(tài)的預(yù)測值 +修正值 修正值＝修正系數(shù) (當(dāng)前的輸出－上一步的預(yù)測輸出 ) 于是問題變?yōu)槿绾芜x擇 K(k)，使預(yù)測更為準(zhǔn)確。而準(zhǔn)確的標(biāo)準(zhǔn)為 P(k)最?。? })](?)()][(?)({[)](~)(~[)( TT xxxxxxP kkkkEkkEk ????卡爾曼濾波 估計誤差 )()()1|(~][ )()()1|(~)()1|(~ )()()1|(?)()()()1|(?)( )]1|(?)()()[()1|(?)( )(?)()(~kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkωKx)CK(IωKxCKxωKxCKCxKxxxCωCxKxxxxx?????????????????????????)]1|(?)()[()1|(?)(? ????? kkkkkkk xCyKxx卡爾曼濾波 估計誤差的協(xié)方差 )() W KK()CK(IP)CK(I)(Kωω)K()CK(Ixx)CK(IωKx)CK(IωKx)CK(IPTTTTTTTkkkkkkkkkEkkkkkkEkkkkkkkkkkkEk??????????????????][)1|(][ )]()([ ][)]1|(~)1|(~[][ )}()()1|(~][{ )}()()1|(~][{ )(如果 P(k)為最小，則任意給定 K(k)+△ K(k)所產(chǎn)生的 P(k)+△ P(k)中△ P(k)＝ 0 卡爾曼濾波 給定 K(k)+△ K(k)，生成 P(k)+△ P(k)，用 [P(k)+△ P(k)]P(k)=△ P(k)有 )WK(CP)CK(IR)(KR)RK(KWKWKKKCP)CK(I)CK(IP)CK(PTTTTTTTTkkkkkkkkkkkkkkkkkkk?????????????????????)1|(][ )()()()(Δ )](Δ)[1|(][ ])[1|(][ )(如果要△ P(k) =0，進一步推導(dǎo)有 1])1|([)1|()(0])1|()[()1|( )()1|(][ )(???????????????WCCPCPKWCCPKCPWKCP)CK(IRTTTTTkkkkkkkkkkkkkkk卡爾曼濾波 K(k)的計算 給定參數(shù) F、 C、 V、 W以及 P(0)=0或 P(0)=I和 k＝ 1，2， ? ，計算 VFFPP T ???? )1()1|( kkk)() W KK()CK(IP)CK(IP TT kkkkkkk ????? ][)1|(][ )(1])1|([)1|()( ????? WCCPCPK TT kkkkk?????????????????????????????)()()(?)()]1|(?)()[()1|(?)(?)1()1(?)1|(? )()()()()()()1(kkkkkkkkkkkkkkkkkkkkkkCxyxLuxCyKxFxGuxFxωCxyνGuFxxdd卡爾曼濾波