【正文】 kkk 0),c o v (0][ ?? ??0),c o v ( *1,1,1 ????? ??? kkkkkkkkkkkk vwRSJRJS ，則有，即只需令注意到模型中只有系統(tǒng)方程不同，所以與前相比，只有預(yù)測(cè)方程不同。 方法： 引入類(lèi)似于 Lagrange 乘子的待定常數(shù)矩陣，適當(dāng)選取這個(gè)矩陣，變非約束方稱(chēng)為約束方程，將約束條件納入方程，變成符合基本假設(shè)條件的形式。和的差別主要在于，因此濾波方程與之前狀態(tài)方程和觀測(cè)方程中由于這兩項(xiàng)分別出現(xiàn)在1|1|~??? kkkk zx1||1,11,1,1|11,1|11|1|1|1|11,1|11,1|1|1||][][?~??~???????????????????????????????????????????kkkkkkTkkkkkTkkkkkkkkkTkkkkTkkkkkkkkkkkkkkkkkkkkkkkkkkkPHKIPQPPRHPHHPKxHyzzuxxzKxx濾波方程： 24 系統(tǒng)噪聲和測(cè)量噪聲相關(guān)時(shí)的卡爾曼濾波 kkkkkkkkkkkkkkvxHzwuxx???????? ?????? 11,11,11,系統(tǒng)模型： kjkwv SjkR ??),(相關(guān)噪聲： 其他條件與 。 )()()1|(?)()(kkKBDkkxkzkBC??：對(duì)預(yù)測(cè)的修正：新息 ???????????????????????????? 93332112??001 xx和濾波誤差方差陣?；舅枷攵伎梢詺w結(jié)為 “ 預(yù)測(cè)+修正 ” 。 最小二乘 amp。 ? 最小二乘估計(jì)法可行的條件是只需要系統(tǒng)的測(cè)量方程和測(cè)量誤差方差的信息，有遞推算法，最優(yōu)解是狀態(tài)值； ? 維納濾波需要知道系統(tǒng)的輸入和噪聲的功率譜密度，以及輸入和噪聲的互譜密度，屬于批處理方法，最優(yōu)解是濾波器的傳遞函數(shù)或是沖激響應(yīng)，對(duì)狀態(tài)為向量的情況不容易實(shí)現(xiàn)； ? 卡爾曼濾波可行的條件是不僅需要系統(tǒng)的測(cè)量方程，還需要系統(tǒng)的狀態(tài)方程以及系統(tǒng)干擾和測(cè)量誤差的均值和方差信息，遞推方法，可以實(shí)時(shí)實(shí)現(xiàn)，最優(yōu)解是系統(tǒng)的狀態(tài)。 ? 維納濾波和卡爾曼濾波都是最小方差估計(jì)。 3. 估計(jì)方差 的作用：計(jì)算增益，但給出了誤差分析。反比于正比于 RPK kkkk 1?T kkkkkT kkkkkkkk QPP 1,11,1,111,1 ???????? ??????6. 預(yù)測(cè)方差： 。 時(shí)間更新 zK K=k+1 測(cè)量更新 初始狀態(tài)和方差 10 111,1 ?? ???? ?? kkkkkk xx狀態(tài)預(yù)測(cè) T kkkkkT kkkkkkkk QPP 1,11,1,111,1 ???????? ??????方差預(yù)測(cè) 111 ][ ??? ?? kTkkkkTkkkk RHPHHPK增益矩陣 11| ~?? ?? ?? kkkkkkk zKxx狀態(tài)估值 方差估值 1][ ??? kkkkkk PHKIP預(yù)測(cè) /時(shí)間更新： 校正 /觀測(cè)更新： ]?[~ 11 ?? ?? kkkkkk xHzz新息序列 離散卡爾曼濾波器推算方程 11 離散卡爾曼濾波公式公式匯總 12 卡爾曼濾波器結(jié)構(gòu)圖 延時(shí) 一步 1, ??kkkz+ 1| ?kkzkKkx?+ + 當(dāng)前估計(jì)值 上一步估計(jì)值 1|? ?kkxkH一步預(yù)測(cè) 1??kx13 1. 濾波遞推實(shí)現(xiàn)，必須給定初值（狀態(tài)和方差）； 幾點(diǎn)說(shuō)明： 2. 當(dāng)前狀態(tài)最優(yōu)估值： 11| ~?? ?? ?? kkkkkkk zKxx。估計(jì)誤差方差增益矩陣求取原則：使 ]~~[ || kkTkk xxEkkkkk xxx ?~ | ??KF公式 1： 狀態(tài)預(yù)測(cè) 直觀推導(dǎo)法 推導(dǎo)基本步驟：預(yù)測(cè)，校正。態(tài)計(jì)算當(dāng)前時(shí)刻的最優(yōu)狀，和上一時(shí)刻的估計(jì)值利用現(xiàn)有觀測(cè)值kkkkkkxxzzzz|1|1121??, ????要求： 零均值，方差陣為對(duì)角陣 過(guò)程噪聲和觀測(cè)噪聲不相關(guān) 初值和過(guò)程噪聲、觀測(cè)噪聲均不相關(guān) 離散卡爾曼濾波器公式推導(dǎo) 的線(xiàn)性函數(shù)。 ? 維納濾波的不足： ? 與維納濾波相比，卡爾曼濾波不僅適用于非平穩(wěn)隨機(jī)過(guò)程，而且可以遞推實(shí)現(xiàn)。最優(yōu)估計(jì) 第 6章 線(xiàn)性離散系統(tǒng)卡爾曼濾波 ? 線(xiàn)性離散系統(tǒng)卡爾曼濾波器的推導(dǎo) ? 帶有控制項(xiàng)和測(cè)量系統(tǒng)偏差時(shí)的卡爾曼濾波器 ? 系統(tǒng)干擾和測(cè)量噪聲相關(guān)時(shí)的卡爾曼濾波器 ? 有色噪聲下的卡爾曼濾波器 ? 卡爾曼濾波器穩(wěn)定性和魯棒性 ? 線(xiàn)性離散系統(tǒng)的最優(yōu)預(yù)測(cè)與平滑 3 引 言 ? 1960年卡爾曼（ Kalman）將狀態(tài)空間分析方法引入濾波理論，得到時(shí)域上的遞推濾波算法，即卡爾曼濾波。 (1) 要求隨機(jī)過(guò)程是平穩(wěn)的，要求太強(qiáng)； (2) 以單入單出系統(tǒng)推導(dǎo)出的，難以推廣到高維的情況； (3) 維納 霍夫方程很難求解，在工程上難以實(shí)現(xiàn)； (4) 是批處理方法，不能滿(mǎn)足在線(xiàn)快速處理大量數(shù)據(jù)的需要。 系統(tǒng)和量測(cè)的數(shù)學(xué)模型 噪聲及初值統(tǒng)計(jì)特性 狀態(tài)和估計(jì)方差初值 量測(cè)數(shù)據(jù) 系統(tǒng)狀態(tài)的最優(yōu)估值 Kalman Filter: 4 0),(),(0][),(0][?????jkRRjkRvEQjkRwEwvkjkvvkkjkwwk??，? 噪聲和初值的統(tǒng)計(jì)特性 ： 0000 )v a r ( PxxEx ?? ，0),0(0),0(00????TkxvTkxwvExkRwExkRkkkkkkkkkkkvxHzwxx?????? ???? 11,11,? 離散模型 ： 已知： 。是 1211|1 ,? ??? kkk zzzx ?5 ? 預(yù)測(cè) /時(shí)間更新 (利用系統(tǒng)方程對(duì)狀態(tài)及觀測(cè)做一步預(yù)測(cè) )： ? 校正 /測(cè)量更新 (利用新觀測(cè)對(duì)預(yù)測(cè)狀態(tài)進(jìn)行校正 )： KF公式 2： 觀測(cè)預(yù)測(cè) ]?[????1|1||1|1|?????????????kkkkkkkkkkkkkzzKxxzzx KF公式 3：狀態(tài)估計(jì) 待定校正增益陣 ? 如何求 ？ kK最小。 1|11,1| ?? ???? ?? kkkkkk xx??? 0][ 1kwE1|1| ?? ?? ? kkkkk xHz?? 0][ kvE}~][][~][~~]{[1|1|1|1|TkTkkkkkTkkTkkkkTkTkkkTkkTkkkkkkKvxHKIHKIxvKKvvKHKIxxHKIE????????????])?[?(?~1|1|||?? ??????kkkkkkkkkkkkzzKxxxxx0~0~ 1|1| ?? ?? T kkkTkkk xEvvxE ，TkkkTkkkkkkT kkkk KRKHKIPHKIxxE ???? ? ][][~~ 1|||估計(jì)誤差： kkkkkk vKxHKI ??? ? 1|~][])?[?( 1|1| ?? ????? kkkkkkkkkk xHvxHKxx11| ?~ ?? ?? kkkkk xxx]~[~ 1|1| kkkkkkk vxHKx ??? ??估計(jì)誤差方差： ? ? ? ?TkTkTkkT kkkkkkkkT kkkk KvHKIxvKxHKIExxE ????? ?? ][~~)(~~ 1|1|||TkkkTkkkkkk KRKHKIPHKI ???? ? ][]{[ 1|TkkkTkkkkkkkk KRKHKIPHKIP ???? ? ][][ 1||11|1| )( ??? ?? kTkkkkTkkkk RHPHHPK0???kkKJ（ 2）將 Jk 對(duì) Kk 的偏導(dǎo)數(shù)為零，即 1|1|11|1|1||][)(???????????kkkkkkkkTkkkkTkkkkkkkPHKIPHRHPHHPPPKF公式 5：增益矩陣 估計(jì)誤差方差矩陣，與 KF公式 4等價(jià) KF公式 4：估計(jì)誤差方差矩陣 kkk PJ |tr a c e?為了求得 Kk ，（ 1）選擇代價(jià)函數(shù)： ABA B AtrA T 2)]([ ???利用公式：以下求 1| ?kkP1|1| ?~ ?? ?? kkkkk xxx0]~[ 1,11|11, ??? ????? T kkTkkkkk wxE0]~[ 1,1|111, ??? ????? T kkT kkkkk xwETkkkkkTkkkkkkTkkkkkkkkkkkkkkkkQPwxwxEP1,11,1,1|11,11,1|11,11,1|11,1| ]~][~[???????????????????????????????KF公式 6：預(yù)測(cè)誤差方差矩陣 11,1|11, ~ ????? ???? kkkkkkk wx1|11,11,11, ? ??????? ?????? kkkkkkkkkk xwx預(yù)測(cè)誤差： TkkkkkkkkkkkkkkT kkkk wxwxExxE ]~][~[~~ 11,1|11,11,1|11,1|1| ???????????? ???????預(yù)測(cè)誤差方差： TkkTkkkkkTkkTkkkkkTkkTkkkkTkkTkkkkkkwxExwEwwExxE1,11|11,1,1|111,1,111,1,1|11|11,]~[]~[][]~~[????????????????????????????????9 卡爾曼濾波的實(shí)質(zhì) ? 遞推估計(jì)的過(guò)程； ? 預(yù)測(cè) +校正的過(guò)程。最優(yōu)預(yù)測(cè)值，即則最優(yōu)估值，即無(wú)新息，若 1||1 ??0~ ?? ??? kkkkkk xxz4. KF是反饋校正過(guò)程，其校正項(xiàng)： ，比例因子： 1~ ?kkk zK kK111 ][ ??? ?? kTkkkkTkkkk RHPHHPK5. 增益： 。正比于 11 ?? kkk Q P初始估值可由經(jīng)驗(yàn)給定，方差需經(jīng)測(cè)量，由統(tǒng)計(jì)方法給出； 若濾波是穩(wěn)定的，濾波將不依賴(lài)于初值。 kk P14 ? 最小二乘估計(jì)和卡爾曼濾波估計(jì)都是無(wú)偏估計(jì)，都能在得到系統(tǒng)最優(yōu)狀態(tài)估計(jì)值的同時(shí)，還得到估計(jì)誤差的方差；均適用于狀態(tài)為向量的情況。維納濾波要求系統(tǒng)的輸入是平穩(wěn)過(guò)程，最小二乘和卡爾曼濾波無(wú)此要求。 卡爾曼濾波 amp。 維納濾波 卡爾曼濾波的直觀解釋 卡爾曼濾波的基本思想與艦船組合導(dǎo)航人員作業(yè)中對(duì)船位推算的邏輯思維方法是吻合的。 卡爾曼濾波過(guò)程是：每隔一個(gè)濾波周期，通過(guò)量測(cè)傳感器得到量測(cè)船位 C，同時(shí)經(jīng)過(guò)狀態(tài)轉(zhuǎn)移得到預(yù)測(cè)船位 B，在量測(cè)船位和預(yù)測(cè)船位之間根據(jù)增益 Kk 進(jìn)行折衷，從而獲得最佳估計(jì)船位 D，依此過(guò)程不斷循環(huán)下去。、試求卡爾曼濾波估值，并已知)1(?)1(?10001)1(100433)0()0()()()()(2)()1()()()(2)1(110212212211xxRQzPxxEkvkxkzkxkxkxkwkxkxkxkk???????????????????????????????????????????? ?代入濾波公式，得和、將，解：由給定模型知kkRQPxxH02133)0(?)0(?01102112?????????????????????????????????例 已知離散型線(xiàn)性系統(tǒng)的狀態(tài)方程和觀測(cè)方程如下 ?????????????????????? ????????????????????? 8661800012112100421120001TT QPP? ? ?????? ???????????????????????