卡爾曼濾波器分類及基本公式

1、卡爾曼,濾波器,（,Kalman Filter,）,濾波的基本概念,?,濾波是什么？,所謂濾波，就是從混合在一起的諸多信號(hào)中提取出所需要的信號(hào),。,?,信號(hào)的分類（數(shù)學(xué)關(guān)系）？,（,1,）確定性信號(hào)：可以表示為確定的時(shí)間函數(shù)，可確定其在任何,時(shí)刻的量值。（,具有確定的頻譜,）,（,2,）隨機(jī)信號(hào)：不能用確定的數(shù)學(xué)關(guān)系式來(lái)描述的，不能預(yù)測(cè)其,未來(lái)任何瞬時(shí)值，其值的變化服從統(tǒng)計(jì)規(guī)律。（,頻譜不確定，功,率譜確定,）,濾波的基本概念,?,確定性信號(hào)的濾波,可采用低通、高通、帶通、帶阻等模擬濾波器或者計(jì)算機(jī)通,過(guò)算法實(shí)現(xiàn),常規(guī)濾波,?,隨機(jī)信號(hào)的濾波,根據(jù)有用信號(hào)和干擾信號(hào)的功率譜設(shè)計(jì)濾波器,維納濾波

2、,（,Wiener,Filtering,）或卡爾曼濾波（,Kalman,Filter,）,隨機(jī)信號(hào)的濾波也可以看做是,估計(jì)問(wèn)題。,卡爾曼濾波的由來(lái),卡爾曼濾波的由來(lái),卡爾曼，全名,Rudolf Emil Kalman,，,匈牙利數(shù)學(xué)家，,1930,年出生于匈牙利,首都布達(dá)佩斯。,1953,，,1954,年于麻省,理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩,士學(xué)位。,1957,年于哥倫比亞大學(xué)獲得,博士學(xué)位。我們?cè)诂F(xiàn)代控制理論中要,學(xué)習(xí)的卡爾曼濾波器，正是源于他的,博士論文和,1960,年發(fā)表的論文,A,New Approach to Linear Filtering and,Prediction Pro

3、blems,（線性濾波與預(yù),測(cè)問(wèn)題的新方法）。,卡爾曼濾波的由來(lái),卡爾曼濾波的由來(lái),卡爾曼濾波理論作為最優(yōu)估計(jì)的一種，它的創(chuàng)立是科學(xué)技術(shù)和,社會(huì)需要發(fā)展到一定程度的必然結(jié)果。在,1795,年，高斯為,測(cè)定行星,運(yùn)動(dòng)軌道,而提出,最小二乘估計(jì)法,。為了解決,火力控制系統(tǒng)精度跟蹤,問(wèn)題，維納于,1942,年提出了,維納濾波理論,，利用有用信號(hào)和干擾信,號(hào)的功率譜確定線性濾波器的頻率特性，,首次將數(shù)理統(tǒng)計(jì)理論與線,性理論有機(jī)的聯(lián)系在一起,，形成了對(duì)隨機(jī)信號(hào)做平滑、估計(jì)或者預(yù),測(cè)的最優(yōu)估計(jì)新理論。但是采用,頻域設(shè)計(jì)法,是造成維納濾波器設(shè)計(jì),困難的根本原因。于是，人們逐漸轉(zhuǎn)向?qū)で笤?時(shí)域內(nèi),直接設(shè)計(jì)最優(yōu)

4、,濾波器的方法，而卡爾曼研究的,卡爾曼濾波理論,很好的解決了這個(gè),問(wèn)題,卡爾曼濾波器是什么？,卡爾曼濾波器是什么,簡(jiǎn)單的說(shuō)，卡爾曼濾波器是一個(gè)“,optimal recursive data,processing algorithm (,最優(yōu)化自回歸數(shù)據(jù)處理算法,),”。從形,式上，卡爾曼濾波器是,5,條公式,。,對(duì)于解決很大部分的問(wèn)題，他是最優(yōu)，效率最高甚至,是最有用的。他的廣泛應(yīng)用已經(jīng)超過(guò)了,30,年，包括,機(jī)器人,導(dǎo)航、控制，傳感器數(shù)據(jù)融合,甚至在軍事方面的,雷達(dá)系統(tǒng),以及,導(dǎo)彈追蹤,等等。而近年來(lái)更被應(yīng)用于,計(jì)算機(jī)圖像處理,，,例如頭臉識(shí)別、圖像分割、圖像邊緣檢測(cè)等等。,卡爾曼濾波的特

