1、卡爾曼濾波算法介紹,目錄 Contents,狀態(tài)估計原理簡介,二,卡爾曼濾波算法數(shù)學推導,四,卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,五,濾波算法簡介,一,卡爾曼濾波引例溫度測量,三,目 錄,一：濾波簡介,濾波：通過一定的算法將信號中特定波段頻率濾除，排除可能的隨機干擾，提高檢測精度的一種手段。 功能: 平滑、預測，微分、積分、信號分離和噪聲抑制等功能。,信號種類,數(shù)字濾波：使用軟件編程/可編程邏輯器件設計,模擬濾波：采用電容，電阻和電感的組合來完成。,算法 (頻域/時域),經(jīng)典濾波：信號和噪聲處于不同的頻帶。 高通、低通、帶通、帶阻濾波器。,現(xiàn)代濾波：利用信號和噪聲的隨機統(tǒng)計特性。 維納濾

2、波，Kalman濾波，自適應濾波，小波變換等,二：狀態(tài)估計原理簡介,狀態(tài)估計是卡爾曼濾波的重要組成部分。,定量判斷,隨機狀態(tài)量 (很難直接得到),觀測數(shù)據(jù) (可以直接得到),估計問題：,例如，飛機實時的位置、速度等狀態(tài)參數(shù)需要通過雷達或其它 測量裝置進行觀測，而雷達等測量裝置也存在隨機干擾，因此在觀測到飛機的位置、速度等信號中就夾雜著隨機干擾，要想正確地得到飛機的狀態(tài)參數(shù)是不可能的，只能根據(jù)觀測到的信號來估計和預測飛機的狀態(tài)。,二：狀態(tài)估計原理簡介,定量判斷,內部狀態(tài)量 (動態(tài)規(guī)律，很難直接得到),觀測數(shù)據(jù) (輸入輸出： 外部特性),狀態(tài)估計：,狀態(tài)估計對于了解和控制一個系統(tǒng)具有重要意義。,信

3、號平滑/插值（過去） 信號的濾波（現(xiàn)在） 信號的預測（將來）,隨機過程、噪聲等影響,二：狀態(tài)估計原理簡介,最優(yōu)估計 估計誤差 無偏估計,卡爾曼濾波算法即為遞推最優(yōu)估計理論，采用狀態(tài)空間描述法，以線性最小均方誤差為估計準則來得到對狀態(tài)變量的最優(yōu)估計。,狀態(tài)估計方法：最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。,估計值,真 實 值,三：卡爾曼濾波引例,背景介紹： Kalman，匈牙利數(shù)學家。 卡爾曼濾波器源于他的博士論文和1960年 發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems （線性濾波與預測

4、問題的新方法）。 卡爾曼將狀態(tài)變量引入慮波理論，提出了遞推濾波算法，建立了后來被自動控制界稱道的“卡爾曼濾波”。,三：卡爾曼濾波引例,實質：由量測值重構系統(tǒng)的狀態(tài)向量。它以“預測實測修正”的順序遞推，根據(jù)量測值來消除隨機干擾，再現(xiàn)系統(tǒng)的狀態(tài)。,卡爾曼濾波：是一種高效率的遞歸濾波器(自回歸濾波器) ,它能夠從一系列完全包含噪聲的測量中， 估計動態(tài)系統(tǒng)的狀態(tài)。,基本思想：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)在時刻的估計值。它適合于實時處理和計算機運算。,無需歷史數(shù)據(jù),三：卡爾曼濾波引例,1.被建模的系統(tǒng)是線性的：K時刻的系統(tǒng)狀態(tài)可以用某個

5、矩陣與K-1時刻的系統(tǒng)狀態(tài)的乘積表示。,2.影響測量的噪聲屬于高斯分布的白噪聲：噪聲與時間 不相關，且只用均值和協(xié)方差就可以準確地建模。,這些假設實際上可以運用在非常廣泛的普通環(huán)境中。,卡爾曼濾波器的兩個重要假設：,三：卡爾曼濾波引例,10,房間溫度的當前感覺值,房間溫度計的當前讀數(shù),五分鐘以后房間溫度的實際值,問題描述,已知條件,希望得到,都帶有誤差,感覺值 + 測量值,未來時刻的真實值,？,=,三：卡爾曼濾波引例,預測：根據(jù)K-1時刻溫度的最優(yōu)估計值預測K時刻的溫度為23度，其高斯噪聲的偏差是5度（設k 1時刻溫度的最優(yōu)估算的偏差是3， 自己預測的不確定度是4度，它們平方相加再開方，就是5

