卡爾曼濾波器入門

卡爾曼濾波TheKalmanFiltering1數(shù)字濾波：通過(guò)一種算法排除可能的隨機(jī)干擾，提高檢測(cè)精度的一種手段2RudolfEmilKalman匈牙利數(shù)學(xué)家BS&MSatMITPhDatColumbia1960年發(fā)表的論文《ANewApproachtoLinearFilteringandPredictionProblems》（線性濾波與預(yù)測(cè)問(wèn)題的新方法）3應(yīng)用場(chǎng)合機(jī)器人導(dǎo)航、控制傳感器數(shù)據(jù)融合雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤計(jì)算機(jī)圖像處理頭臉識(shí)別圖像分割圖像邊緣檢測(cè)4KF是根據(jù)上一狀態(tài)的估計(jì)值和當(dāng)前狀態(tài)的觀測(cè)值推出當(dāng)前狀態(tài)的估計(jì)值的濾波方法S(t)=f(S(t-1),O(t))它是用狀態(tài)方程和遞推方法進(jìn)行估計(jì)的，因而卡爾曼濾波對(duì)信號(hào)的平穩(wěn)性和時(shí)不變性不做要求維納濾波：使用全部觀測(cè)值保證平穩(wěn)性5卡爾曼濾波器是一個(gè) optimalrecursivedataprocessingalgorithm 最優(yōu)化自回歸數(shù)據(jù)處理算法最優(yōu)（optimal）依賴于評(píng)價(jià)性能的判據(jù)。 Kalman濾波器充分利用如下信息估 a.系統(tǒng)和測(cè)量裝置的動(dòng)態(tài)特性； b.系統(tǒng)噪聲、測(cè)量誤差和動(dòng)態(tài)模型的不確定性的統(tǒng)計(jì)描述； c.感興趣變量的初始條件的相關(guān)信息。遞歸（recursive）是指kalman不需要保存先前的數(shù)據(jù)，當(dāng)進(jìn)行新的測(cè)量時(shí)也不需要對(duì)原來(lái)數(shù)據(jù)進(jìn)行處理。6房間溫度的當(dāng)前感覺(jué)值房間溫度計(jì)的當(dāng)前讀數(shù)五分鐘以后房間溫度的實(shí)際值問(wèn)題描述已知條件希望得到都帶有誤差感覺(jué)值+測(cè)量值未來(lái)時(shí)刻的真實(shí)值？=7x(k+1)=Ax(k)+Bu(k)+w(k)z(k)=Hx(k)+v(k)狀態(tài)方程stateequation測(cè)量方程measurementequation狀態(tài)空間模型89x(k)k時(shí)刻k+1時(shí)刻k-1時(shí)刻z(k+1)x(k+1|k+1)u(k)z(k)10x(k+1)=Ax(k)+Bu(k)+w(k)z(k)=Hx(k)+v(k)x(k+1)z(k+1)+x(k+1|k+1)思路：加權(quán)求和11x(k+1)=Ax(k)+Bu(k)+w(k)k時(shí)刻k+1k-1x(k+1|k)=Ax(k|k)+Bu(k)+w(k)①x(k+1|k+1)12P(k+1|k)=AP(k|k)A’+Q②x(k+1|k)對(duì)應(yīng)的協(xié)方差也需要預(yù)測(cè)13k時(shí)刻k+1k-1x(k+1|k+1)至此，完成了預(yù)測(cè)階段得到了兩個(gè)量：下一時(shí)刻的狀態(tài)預(yù)測(cè)x(k+1|k)描述x(k+1|k)優(yōu)劣程度的協(xié)方差P(k+1|k)14z(k+1)=Hx(k+1|k)+v(k+1)K時(shí)刻k+1時(shí)刻k-1測(cè)量值z(mì)(k+1)x(k+1|k+1)x(k+1|k)P(k+1|k)15K時(shí)刻k+映1時(shí)刻k-炮1x(狂k+宜1|祖k)P(幸k+漸1|泡k)y(傳k+紅1)緣瑞=統(tǒng)z(宇k+妨1)烘-鏈H償x(凱k+尚1|杰k)k+運(yùn)1時(shí)刻桑，測(cè)漆量余職量y(調(diào)k+魄1)的表灑達(dá)式測(cè)量陰余量y(翅k+肯1)的協(xié)住方差S=球H漠P(鵲k+急1|騾k)略H捐’踐+薦R測(cè)量侵余量y(援k+著1)16x(唱k+縱1|駐k+玩1)率=x厲(k掉+1槐|k升)斯+別K毅y(呀k+欺1)K(分k+液1)爆=那P(蔥k+以1|霞k)除H簽’/照S③④x(擋k+響1|舞k)y(袖k+剩1)加權(quán)x(蹄k+縱1|旺k+段1)17P(鉗k+凝1|殺k+嚼1)壺=(稠I-攻K(賭