5、點(diǎn),卡爾曼濾波的特點(diǎn),（,1,）卡爾曼濾波處理的對(duì)象是,隨機(jī)信號(hào),；,（,2,）被處理的信號(hào),無(wú)有用和干擾之分,，濾波的目的是要估計(jì)出,所有被處理的信號(hào),(,區(qū)別于維納濾波,),；,（,3,）系統(tǒng)的白噪聲激勵(lì)和測(cè)量噪聲并不是需要濾除的對(duì)象，它,們的統(tǒng)計(jì)特性是估計(jì)過(guò)程中需要利用的信息,；（,區(qū)別最小二乘,）,（,4,）算法是,遞推的，,且使用狀態(tài)空間法在,時(shí)域內(nèi),設(shè)計(jì)濾波器，,適用于對(duì),多維隨機(jī)過(guò)程,的估計(jì),；,（,5,）被估計(jì)量既可以,是平穩(wěn),的，也可以,是非平穩(wěn),的；,（,6,）估計(jì)過(guò)程中，只需要考慮,過(guò)程噪聲和測(cè)量噪聲,及,當(dāng)前時(shí)刻,系統(tǒng)狀態(tài)的統(tǒng)計(jì)特性,。（,計(jì)算機(jī)計(jì)算時(shí)，所占空間小,）,

6、思路,思路,Part 1,線性系統(tǒng)的卡爾曼濾波方程,I.,線性離散系統(tǒng),II.,線性連續(xù)系統(tǒng),Part 2,非線性系統(tǒng)的卡爾曼濾波方程,I.,擴(kuò)展卡爾曼濾波器,EKF,II.,無(wú)跡卡爾曼濾波器,UKF,卡爾曼濾波的基本思想,卡爾曼濾波的基本思想,在海圖作業(yè)中，航海長(zhǎng)通常,以前一時(shí)刻的船位為基準(zhǔn),，根據(jù)航向、,船速和海流等一系列因素,推算下一個(gè)船位,，但是他并不輕易認(rèn)為,船位就一定在推算船位上，還要選擇適當(dāng)?shù)姆椒?，通過(guò)儀器得到,另一個(gè)推算船位,。觀測(cè)和推算這兩個(gè)船位一般不重合，航海長(zhǎng)需,要通過(guò)分析和判斷,選擇一個(gè)可靠的船位,，作為船艦當(dāng)前的位置。,以,時(shí)刻的最優(yōu)估計(jì),為準(zhǔn)，預(yù)測(cè),時(shí)刻,的狀態(tài)變量

7、,，同時(shí)又對(duì)該狀態(tài)進(jìn)行,觀測(cè),，得到,觀測(cè)變量,，再在預(yù)測(cè)與觀測(cè)之間進(jìn)行分析，或者,說(shuō)是以觀測(cè)量對(duì)預(yù)測(cè)量進(jìn)行,修正,，從而得到,時(shí)刻的,最優(yōu)狀態(tài)估計(jì),。,1,k,x,?,1,K,?,/,1,?,k,k,x,?,k,z,k,x,K,K,卡,爾,曼,濾,波,思,想,例子,假設(shè)我們要研究一個(gè)房間的溫度，以一分鐘為時(shí)間單位。根,據(jù)我們的經(jīng)驗(yàn)判斷，這個(gè)房間的,溫度是恒定,的，但是對(duì)我們,的經(jīng)驗(yàn)不是完全相信，可能存在上下幾度的偏差，我們把該,偏差看做是高斯白噪聲。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，,溫度計(jì)也不準(zhǔn)確，測(cè)量值會(huì)與實(shí)際值存在偏差，我們也把這,偏差看做是高斯白噪聲?，F(xiàn)在，我們要根據(jù)我們的,經(jīng)驗(yàn)溫度,