6、）。,觀測：從溫度計那里得到k時刻的溫度值， 假設是25度，同時該值的偏差是4度。,三：卡爾曼濾波引例,更新：根據(jù)預測誤差和觀測誤差的協(xié)方差有Kg2=52/(52+42)，即Kg=0.78，則可估算出k時刻的實際溫度值（最優(yōu)估計）是： 23+0 78. 78 (25* (25 23)=24.56度。可以看出，因為溫度計的covariance比較?。ū容^相信溫度計）， 所以估算出的最優(yōu)溫度值偏向溫度計的值。 遞推關鍵：由此可知，進行K+1時刻的最優(yōu)估計，需要K時刻的最優(yōu)估計值和其偏差。偏差計算： (1 Kg) * 5 2) 0.5=2.35。這里的5就是上面k時刻溫度預測為23度時的偏差，得出的

7、2.35就是k時刻估算出的最優(yōu)溫度值的偏差（對應于上面的3）。 卡爾曼濾波器不斷的把covariance遞歸， 從而估算出最優(yōu)的溫度值。其運行的很快，而且它只保留了上一時刻的covariance。,三：卡爾曼濾波引例,四：卡爾曼濾波算法數(shù)學推導,引入一個離散控制過程的系統(tǒng)。 該系統(tǒng)可用一個線性隨機微分方程來描述： X(k)=F X(k-1)+B U(k)+W(k) 加上系統(tǒng)的測量值： Z(k)=H X(k)+V(k),X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。 F和B是系統(tǒng)參數(shù),對于多模型系統(tǒng)，他們?yōu)榫仃?。Z(k)是k時刻的測量值， H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩

8、陣。 W(k)和V(k)分別表示過程和測量的噪聲。它們被假設成高斯白噪聲，它們的協(xié)方差分別是Q，R(這里假設它們不隨系統(tǒng)狀態(tài)變化而變化)。,系統(tǒng)可用一個馬爾可夫鏈表示，該馬爾可夫鏈建立在一個被高斯噪聲干擾的線性算子上的。,卡爾曼濾波器的模型,四：卡爾曼濾波算法數(shù)學推導,圓圈代表向量 方塊代表矩陣 星號代表高斯噪聲,四：卡爾曼濾波算法數(shù)學推導,x(k-1),k時刻,k+1時刻,k-1時刻,z(k),x(k|k),u(k),四：卡爾曼濾波算法數(shù)學推導,x(k+1)=F x(k)+B u(k)+w(k),k時刻,k+1,k-1,x(k|k-1)=F x(k-1|k-1)+B u(k)+w(k),x(

9、k-1|k-1),P(k|k-1)=F P(k-1|k-1) F+Q,預測,四：卡爾曼濾波算法數(shù)學推導,18,k時刻,k+1,k-1,x(k|k),至此，完成了預測階段 得到了兩個量： k時刻的狀態(tài)預測x(k|k-1) 描述x(k|k-1)優(yōu)劣程度的協(xié)方差P(k|k-1),預測,四：卡爾曼濾波算法數(shù)學推導,19,z(k)= H x(k|k-1)+v(k),K時刻,k+1,k-1,測量值z(k),x(k+1|k+1),x(k|k-1),P(k|k-1),實測,四：卡爾曼濾波算法數(shù)學推導,20,K時刻,k+1時刻,k-1,x(k|k-1),P(k|k-1),x(k|k)=x(k|k-1) + Kg