8、和溫度計(jì)的,測(cè)量值,及它們,各自的噪聲,來(lái)估算出房間的實(shí)際溫,度。,卡爾曼濾波的基本方程,例子,假如我們要估算,k,時(shí)刻的實(shí)際溫度值。首先你要,根據(jù),k-1,時(shí)刻,的溫度值，來(lái)預(yù)測(cè),k,時(shí)刻的溫度（,K,時(shí)刻的經(jīng)驗(yàn)溫度）,。因?yàn)?你相信溫度是恒定的，所以你會(huì)得到,k,時(shí)刻的溫度預(yù)測(cè)值是跟,k-1,時(shí)刻一樣的，假設(shè)是,23,度（,*,公式一）,，同時(shí)該值,（預(yù)測(cè),值）的高斯噪聲的偏差是,5,度,（,5,是這樣得到的：如果,k-1,時(shí),刻估算出的最優(yōu)溫度值的偏差是,3,，你對(duì)自己預(yù)測(cè)的不確定度,是,4,度，他們平方相加再開(kāi)方，就是,5,（,*,公式二）,）,。然后，,你從溫度計(jì)那里得到了,k,時(shí)刻

9、的溫度值，假設(shè)是,25,度,，同時(shí)該,值的偏差是,4,度,。,卡爾曼濾波的基本方程,例子,卡爾曼濾波的基本方程,現(xiàn)在，我們用于估算,K,時(shí)刻房間的實(shí)際溫度有兩個(gè)溫度值：估計(jì)值,23,度和測(cè)量值,25,度。究竟實(shí)際溫度是多少呢？是相信自己還是相信,溫度計(jì)？,究竟相信誰(shuí)多一點(diǎn)？,我們需要用他們的均方誤差來(lái)判斷。,因?yàn)椋?（,*,公式三）,，所以我們可以,估算出,K,時(shí),刻的最優(yōu)溫度值,為：,度,（,*,公式四）,。,得到了,K,時(shí)刻的最優(yōu)溫度，下一步就是對(duì),K+1,時(shí)刻的溫度值進(jìn)行最,優(yōu)估算，需要得到,K,時(shí)刻的最優(yōu)溫度（,24.56,）的偏差，算法如下：,（,*,公式五）,就這樣，卡爾曼濾波器就

10、不斷的把均方誤差遞歸，從而估算出最優(yōu),的溫度值，運(yùn)行速度快，且,只保留上一時(shí)刻的協(xié)方差,。,2,2,2,2,5,0.78,5,4,H,H,?,?,?,?,23,0.78*(25,23),24.56,?,?,?,?,?,2,1,*5,2.35,H,?,?,無(wú)控制,離散型,卡爾曼濾波器的基本公式,系統(tǒng)的狀態(tài)方程,：,系統(tǒng)的測(cè)量方程,：,?,?,?,?,?,?,1,1,1,1,k,k,k,x,k,x,k,w,k,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,k,Z,k,C,x,k,v,k,?,?,?,?,?,1,w,k,?,?,?,v,k,為過(guò)程噪聲,；,為測(cè)量噪聲,;,如果,?,?,

11、1,w,k,?,?,?,v,k,?,?,?,?,0,;,0,;,0,k,k,j,k,kj,k,k,j,k,kj,k,j,E,w,Cov,w,w,Q,E,V,Cov,V,V,R,Cov,W,V,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,滿足,為過(guò)程噪聲的協(xié)方差，其為,非負(fù)定陣,；,為測(cè)量噪聲的協(xié)方差，其為,正定陣,。,k,Q,k,R,系統(tǒng)測(cè)量方程的輸出量,?,?,Z,k,是可以實(shí)際測(cè)量的量。,?,為噪聲驅(qū)動(dòng)陣,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.1,無(wú)控制的離散型卡爾曼濾波基本方程,無(wú)控制,離散型,卡爾曼濾波的基本方程,/,1,1,1,?,k,k,k

12、,k,k,x,x,?,?,?,?,?,?,/,1,1,1,1,1,1,1,?,T,T,k,k,k,k,k,k,k,k,k,k,P,P,Q,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,1,/,1,/,1,?,?,T,T,k,k,k,k,k,k,k,k,k,H,P,C,C,P,C,R,?,?,?,?,?,?,?,?,?,?,?,?,?,?,/,1,/,1,?,?,k,k,k,k,k,k,k,k,x,x,H,Z,C,x,?,?,?,?,?,?,?,?,/,1,?,k,k,k,k,k,P,I,H,C,P,?,?,?,?,?,（,1,）狀態(tài)的一步預(yù)測(cè)方程：,（,2,）均方誤差的一步預(yù)測(cè)：

13、,（,3,）濾波增益方程（權(quán)重）：,（,4,）濾波估計(jì)方程（,K,時(shí)刻的最優(yōu)值）：,（,5,）均方誤差更新矩陣（,K,時(shí)刻的最優(yōu)均方誤差）,：,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.1,無(wú)控制的離散型卡爾曼濾波基本方程,帶有控制的離散型卡爾曼濾波基本方程,系統(tǒng)的狀態(tài)方程,：,系統(tǒng)的測(cè)量方程,：,1,1,1,1,1,1,k,k,k,k,k,k,k,k,x,x,w,B,u,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,k,Z,k,C,x,k,v,k,?,?,如果,?,?,1,w,k,?,?,?,v,k,?,?,?,?,0,;,0,;,0,k,k,j,k,kj,k,k

14、,j,k,kj,k,j,E,W,Cov,W,W,Q,E,V,Cov,V,V,R,Cov,W,V,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,滿足,為過(guò)程噪聲的協(xié)方差，其為,非負(fù)定陣,；,為測(cè)量噪聲的協(xié)方差，其為,正定陣,。,k,Q,k,R,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.2,帶有控制的離散型卡爾曼濾波基本方程,/,1,1,1,1,1,?,k,k,k,k,k,k,k,x,x,B,u,?,?,?,?,?,?,?,?,?,/,1,1,1,1,1,1,1,?,T,T,k,k,k,k,k,k,k,k,k,k,P,P,Q,?,?,?,?,?,?,?,?,?,

15、?,?,?,?,?,?,1,/,1,/,1,?,?,T,T,k,k,k,k,k,k,k,k,k,H,P,C,C,P,C,R,?,?,?,?,?,?,?,?,?,?,?,?,?,?,/,1,/,1,1,1,?,?,k,k,k,k,k,k,k,k,k,k,k,x,x,H,Z,C,x,C,B,u,?,?,?,?,?,?,?,?,?,?,?,/,1,?,k,k,k,k,k,P,I,H,C,P,?,?,?,?,?,（,1,）狀態(tài)的一步預(yù)測(cè)方程：,（,2,）均方誤差的一步預(yù)測(cè)：,（,3,）濾波增益方程（權(quán)重）：,（,4,）濾波估計(jì)方程（,K,時(shí)刻的最優(yōu)值）：,（,5,）濾波均方誤差更新矩陣（,K,時(shí)刻的最

16、優(yōu)均方誤差）,：,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.2,帶有控制的離散型卡爾曼濾波基本方程,帶有控制的離散型卡爾曼濾波基本方程,線性離散型卡爾曼濾波方程的,一般形式,系統(tǒng)方程和測(cè)量方程的一般形式：,1,1,1,1,1,1,k,k,k,k,k,k,k,k,k,k,k,k,x,x,B,u,w,Z,C,x,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,0,;,0,;,k,k,j,k,kj,k,k,j,k,kj,k,j,k,kj,E,w,Cov,w,w,Q,E,v,Cov,v,v,R,Cov,w,v,S,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?

17、,?,?,?,?,?,如果,1,k,w,?,k,v,滿足,為過(guò)程噪聲的協(xié)方差，其為,非負(fù)定陣,；,為測(cè)量噪聲的協(xié)方差，其為,正定陣,。,k,Q,k,R,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.3,離散型卡爾曼濾波方程的一般形式,*,*,1,1,1,1,1,1,1,;,k,k,k,k,k,k,k,k,k,k,k,k,k,x,x,B,u,J,Z,w,Z,C,x,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,*,*,1,1,1,1,1,1,1,1,1,;,k,k,k,k,k,k,k,k,k,k,k,J,C,w,w,J,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,

18、?,?,引入矩陣,，對(duì)狀態(tài)方程進(jìn)行等效變換：,1,k,k,k,k,J,S,R,?,?,?,其中：,?,?,*,*,*,0,;,0,;,0,k,k,j,k,kj,k,k,j,k,kj,k,j,E,w,Cov,w,w,Q,E,v,Cov,v,v,R,Cov,w,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,為過(guò)程噪聲的協(xié)方差，其為,非負(fù)定陣,；,為測(cè)量噪聲的協(xié)方差，其為,正定陣,。,k,Q,k,R,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.3,離散型卡爾曼濾波方程的一般形式,一般形式的卡爾曼濾波方程,*,/,1,1,1,1,1,1,1,?,k