10、(k) (Z(k)-H X(k|k-1),Kg(k)=P(k|k-1)H/(HP(k|k-1) H +R),修正,P(k|k)=(I-Kg(k)H)P(k|k-1),x(k+1|k+1),四：卡爾曼濾波算法數(shù)學推導,時間更新（預測） （1）計算先驗狀態(tài)估計值 x(k|k-1)=F x(k-1|k-1)+B u(k)+w(k) （2）計算先驗狀態(tài)估計值的協(xié)方差 P(k|k-1)=F P(k-1|k-1) F+Q,測量更新（修正） （1）計算加權矩陣(卡爾曼增益) Kg(k)=P(k|k-1)H/(HP(k|k-1) H +R) （2）對預測值進行修正 x(k|k)=x(k|k-1) + Kg(k

11、) (Z(k)-H X(k|k-1) （3）更新修正值的協(xié)方差 P(k|k)=(I-Kg(k)H)P(k|k-1),初始值x(0)、P(0),四：卡爾曼濾波算法數(shù)學推導,針對上述不足，很多學者提出了不同的方法加以克服，如限定記憶法、平方根濾波、漸消記憶濾波、自適應卡爾曼濾波（AKF）、抗野值濾波等。其中，AKF因為具有自適應特性非常適合動態(tài)系統(tǒng)濾波而受到廣泛重視。因此，在采用卡爾曼濾波處理動態(tài)測量數(shù)據(jù)時，一般都要考慮采取適當?shù)淖赃m應濾波方法來解決這一問題。,卡爾曼濾波的發(fā)展,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,23,應用場合,機器人導航、控制 傳感器數(shù)據(jù)融合 雷達系統(tǒng)以及導彈追蹤 計

12、算機圖像處理 頭臉識別 圖像分割 圖像邊緣檢測,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,多傳感器數(shù)據(jù)融合：將經(jīng)過集成處理的多種傳感器信息進行合成，形成對被測對象及其性質的最佳一致估計。,提高信息的準確性和全面性。比單一傳感器獲得有關周圍環(huán)境更準確、全面的信息。 降低信息的不確定性。一組傳感器采集的信息存在互補性，可以對單一傳感器的不確定性和測量范圍的局限性進行補償。 提高系統(tǒng)的可靠性，某個或幾個傳感器失效時，系統(tǒng)仍能正常運行； 增加系統(tǒng)的實時性。,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,數(shù)據(jù)融合的模型: (a)集中式融合系統(tǒng); (b)無反饋式分布融合系統(tǒng); (c)有反饋式分布融合系

13、統(tǒng); (d)有反饋的全并行系統(tǒng),(a)集中式融合系統(tǒng),集中式融合系統(tǒng)可利用所有傳感器的信息進行狀態(tài)估計、速度估計和預測計算。,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,(b)無反饋式分布融合系統(tǒng),各傳感器分別濾波，將各局部狀態(tài)估計送給融合中心進行融合,最后給出融合結果。,數(shù)據(jù)融合的模型: (a)集中式融合系統(tǒng); (b)無反饋式分布融合系統(tǒng); (c)有反饋式分布融合系統(tǒng); (d)有反饋的全并行系統(tǒng),五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,(c)有反饋式分布融合系統(tǒng),融合中心到各傳感器有反饋通道，提高各傳感器狀態(tài)估計和預測精度。,數(shù)據(jù)融合的模型: (a)集中式融合系統(tǒng); (b)無反饋式分

14、布融合系統(tǒng); (c)有反饋式分布融合系統(tǒng); (d)有反饋的全并行系統(tǒng),五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,(d)有反饋的全并行系統(tǒng),數(shù)據(jù)融合的模型: (a)集中式融合系統(tǒng); (b)無反饋式分布融合系統(tǒng); (c)有反饋式分布融合系統(tǒng); (d)有反饋的全并行系統(tǒng),最復雜,但也是最有潛力的融合系統(tǒng)。,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,以跟蹤雷達為背景,它配有電視角跟蹤儀、激光測距器等傳感器，通過聯(lián)合卡爾曼濾波實現(xiàn)其數(shù)據(jù)融合。,濾波結構框圖,1.雷達、電視局部卡爾曼濾波，分別得到各局部最優(yōu)估計 。 2.將全部局部最優(yōu)估計送到融合中心進行全局融合。,3.融合中心按照“信息分配”原則

15、形成的信息分配量，向雷達與電視進行信息反饋。,謝 謝,目錄 Contents,狀態(tài)估計原理簡介,二,卡爾曼濾波算法數(shù)學推導,四,卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,五,濾波算法簡介,一,卡爾曼濾波引例溫度測量,三,一：濾波簡介,濾波：通過一定的算法將信號中特定波段頻率濾除，排除可能的隨機干擾，提高檢測精度的一種手段。 功能: 平滑、預測，微分、積分、信號分離和噪聲抑制等功能。,信號種類,數(shù)字濾波：使用軟件編程/可編程邏輯器件設計,模擬濾波：采用電容，電阻和電感的組合來完成。,算法 (頻域/時域),經(jīng)典濾波：信號和噪聲處于不同的頻帶。 高通、低通、帶通、帶阻濾波器。,現(xiàn)代濾波：利用信號和噪

16、聲的隨機統(tǒng)計特性。 維納濾波，Kalman濾波，自適應濾波，小波變換等,二：狀態(tài)估計原理簡介,狀態(tài)估計是卡爾曼濾波的重要組成部分。,定量判斷,隨機狀態(tài)量 (很難直接得到),觀測數(shù)據(jù) (可以直接得到),估計問題：,例如，飛機實時的位置、速度等狀態(tài)參數(shù)需要通過雷達或其它 測量裝置進行觀測，而雷達等測量裝置也存在隨機干擾，因此在觀測到飛機的位置、速度等信號中就夾雜著隨機干擾，要想正確地得到飛機的狀態(tài)參數(shù)是不可能的，只能根據(jù)觀測到的信號來估計和預測飛機的狀態(tài)。,二：狀態(tài)估計原理簡介,定量判斷,內部狀態(tài)量 (動態(tài)規(guī)律，很難直接得到),觀測數(shù)據(jù) (輸入輸出： 外部特性),狀態(tài)估計：,狀態(tài)估計對于了解和控制

17、一個系統(tǒng)具有重要意義。,信號平滑/插值（過去） 信號的濾波（現(xiàn)在） 信號的預測（將來）,隨機過程、噪聲等影響,二：狀態(tài)估計原理簡介,最優(yōu)估計 估計誤差 無偏估計,卡爾曼濾波算法即為遞推最優(yōu)估計理論，采用狀態(tài)空間描述法，以線性最小均方誤差為估計準則來得到對狀態(tài)變量的最優(yōu)估計。,狀態(tài)估計方法：最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。,估計值,真 實 值,三：卡爾曼濾波引例,背景介紹： Kalman，匈牙利數(shù)學家。 卡爾曼濾波器源于他的博士論文和1960年 發(fā)表的論文A New Approach to Linear Filtering and Prediction Prob

18、lems （線性濾波與預測問題的新方法）。 卡爾曼將狀態(tài)變量引入慮波理論，提出了遞推濾波算法，建立了后來被自動控制界稱道的“卡爾曼濾波”。,三：卡爾曼濾波引例,實質：由量測值重構系統(tǒng)的狀態(tài)向量。它以“預測實測修正”的順序遞推，根據(jù)量測值來消除隨機干擾，再現(xiàn)系統(tǒng)的狀態(tài)。,卡爾曼濾波：是一種高效率的遞歸濾波器(自回歸濾波器) ,它能夠從一系列完全包含噪聲的測量中， 估計動態(tài)系統(tǒng)的狀態(tài)。,基本思想：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)在時刻的估計值。它適合于實時處理和計算機運算。,無需歷史數(shù)據(jù),三：卡爾曼濾波引例,1.被建模的系統(tǒng)是線性的：