19、,k,k,k,k,k,k,k,k,x,x,B,u,J,Z,?,?,?,?,?,?,?,?,?,?,?,?,*,*,/,1,1,1,1,1,1,1,1,1,1,?,(,),T,T,T,T,k,k,k,k,k,k,k,k,k,k,k,k,k,P,P,Q,J,S,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,1,/,1,/,1,?,?,T,T,k,k,k,k,k,k,k,k,k,H,P,C,C,P,C,R,?,?,?,?,?,?,?,?,?,?,?,?,?,?,/,1,1,1,1,1,/,1,1,1,?,?,k,k,k,k,k,k,k,k,k,k,k,k,k,k,k

20、,x,x,B,u,J,Z,H,Z,C,x,C,B,u,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,/,1,?,k,k,k,k,k,P,I,H,C,P,?,?,?,?,?,（,1,）狀態(tài)的一步預(yù)測(cè)方程：,（,2,）均方誤差的一步預(yù)測(cè)：,（,3,）濾波增益方程（權(quán)重）：,（,4,）濾波估計(jì)方程（,K,時(shí)刻的最優(yōu)值）：,（,5,）濾波均方誤差更新矩陣（,K,時(shí)刻的最優(yōu)均方誤差）,：,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.3,離散型卡爾曼濾波方程的一般形式,離散型卡爾曼濾波基本方程使用要點(diǎn),（,1,）,濾波初值的選取,卡爾曼濾波是一種遞推算法，啟動(dòng)時(shí)必須先給初值,情況

21、一,：一般情況下，取,，卡爾曼濾,波器是無(wú)偏的，即,濾波穩(wěn)定,，但是實(shí)際上這樣的初值很難得到；,情況二,：如果系統(tǒng)是一致完全隨機(jī)可控和一致完全隨機(jī)可觀測(cè),的，則卡爾曼濾波器一定是一致漸近穩(wěn)定的，此時(shí)盲目的選取,濾波初值不影響最終估計(jì)值（大多數(shù)情況下）。,0,0,?,x,P,?,?,?,?,0,0,0,0,?,x,E,x,P,Cov,x,?,?,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.4,離散型卡爾曼濾波基本方程使用要點(diǎn),離散型卡爾曼濾波基本方程使用要點(diǎn),（,2,）,估計(jì)均方誤差的等價(jià)形式及選用,?,?,?,?,?,?,?,?,/,1,/,1,1,1,1,/,1,?,-,;,(1),?,

22、;,(2),?,(3),k,k,k,k,k,T,T,k,k,k,k,k,k,k,k,k,k,T,k,k,k,k,k,k,P,I,H,C,P,P,I,H,C,P,I,H,C,H,R,H,P,P,C,R,C,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,公式（,1,）形式簡(jiǎn)單，計(jì)算量小，但是積累誤差容易使協(xié),方差矩陣失去非負(fù)定性甚至對(duì)稱性，所以實(shí)際中常使用,公式（,2,）；,如果在濾波初值對(duì)被估計(jì)量的統(tǒng)計(jì)特性缺乏了解，選取,濾波初值盲目，則宜采用公式（,

23、3,）。,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.4,離散型卡爾曼濾波基本方程使用要點(diǎn),離散型卡爾曼濾波基本方程使用要點(diǎn),（,3,）,連續(xù)系統(tǒng)離散化,卡爾曼濾波的基本方程只適用于系統(tǒng)方程和測(cè)量方程均為,離散,的情況，但實(shí)際的物理系統(tǒng)一般都是連續(xù)的，動(dòng)力學(xué)特,性用,連續(xù)微分方程,來(lái)描述，所以在使用基本方程之前，需要,對(duì)系統(tǒng)方程和測(cè)量方程進(jìn)行,離散化,處理。,連續(xù)系統(tǒng)的離散化處理包括對(duì),過(guò)程白噪聲,的等效離散化處,理。,1,基于離散系統(tǒng)模型的卡爾曼濾波的基本公式,1.4,離散型卡爾曼濾波基本方程使用要點(diǎn),連續(xù)系統(tǒng)的卡爾曼濾波基本方程,通過(guò)對(duì)實(shí)際的物理系統(tǒng)進(jìn)行分析后得到的系統(tǒng)模型一般為,連續(xù)