19、K時刻的系統(tǒng)狀態(tài)可以用某個矩陣與K-1時刻的系統(tǒng)狀態(tài)的乘積表示。,2.影響測量的噪聲屬于高斯分布的白噪聲：噪聲與時間 不相關，且只用均值和協(xié)方差就可以準確地建模。,這些假設實際上可以運用在非常廣泛的普通環(huán)境中。,卡爾曼濾波器的兩個重要假設：,三：卡爾曼濾波引例,39,房間溫度的當前感覺值,房間溫度計的當前讀數(shù),五分鐘以后房間溫度的實際值,問題描述,已知條件,希望得到,都帶有誤差,感覺值 + 測量值,未來時刻的真實值,？,=,三：卡爾曼濾波引例,預測：根據(jù)K-1時刻溫度的最優(yōu)估計值預測K時刻的溫度為23度，其高斯噪聲的偏差是5度（設k 1時刻溫度的最優(yōu)估算的偏差是3， 自己預測的不確定度是4度，

20、它們平方相加再開方，就是5）。,觀測：從溫度計那里得到k時刻的溫度值， 假設是25度，同時該值的偏差是4度。,三：卡爾曼濾波引例,更新：根據(jù)預測誤差和觀測誤差的協(xié)方差有Kg2=52/(52+42)，即Kg=0.78，則可估算出k時刻的實際溫度值（最優(yōu)估計）是： 23+0 78. 78 (25* (25 23)=24.56度。可以看出，因為溫度計的covariance比較?。ū容^相信溫度計）， 所以估算出的最優(yōu)溫度值偏向溫度計的值。 遞推關鍵：由此可知，進行K+1時刻的最優(yōu)估計，需要K時刻的最優(yōu)估計值和其偏差。偏差計算： (1 Kg) * 5 2) 0.5=2.35。這里的5就是上面k時刻溫度預