24、型的。連續(xù)型卡爾曼濾波方程可在離散型卡爾曼濾波器基,本方程的基礎(chǔ)上推導(dǎo)出來(lái)?；舅悸罚?將連續(xù)系統(tǒng)離散化，應(yīng),用離散型卡爾曼濾波器的基本方程和導(dǎo)數(shù)概念推導(dǎo)出連續(xù)型濾,波方程,。,采用遞推算法是離散型卡爾曼濾波的最大優(yōu)點(diǎn)，算法可由,計(jì)算機(jī)執(zhí)行，不必存儲(chǔ)時(shí)間過(guò)程中得大量測(cè)量信息。連續(xù)型卡,爾曼濾波則根據(jù)連續(xù)時(shí)間過(guò)程中的測(cè)量值，采用,求解矩陣微分,方程,的方法估計(jì)系統(tǒng)狀態(tài)變量的時(shí)間連續(xù)值，因此算法,失去了,遞推性,。,2,基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式,連續(xù)系統(tǒng)的狀態(tài)空間表達(dá)式為,：,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,x,t,A,t,x,

25、t,B,t,u,t,G,t,w,t,z,t,C,t,x,t,v,t,?,?,?,?,?,0,;,(,),E,w,t,Cov,w,t,w,Q,t,t,E,v,t,Cov,v,t,v,R,t,t,Cov,w,t,v,S,t,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,Q,t,為非負(fù)定矩陣；,?,?,R,t,為正定陣,其中：,2,基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式,連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程,2,基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式,與連續(xù)系統(tǒng)模型等效的離散系統(tǒng)的數(shù)學(xué)模型：

26、,連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,x,t,t,t,t,t,x,t,t,t,t,u,t,t,t,t,W,t,z,t,t,C,t,t,x,t,t,V,t,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,t,t,t,I,A,t,t,t,t,t,t,B,t,t,t,t,t,t,G,t,t,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,

27、其中,：,?,?,?,?,W,t,V,t,是零均值分段常值白噪聲過(guò)程，其協(xié)方差為：,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,0,0,;,;,;,;,0,1,2,kj,kj,kj,Q,t,R,t,Cov,W,t,W,Cov,V,t,V,t,t,S,t,Cov,W,t,V,t,t,k,t,t,j,t,k,j,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,L,連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程,?,?,?,?,?,?,?,?,1,J,t,t,t,t,S,t,R,t,?,?,?,?,?,

28、引入矩陣,J,來(lái)去除過(guò)程噪聲與測(cè)量噪聲的相關(guān)性,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,x,t,t,t,t,t,t,x,t,t,t,t,u,t,J,t,z,t,C,t,x,t,?,?,?,?,?,?,?,?,?,?,?,?,（,1,）狀態(tài)的一步預(yù)測(cè)方程：,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,T,T,T,P,t,t,t,t,t,t,J,t,C,t,P,t,t,t,t,J,t,C,t,Q,t,R,t,t,t,t,t,t,t,J,t,J,t,t,t,?,?,?,?,?,?,?,

29、?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,（,2,）均方誤差的一步預(yù)測(cè)：,?,?,?,?,?,?,?,?,?,?,?,?,?,?,1,?,?,T,T,R,t,H,t,t,P,t,t,t,C,t,t,C,t,t,P,t,t,t,C,t,t,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,（,3,）濾波增益方程（權(quán)重）：,連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程,2,基于連續(xù)系統(tǒng)模型的卡爾曼濾波的基本公式,?,?,?,?,?,?,?,?,P,t,t,I,H,t,t,C,t,t,P,t,t,t,?,?,?,?,?,?,?,

30、?,?,?,?,?,?,?,（,4,）濾波估計(jì)方程（,K,時(shí)刻的最優(yōu)值）：,（,5,）濾波均方誤差更新矩陣（,K,時(shí)刻的最優(yōu)均方誤差）,：,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,1,1,x,t,t,x,t,t,t,H,t,t,z,t,t,t,x,t,A,t,x,t,B,t,u,t,G,t,S,t,R,t,z,t,C,t,x,t,t,H,t,t,z,t,t,C,t,t,x,t,A,