21、測為23度時的偏差，得出的2.35就是k時刻估算出的最優(yōu)溫度值的偏差（對應于上面的3）。 卡爾曼濾波器不斷的把covariance遞歸， 從而估算出最優(yōu)的溫度值。其運行的很快，而且它只保留了上一時刻的covariance。,三：卡爾曼濾波引例,四：卡爾曼濾波算法數(shù)學推導,引入一個離散控制過程的系統(tǒng)。 該系統(tǒng)可用一個線性隨機微分方程來描述： X(k)=F X(k-1)+B U(k)+W(k) 加上系統(tǒng)的測量值： Z(k)=H X(k)+V(k),X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。 F和B是系統(tǒng)參數(shù),對于多模型系統(tǒng)，他們?yōu)榫仃嚒(k)是k時刻的測量值， H是測量系統(tǒng)的參

22、數(shù)，對于多測量系統(tǒng)，H為矩陣。 W(k)和V(k)分別表示過程和測量的噪聲。它們被假設成高斯白噪聲，它們的協(xié)方差分別是Q，R(這里假設它們不隨系統(tǒng)狀態(tài)變化而變化)。,系統(tǒng)可用一個馬爾可夫鏈表示，該馬爾可夫鏈建立在一個被高斯噪聲干擾的線性算子上的。,卡爾曼濾波器的模型,四：卡爾曼濾波算法數(shù)學推導,圓圈代表向量 方塊代表矩陣 星號代表高斯噪聲,四：卡爾曼濾波算法數(shù)學推導,x(k-1),k時刻,k+1時刻,k-1時刻,z(k),x(k|k),u(k),四：卡爾曼濾波算法數(shù)學推導,x(k+1)=F x(k)+B u(k)+w(k),k時刻,k+1,k-1,x(k|k-1)=F x(k-1|k-1)+B

23、 u(k)+w(k),x(k-1|k-1),P(k|k-1)=F P(k-1|k-1) F+Q,預測,四：卡爾曼濾波算法數(shù)學推導,47,k時刻,k+1,k-1,x(k|k),至此，完成了預測階段 得到了兩個量： k時刻的狀態(tài)預測x(k|k-1) 描述x(k|k-1)優(yōu)劣程度的協(xié)方差P(k|k-1),預測,四：卡爾曼濾波算法數(shù)學推導,48,z(k)= H x(k|k-1)+v(k),K時刻,k+1,k-1,測量值z(k),x(k+1|k+1),x(k|k-1),P(k|k-1),實測,四：卡爾曼濾波算法數(shù)學推導,49,K時刻,k+1時刻,k-1,x(k|k-1),P(k|k-1),x(k|k)=

24、x(k|k-1) + Kg(k) (Z(k)-H X(k|k-1),Kg(k)=P(k|k-1)H/(HP(k|k-1) H +R),修正,P(k|k)=(I-Kg(k)H)P(k|k-1),x(k+1|k+1),四：卡爾曼濾波算法數(shù)學推導,時間更新（預測） （1）計算先驗狀態(tài)估計值 x(k|k-1)=F x(k-1|k-1)+B u(k)+w(k) （2）計算先驗狀態(tài)估計值的協(xié)方差 P(k|k-1)=F P(k-1|k-1) F+Q,測量更新（修正） （1）計算加權矩陣(卡爾曼增益) Kg(k)=P(k|k-1)H/(HP(k|k-1) H +R) （2）對預測值進行修正 x(k|k)=x(

25、k|k-1) + Kg(k) (Z(k)-H X(k|k-1) （3）更新修正值的協(xié)方差 P(k|k)=(I-Kg(k)H)P(k|k-1),初始值x(0)、P(0),四：卡爾曼濾波算法數(shù)學推導,針對上述不足，很多學者提出了不同的方法加以克服，如限定記憶法、平方根濾波、漸消記憶濾波、自適應卡爾曼濾波（AKF）、抗野值濾波等。其中，AKF因為具有自適應特性非常適合動態(tài)系統(tǒng)濾波而受到廣泛重視。因此，在采用卡爾曼濾波處理動態(tài)測量數(shù)據(jù)時，一般都要考慮采取適當?shù)淖赃m應濾波方法來解決這一問題。,卡爾曼濾波的發(fā)展,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,52,應用場合,機器人導航、控制 傳感器數(shù)據(jù)融合 雷達系統(tǒng)以及導彈追蹤 計算機圖像處理 頭臉識別 圖像分割 圖像邊緣檢測,五：卡爾曼濾波的典型應用多傳感器數(shù)據(jù)融合處理,多傳感器數(shù)據(jù)融合：將經(jīng)過集成處理的多種傳感器信息進行合成，形成對被測對象