31、t,x,t,B,t,u,t,G,t,S,t,R,t,z,t,C,t,x,t,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,%,將其變形求極限,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,1,s,s,x,t,A,t,x,t,B,t,u,t,H,t,z,t,H,t,x,t,H,t,H,t,G,t,S,t,R,t,?,?,?,?,?,?,?,?,?,?,?,0,;,0,E,w,t,Cov,w,t,w,Q,t,t,E,v,

32、t,Cov,v,t,v,R,t,t,Cov,w,t,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,其中：,假設(shè)在,時(shí)刻已獲得系統(tǒng)狀態(tài),的濾波估計(jì),，將,和,在,附近線性化，即,非線性系統(tǒng)將隨時(shí)在新,估計(jì)的結(jié)果附近進(jìn)行線性化,。,t,x,?,?,?,?,f,x,t,t,?,?,?,?,C,x,t,t,?,?,?,x,t,?,?,?,x,t,3,非線性系統(tǒng)的卡爾曼濾波方程,3.1,擴(kuò)展卡爾曼濾波器,擴(kuò)展卡爾曼濾波器（,EKF,）,將,和,在,附近展開(kāi)成泰勒級(jí)數(shù)，,忽略二階以上的高階項(xiàng)，則得線性化方程為：,

33、?,?,?,?,f,x,t,t,?,?,?,?,C,x,t,t,?,?,?,?,?,x,t,x,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,f,x,t,t,x,t,f,x,t,t,x,t,x,t,g,x,t,t,w,t,x,t,C,x,t,t,z,t,h,x,t,t,x,t,x,t,v,t,x,t,?,?,?,?,?,?,?,?,?,?,?,?,;,?,?,?,?,?,;,?,?,?,?,?,f,x,t,t,C,x,t,t,F,t,H,

34、t,G,t,g,x,t,t,x,t,x,t,f,x,t,t,u,t,f,x,t,t,x,t,x,t,C,x,t,t,y,t,h,x,t,t,x,t,x,t,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,(,),(,),x,t,F,t,x,t,u,t,G,t,w,t,z,t,H,t,x,t,y,t,v,t,?,?,?,?,?,?,&,將其變形，取,非線性系統(tǒng)線性化后的系統(tǒng)狀態(tài)空間表達(dá)式為：,3,非線性系統(tǒng)的卡爾曼濾波方程,3.1,擴(kuò)展卡爾曼濾波器,EKF,基本方程,3,非線性系統(tǒng)的卡爾曼濾波方程,3.1,擴(kuò)展卡

35、爾曼濾波器,系統(tǒng)模型,測(cè)量模型,初始條件,其他規(guī)定,狀態(tài)估計(jì)方程,誤差協(xié)方差,增益矩陣,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,0,0,x,t,f,x,t,t,g,x,t,t,w,t,w,t,N,Q,t,z,t,C,x,t,t,v,t,v,t,N,R,t,?,?,?,?,?,?,?,?,?,?,?,?,&,?,?,?,?,?,?,?,?,0,0,?,0,x,t,N,x,P,Cov,w,t,v,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,x,t,F,x,t,t,K,t,z,t,

36、C,x,t,t,?,?,?,?,?,?,?,&,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,?,T,T,T,P,t,F,x,t,t,P,t,P,t,F,x,t,t,G,t,Q,t,G,t,K,t,R,t,K,t,?,?,?,?,&,?,?,?,?,?,?,?,?,?,?,1,?,T,K,t,P,t,H,x,t,t,R,t,?,?,EKF,的不足,(1),當(dāng)非線性函數(shù)的,Taylor,展開(kāi)式的高階項(xiàng)無(wú)法忽略時(shí)，,線,性化會(huì)使系統(tǒng)產(chǎn)生較大的誤差，甚至于濾波器難以穩(wěn)定；,(2),在許多實(shí)際問(wèn)題中很難得到非線性函數(shù)的雅克比矩陣求,導(dǎo)；,(3)EKF,需要求導(dǎo)，所以必須清楚了解非線性函數(shù)的具體形,式，無(wú)法作到黑盒封裝，從而難以模塊化應(yīng)用。,3,非線性系統(tǒng)的卡爾曼濾波方程,3.1,擴(kuò)展卡爾曼濾波器,（,EKF),3,非線性系統(tǒng)的卡爾曼濾波方程