卡爾曼濾波

概述

??卡爾曼濾波（Kalman filter）是一種高效率的遞歸濾波器（自回歸濾波器）抬探，它能夠從一系列的不完全及包含雜訊的測(cè)量中窜觉，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)狮斗〈嘌停卡爾曼濾波會(huì)根據(jù)各測(cè)量量在不同時(shí)間下的值，考慮各時(shí)間下的聯(lián)合分布凝果，再產(chǎn)生對(duì)未知變數(shù)的估計(jì)，因此會(huì)比只以單一測(cè)量量為基礎(chǔ)的估計(jì)方式要準(zhǔn)≈笙樱卡爾曼濾波得名自主要貢獻(xiàn)者之一的魯?shù)婪颉た柭?/a>，最早用于解決阿波羅計(jì)劃的軌道預(yù)測(cè)問(wèn)題抱虐。
??上面的定義摘自維基百科昌阿，是對(duì)卡爾曼濾波的專業(yè)闡述，為了便于理解通過(guò)下面兩個(gè)例子來(lái)對(duì)卡爾曼濾波有個(gè)大致的了解恳邀。
??航天器的發(fā)動(dòng)機(jī)能夠在足夠高的溫度下燃燒燃料懦冰，為航天器提供足夠的動(dòng)力，航天器發(fā)動(dòng)機(jī)燃燒室在燃燒時(shí)溫度可以達(dá)到數(shù)千攝氏度谣沸，過(guò)高的溫度可能會(huì)損壞發(fā)動(dòng)機(jī)的機(jī)械部件刷钢，導(dǎo)致火箭發(fā)射失敗，因此需要密切關(guān)注火箭燃燒室的內(nèi)部溫度乳附，顯然在燃燒室內(nèi)部放置溫度傳感器會(huì)直接被融化内地。此時(shí)，無(wú)法直接測(cè)量燃燒室的內(nèi)部溫度赋除，基于這種情況瓤鼻，可以在燃燒室外放置一個(gè)溫度傳感器測(cè)量燃燒室的外部溫度，使用卡爾曼濾波器利用外部溫度來(lái)估算燃燒室的內(nèi)部溫度贤重。

??這是卡爾曼濾波器一種使用方式：

當(dāng)系統(tǒng)的狀態(tài)無(wú)法通過(guò)直接測(cè)量得到但是可以間接測(cè)量時(shí)茬祷，可以使用卡爾曼濾波通過(guò)間接測(cè)量值來(lái)估算系統(tǒng)的狀態(tài)。

總所周知并蝗，汽車的導(dǎo)航系統(tǒng)使用車載傳感器（Onboard sensors）得到汽車的當(dāng)前位置并導(dǎo)航到目的地祭犯。常用的車載傳感器有：慣性測(cè)量單元（Inertial measurement unit,IMU）使用加速度計(jì)和陀螺儀來(lái)測(cè)量汽車的加速度和角速度；里程表(Odometer)測(cè)量汽車的相對(duì)行駛距離滚停；GPS接收器(GPS receiver)接收來(lái)自GPS衛(wèi)星的信號(hào)沃粗，來(lái)確定汽車在地球表面的位置。IMU中的加速度計(jì)提供了汽車當(dāng)前的加速度大小及方向键畴，但是想要獲得汽車的位置最盅，需要對(duì)加速度進(jìn)行兩次積分（ $s =\iint a(t)dt$ ）得到汽車的位置，數(shù)值積分算法在計(jì)算位置時(shí)會(huì)存在微小的誤差并且會(huì)隨著時(shí)間的累積會(huì)不斷起惕，最終產(chǎn)生積分漂移涡贱，完全偏離汽車的正確位置；里程表容易受到輪胎壓力和道路狀況的影響惹想；而GPS位置跟新速度慢问词，而且會(huì)存在一定噪聲，最大問(wèn)題還是在車輛通過(guò)隧道或車庫(kù)時(shí)嘀粱，信號(hào)很差激挪，甚至無(wú)法接收到信號(hào)辰狡。三種車載傳感器各自有各自的局限和優(yōu)勢(shì)，只用單獨(dú)一種傳感器進(jìn)行定位效果都不盡如人意垄分，此時(shí)可以使用卡爾曼濾波融合三種傳感器得到汽車位置的最優(yōu)估計(jì)值宛篇。

??這是卡爾曼濾波的另一種使用方式：

組合各種可能受到噪聲影響的傳感器測(cè)量值，得到一個(gè)最優(yōu)的估計(jì)值薄湿。

??卡爾曼濾波主要應(yīng)用于高精度傳感系統(tǒng)中叫倍，在時(shí)下大熱的機(jī)器人、無(wú)人機(jī)嘿般、自動(dòng)導(dǎo)航都有著應(yīng)用段标，由于其遞歸的特性（即只要獲知上一時(shí)刻狀態(tài)的估計(jì)值以及當(dāng)前狀態(tài)的觀測(cè)值就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)值），因此不需要記錄觀測(cè)或者估計(jì)的歷史信息炉奴。與大多數(shù)濾波器不同之處逼庞，卡爾曼濾波器是一種純粹的時(shí)域濾波器，它不需要像低通濾波器等頻域濾波器那樣瞻赶，需要在頻域設(shè)計(jì)再轉(zhuǎn)換到時(shí)域?qū)崿F(xiàn)赛糟。
??其本質(zhì)思想是采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻地估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)砸逊，求出現(xiàn)時(shí)刻的估計(jì)值璧南，它適合于實(shí)時(shí)處理和計(jì)算機(jī)運(yùn)算。下面我們一步步來(lái)解開卡爾曼濾波的神秘面紗师逸。

隱馬爾可夫

隨機(jī)過(guò)程

??隨機(jī)過(guò)程被認(rèn)為是概率論的“動(dòng)力學(xué)”部分司倚，其研究對(duì)象是隨時(shí)間演變的隨機(jī)現(xiàn)象。對(duì)于這種現(xiàn)象篓像，已不能用隨機(jī)變量或多維隨機(jī)變量來(lái)合理表達(dá)动知，而需要一族（無(wú)限多個(gè)）隨機(jī)變量來(lái)描述。高等教育出版社的《概率論和數(shù)理統(tǒng)計(jì)》第四版是這樣定義隨機(jī)過(guò)程：

? 設(shè) $T$ 是一無(wú)限實(shí)數(shù)集员辩，我們把依賴于參數(shù) $t\in T$ 的一族隨機(jī)變量稱為隨機(jī)過(guò)程盒粮，記為 $\{X(t),t\in T\}$ ,其中對(duì) $\forall t \in T,X(t)$ 是一隨機(jī)變量， $T$ 叫做參數(shù)集奠滑。通常把 $t$ 看作時(shí)間丹皱，稱 $X(t)$ 為時(shí)刻 $t$ 時(shí)過(guò)程的狀態(tài)，而 $X(t_1)=x,x \in R$ 定義為 $t=t_1$ 時(shí)過(guò)程處于狀態(tài) $x$ 宋税，對(duì)于 $\forall t\in T,X(t)$ 的所有可能取一切值的全體稱為隨機(jī)過(guò)程的狀態(tài)空間摊崭，泊松過(guò)程和維納過(guò)程都是典型的隨機(jī)過(guò)程，篇幅有限這里不做進(jìn)一步展開弃甥，有興趣的讀者可以自行Google爽室。
馬爾可夫過(guò)程

???一個(gè)隨機(jī)過(guò)程在時(shí)刻 $t_0$ 所處的狀態(tài)為已知的條件下，隨機(jī)過(guò)程在時(shí)刻 $t（t>t_0)$ 所處的狀態(tài)與其在時(shí)刻 $t_0$ 之前所處的狀態(tài)無(wú)關(guān)淆攻。簡(jiǎn)而言之阔墩，就是“將來(lái)”僅依賴于”現(xiàn)在”與“過(guò)去”無(wú)關(guān)。我們稱這種特性為馬爾可夫性或無(wú)后效性瓶珊。而這種具有馬爾可夫性質(zhì)的隨機(jī)過(guò)程稱之為馬爾可夫過(guò)程啸箫，可以推出，泊松過(guò)程是時(shí)間連續(xù)狀態(tài)離散的馬爾可夫過(guò)程伞芹，而維納過(guò)程則是時(shí)間狀態(tài)都連續(xù)的馬爾可夫過(guò)程忘苛。對(duì)于時(shí)間和狀態(tài)都是離散的馬爾可夫過(guò)程就是構(gòu)成卡爾曼濾波的重要基石馬爾可夫鏈：

對(duì)于隨機(jī)過(guò)程 $\{X_n,n=1,2,\cdots\}$ ,若其條件概率分布滿足: $P\{X_{n+1}=x_{n+1}|X_n=x_n,\cdots,X_1=x_1\}=P\{X_{n+1}=x_{n+1}|X_n=x+n\}$ ，則稱此隨機(jī)過(guò)程為馬爾可夫鏈唱较。
狀態(tài)轉(zhuǎn)移矩陣

???在馬爾可夫鏈中，從一個(gè)狀態(tài)轉(zhuǎn)移到另一個(gè)狀態(tài)的概率稱為狀態(tài)轉(zhuǎn)移概率，如果系統(tǒng)的可能狀態(tài)是有限的冰蘑，例如有 $K$ 個(gè)狀態(tài)厌秒，則狀態(tài)轉(zhuǎn)移概率構(gòu)成一個(gè) $K\times K$ 的狀態(tài)轉(zhuǎn)移矩陣： $P=\left[ \begin{matrix} p_{11} & p_{12} & \cdots & p_{1K} \\ p_{21} & p_{22} & \cdots & p_{2K}\\ \cdots & \cdots & \cdots &\cdots \\ p_{K1} & p_{K2} & \cdots & p_{KK}\end{matrix} \right]$ ，其中矩陣的每一行的和為1汉形，該矩陣是一個(gè)隨機(jī)矩陣纸镊，任何一個(gè)隨機(jī)矩陣都可以作為狀態(tài)轉(zhuǎn)移矩陣。
隱馬爾可夫模型

??在馬爾可夫模型中概疆，系統(tǒng)的狀態(tài)是直接可見的逗威，這樣狀態(tài)的轉(zhuǎn)移概率就可以構(gòu)成該系統(tǒng)的全部參數(shù)。但是岔冀，在實(shí)際生活中更多的情況是：系統(tǒng)狀態(tài)并不是直接可見凯旭，我們往往只能觀測(cè)到受狀態(tài)影響的某些變量。顯然對(duì)這些可觀測(cè)到的變量使用馬爾可夫模型對(duì)系統(tǒng)進(jìn)行狀態(tài)分析是不嚴(yán)謹(jǐn)?shù)氖固住榇斯藓簦瑢?duì)于這種含有隱含未知參數(shù)的馬爾可夫過(guò)程，數(shù)學(xué)家通過(guò)隨機(jī)過(guò)程可觀察的參數(shù)確定該過(guò)程的隱含參數(shù)童漩，然后利用這些參數(shù)來(lái)作進(jìn)一步分析弄贿，這也是大名鼎鼎的隱馬爾可夫模型（Hidden Markov model，HMM）的本質(zhì)矫膨，至于隱馬爾可夫模型的更多詳細(xì)內(nèi)容差凹，如果進(jìn)行展開的話，需要花費(fèi)大量篇幅侧馅，在此先不做展開危尿。隱馬爾可夫模型在語(yǔ)音識(shí)別和模式識(shí)別領(lǐng)域都有著應(yīng)用，而卡爾曼濾波也是構(gòu)建在隱馬爾可夫模型之上的馁痴。

線形卡爾曼濾波

??卡爾曼濾波是一個(gè)遞歸濾波器谊娇，其算法本質(zhì)是通過(guò)動(dòng)態(tài)系統(tǒng)的有限個(gè)觀測(cè)值，估計(jì)動(dòng)態(tài)系統(tǒng)的隱藏狀態(tài)罗晕。接下來(lái)济欢，我們從動(dòng)態(tài)系統(tǒng)的狀態(tài)空間模型開始赠堵，一步步推導(dǎo)卡爾曼濾波算法。

狀態(tài)空間模型

??動(dòng)態(tài)系統(tǒng)都具有一個(gè)基本特征：系統(tǒng)的狀態(tài)法褥，那么什么是系統(tǒng)的狀態(tài)呢茫叭，其定義如下：

???一個(gè)隨機(jī)動(dòng)態(tài)系統(tǒng)的狀態(tài)被定義為最少量的信息，這些信息包含過(guò)去作用于該系統(tǒng)的輸入的影響半等，并足以完全描述系統(tǒng)將來(lái)的行為揍愁。（該定義截取自機(jī)械工業(yè)出版社譯制的《神經(jīng)網(wǎng)絡(luò)與機(jī)器學(xué)習(xí)》第三版第十四章動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)的貝葉斯濾波）

???通常我們使用狀態(tài)空間模型來(lái)描述動(dòng)態(tài)系統(tǒng)狀態(tài)對(duì)外部世界的影響，一般而言杀饵，狀態(tài)空間模型分為兩個(gè)部分：
- 系統(tǒng)模型
  
  ? ??系統(tǒng)模型使用時(shí)域函數(shù)來(lái)描述動(dòng)態(tài)系統(tǒng)狀態(tài)的演變莽囤，其數(shù)學(xué)表示為一階馬爾可夫鏈： $x_{n+1}=\zeta_{n}(x_n,\omega_n)$ ，其中 $n$ 表示離散時(shí)間切距，向量 $x_n$ 表示動(dòng)態(tài)系統(tǒng)當(dāng)前的狀態(tài)朽缎，向量 $x_{n+1}$ 表示下一狀態(tài)的值，向量 $\omega_n$ 表示過(guò)程噪聲蔚舀， $\zeta$ 為 $x_n,\omega_n$ 的向量函數(shù)饵沧，會(huì)隨時(shí)間改變。
- 測(cè)量模型
  
  ???測(cè)量模型描述了動(dòng)態(tài)系統(tǒng)狀態(tài)對(duì)外部世界的影響赌躺，公式如下： $y_n = \xi(x_n,v_n)$ 狼牺，其中向量 $y_n$ 表示外部世界對(duì)動(dòng)態(tài)系統(tǒng)的一組觀測(cè)值，向量 $v_n$ 是外部世界噪聲的測(cè)量值礼患， $\xi$ 是 $x_n,v_n$ 的向量函數(shù)是钥，會(huì)隨時(shí)間改變。
???對(duì)于狀態(tài)空間模型缅叠，有著如下假設(shè)：
- 動(dòng)態(tài)系統(tǒng)的任意時(shí)刻 $k$ ,其過(guò)程噪聲 $\omega_k$ 與初始狀態(tài) $x_0$ 無(wú)關(guān)悄泥；
- 動(dòng)態(tài)系統(tǒng)的過(guò)程噪聲 $\omega_n$ 于測(cè)量噪聲 $v_n$ 是統(tǒng)計(jì)獨(dú)立，也就是說(shuō)對(duì) $\forall i,j$ 都有 $E[\omega_iv_j^T]=0$ 成立肤粱；
???一般而言弹囚，跟狀態(tài)與狀態(tài)之間的是否為線形關(guān)系及過(guò)程噪聲、測(cè)量噪聲二者是否服從高斯分布將狀態(tài)空間模型分為四大類：
- 線形高斯模型
- 線形非高斯模型
- 非線性高斯模型
- 非線性非高斯模型
???顯然不管是非線性還是非高斯或二者兼之的狀態(tài)空間模型领曼，其處理難度是遠(yuǎn)高于線形高斯模型的鸥鹉，我們先來(lái)處理最簡(jiǎn)單的狀態(tài)空間模型——線形高斯模型。
線形卡爾曼濾波的理論推導(dǎo)
- 對(duì)于線形高斯模型庶骄，其狀態(tài)空間模型可設(shè)為如下形式：
  
  $\begin{equation} \left\{ \begin{array}{lr} x_{n}=A_{n}x_{n-1}+\omega_{n} \\ y_n = H_nx_n+v_n \end{array} \right. \end{equation}$
  
  ??其中 $\omega_n\sim N(0,Q),v_n\sim N(0,R)$ 毁渗， $A_{n+1}$ 是動(dòng)態(tài)系統(tǒng)從狀態(tài) $x_n$ 到 $x_{n+1}$ 的狀態(tài)轉(zhuǎn)移矩陣， $H_n$ 是測(cè)量矩陣单刁，表示系統(tǒng)狀態(tài) $x_n$ 對(duì) $y_n$ 的增益灸异，將系統(tǒng)狀態(tài)映射到外部世界。
  
  ??考慮到動(dòng)態(tài)系統(tǒng)會(huì)受到系統(tǒng)中已知的控制器的控制信息的影響，需要在系統(tǒng)模型中加入這部分信息肺樟，修正后的狀態(tài)空間模型如下：
  
  $\begin{equation} \left\{ \begin{array}{lr} x_{n}=A_{n}x_{n-1}+B_{n}\mu_{n}+\omega_{n} \\ y_n = H_nx_n+v_n \end{array} \right. \end{equation}$
  
  其中 $\mu_n$ 是系統(tǒng)的控制器向量檐春， $B_n$ 是系統(tǒng)的控制向量。
  
  ???為方便進(jìn)行推導(dǎo)儡嘶，定義 $\hat{x}^{-}_k \in R^n$ （ $^-$ 代表先驗(yàn),^代表估計(jì)）為在已知第 $k$ 步以前狀態(tài)情況下第 $k$ 步的先驗(yàn)狀態(tài)估計(jì)喇聊。定義 $\hat{x}_k\in R^n$ 為已知觀測(cè)變量 $y_k$ 時(shí)恍风，第 $k$ 步的后驗(yàn)估計(jì)狀態(tài)蹦狂，由此定義先驗(yàn)估計(jì)誤差和后驗(yàn)估計(jì)誤差：
  
  $\begin{equation} \left\{ \begin{array}{lr} e_k^-\equiv{x_k-\hat{x}_k^-} \\ e_k\equiv{x_k-\hat{x}_k} \end{array} \right. \end{equation}$
  
  ? 先驗(yàn)估計(jì)誤差的協(xié)方差為：
  
  $P_k^-=E[e_k^-{e_k^-}^T]$
  
  ? 后驗(yàn)誤差估計(jì)的協(xié)方差為：
  
  $P_k=E[e_ke_k^T]$
  
  ???顯然 $P_k^-$ 是真實(shí)值和預(yù)測(cè)值之間的協(xié)方差， $P_k$ 是真實(shí)值和最優(yōu)估計(jì)值之間的協(xié)方差朋贬】ǎ卡爾曼濾波的核心就是如何根據(jù) $k-1$ 時(shí)刻的最優(yōu)狀態(tài)估計(jì) $\hat{x}_{k-1}$ 和第 $k$ 時(shí)刻的觀測(cè)值 $y_k$ 得到 $k$ 時(shí)刻的最優(yōu)狀態(tài)估計(jì)值 $\hat{x}_k$ ,顯然根據(jù)定義 $P_k$ 越小，估計(jì)值越接近于真實(shí)值锦募，此時(shí)摆屯，只需求解當(dāng)前條件下使得 $P_k$ 最小的狀態(tài)估計(jì)值即是當(dāng)前時(shí)刻狀態(tài)的最優(yōu)估計(jì)值。利用上文修正后的狀態(tài)空間模型的系統(tǒng)模型得到 $k$ 時(shí)刻狀態(tài)的預(yù)測(cè)值 $\hat{x}_k^-=A\hat{x}_{k-1}+B\mu_k$ 糠亩，為了得到 $k$ 時(shí)刻的最小協(xié)方差 $P_k$ 虐骑，卡爾曼濾波定義參數(shù)卡爾曼增益 $K=\frac{\hat{x}_k-\hat{x}_k^-}{y_k-H\hat{x}_k^-}$ ，其中赎线，觀測(cè)向量及其預(yù)測(cè)之差 $y_k-H\hat{x}_k^-$ 被稱為測(cè)量過(guò)程的新息或殘余廷没。新息反應(yīng)了預(yù)測(cè)值和實(shí)際值之間的不一致程度，為零時(shí)表明二者完全吻合垂寥，推導(dǎo)過(guò)程如下颠黎。
  
  由卡爾曼增益的定義式變形得到:
  
  $\hat{x}_k=\hat{x}_k^-+K(y_k-H\hat{x}_k^-)$
  
  將狀態(tài)空間模型的觀測(cè)模型代入得到:
  
  $\hat{x}_k=\hat{x}_k^-+K(Hx_k +v_k-H\hat{x}_k^-)$
  
  進(jìn)一步整理變換得到:
  
  $\hat{x}_k-x_k=\hat{x}_k^—x_k+KH(x_k-\hat{x}_k^-)+Kv_k$
  
  結(jié)合先驗(yàn)估計(jì)誤差和后驗(yàn)估計(jì)誤差的定義可知:
  
  $e_k=(I-KH)e_k^-+Kv_k$
  
  代入后驗(yàn)誤差協(xié)方差的定義計(jì)算得到:
  
  $P_k=E[e_ke_k^T]= E[[(I-KH)e_k^—Kv_k][(I-KH)e_k^—Kv_k]^T]$
  
  展開可知:
  
  $P_k=P^-_k-KHP^-_k-P^-_kH^TK^T+K(HP^-_kH^T+R)K^T=P(K)$
  
  要求 $P_k$ 的最小值，結(jié)合上式對(duì)卡爾曼增益 $K$ 求偏導(dǎo)滞项，得到：
  
  $\frac{\partial{P_k}}{\partial{K}}=-2P^-_kH^T+2KHP_k^-H^T+2KR$
  
  ??需要注意的是狭归，因?yàn)樯婕暗骄仃噷?dǎo)數(shù)，與常規(guī)導(dǎo)數(shù)求導(dǎo)略有不同文判，有興趣的讀者过椎，可以結(jié)合下面的矩陣求導(dǎo)規(guī)則進(jìn)行求導(dǎo)：
  
  若 $Y=AX$ ,則 $\frac{d{Y}}{d{X}}=A^T$ ；
  
  若 $Y=XA$ ,則 $\frac{d{Y}}{d{X}}=A$ 戏仓；
  
  若 $Y=A^TXB$ ,則 $\frac{d{Y}}{d{X}}=AB^T$ 疚宇；
  
  若 $Y=A^TX^TB$ ,則 $\frac{d{Y}}{d{X}}=BA^T$ ；
  
  若 $Y=X^TX$ ,則 $\frac{d{Y}}{d{X}}=$ X柜去；
  
  若 $Y=AX^T$ ,則 $\frac{d{Y}}{d{X}}=A$ 灰嫉；
  
  若 $Y=u(X)^Tv(X)$ ,則 $\frac{d{uv}}{d{X}}=\frac{du^T}{dX}v+\frac{dv^T}{X}u$ ；
  
  令 $\frac{\partial{P_k}}{\partial{K}}=0$ 嗓奢，求解得到:
  
  $K=\frac{P^-_kH^T}{HP^-_KH^T+R}$
  
  直觀來(lái)看讼撒，一方面：
  
  卡爾曼增益
  
  即，隨著觀測(cè)噪聲協(xié)方差 $R$ 的減小，卡爾曼增益逐漸增大根盒，當(dāng) $R=0$ 時(shí)钳幅，取得最大值 $H^{-1}$ ;
  
  另一方面：
  
  $\lim\limits_{P_k^-\to0}K=0$
  
  即，隨著先驗(yàn)估計(jì)協(xié)方差 $P_k^-$ 的減小炎滞，卡爾曼增益隨之減小敢艰，當(dāng) $P_k^-=0$ 時(shí)，取得最小值0册赛；
  
  結(jié)合卡爾曼增益的定義可知钠导，卡爾曼增益實(shí)際上表征了狀態(tài)最優(yōu)估計(jì)過(guò)程中模型預(yù)測(cè)誤差與量測(cè)誤差的比重，隨著觀測(cè)噪聲協(xié)方差趨近于零森瘪，模型中預(yù)測(cè)誤差的比重越來(lái)越大牡属，此時(shí)模型更信任模型中觀測(cè)值的信息；另一方面扼睬，隨著先驗(yàn)估計(jì)誤差協(xié)方差趨近于零逮栅，模型中預(yù)測(cè)誤差的比重越來(lái)越小，此時(shí)模型更信任模型中預(yù)測(cè)值的信息窗宇。
  
  代入矩陣函數(shù) $P(K)$ 得到：
  
  $P_k=(I-KH)P_k^-$
  
  至此措伐，我們已經(jīng)得到了第 $k$ 時(shí)刻最優(yōu)狀態(tài)估計(jì)值為：
  
  $\hat{x}_k =\hat{x}_k^-+K(y_k-H\hat{x}_k^-)$
??但是，到了這一步問(wèn)題仍然沒有得到解決军俊，因?yàn)樵谟?jì)算 $k$ 時(shí)刻的卡爾曼增益時(shí)侥加，仍有一個(gè)值未確定的：先驗(yàn)估計(jì)的協(xié)方差矩陣 $P_k^-$ ，在計(jì)算 $P_k^-$ 之前蝇完，需要先計(jì)算先驗(yàn)誤差：

$e^-_k=x_k-\hat{x}_k^-=(Ax_{k-1}+B\mu_k+\omega_k)-(A\hat{x}_{k-1}+Bu_k)=Ae_{k-1}+\omega_k$

因?yàn)?/p>
$P_k^-=E[e_k^-{e_k^-}^T]=E[(Ae_{k-1}+\omega_k)(Ae_{k-1}+\omega_k)^T]=E[Ae_{k-1}e_{k-1}^TA^T]+E[\omega_k\omega_k^T]$

得到：

$P_k^-=AP_{k-1}A^T+Q$

到這步官硝，卡爾曼濾波形成一個(gè)完整的理論閉環(huán)。

離散線形卡爾曼濾波算法

??卡爾曼濾波器采用反饋控制的方法來(lái)估計(jì)過(guò)程狀態(tài)：濾波器估計(jì)過(guò)程某一時(shí)刻的狀態(tài)短蜕，然后以（含噪聲的）觀測(cè)變量的方式獲得反饋氢架。因此卡爾曼濾波器可分為兩個(gè)部分：

時(shí)間更新方程

???時(shí)間更新方程負(fù)責(zé)及時(shí)向前推算當(dāng)前狀態(tài)變量和誤差協(xié)方差估計(jì)的值，以便為下一個(gè)時(shí)間狀態(tài)構(gòu)造先驗(yàn)估計(jì)朋魔，其數(shù)學(xué)表示如下：

$\hat x_k^- = A \hat x_{k-1}+B\hat \mu_k+w_k$

$P_{k}^-=AP_{k-1}A^T+Q$
測(cè)量更新方程

???測(cè)量更新方程負(fù)責(zé)反饋——也就是說(shuō)岖研，它將先驗(yàn)估計(jì) 和新的測(cè)量變量結(jié)合以構(gòu)造改進(jìn)的后驗(yàn)估計(jì)，其數(shù)學(xué)表示如下：

$K_k=P^-_kH^T(HP^-_kH^T+R)^{-1}$

$Z_k=Hx_k+v_k$

$\hat x = \hat x_k^{-}+K_k(Z_k-H\hat x_k^{-})$

$P_k=(I-K_kH)P_k^{-}$

? 離散線形卡爾曼濾波器的遞歸計(jì)算過(guò)程如下圖：

離散卡爾曼濾波的遞歸過(guò)程

???時(shí)間更新方程將當(dāng)前狀態(tài)變量作為先驗(yàn)估計(jì)及時(shí)地向前投射到測(cè)量更新方程警检，測(cè)量更新方程校正先驗(yàn)估計(jì)以獲得狀態(tài)的后驗(yàn)估計(jì)孙援。完成時(shí)間更新方程和測(cè)量更新方程的一輪更新之后，再次重復(fù)這個(gè)過(guò)程扇雕，將上一次計(jì)算得到的后驗(yàn)估計(jì)作為下一次計(jì)算的先驗(yàn)估計(jì)拓售。

卡爾曼濾波器參數(shù)分析

? ??在實(shí)現(xiàn)卡爾曼濾波器算法時(shí)，會(huì)涉及到很多參數(shù)镶奉，如下：

$x_0$ ：動(dòng)態(tài)系統(tǒng)的初始狀態(tài)變量 $x_0$ 一般直接取第一個(gè)測(cè)量值 $z_0$ 础淤；

$P_0$ ：系統(tǒng)初始狀態(tài)變量 $x_0$ 的協(xié)方差崭放，只要初始值不為零，初始協(xié)方差矩陣的取值對(duì)濾波效果影響很小鸽凶，都能很快收斂币砂；

$A$ ：狀態(tài)轉(zhuǎn)移矩陣，是根據(jù)經(jīng)驗(yàn)對(duì)下一個(gè)時(shí)間周期目標(biāo)狀態(tài)的一種預(yù)測(cè)玻侥。在某些情況下决摧，這種預(yù)測(cè)是確定的，而另一些情況下這種預(yù)測(cè)是未知的凑兰；

$B$ ：輸入控制矩陣掌桩，作用在控制向量 $\mu_k$ 上的 $n\times1$ 輸入控制矩陣，由于控制信息是已知的票摇，一般很好確定拘鞋；
$H$ ： $m\times n$ 觀測(cè)模型矩陣，負(fù)責(zé)將系統(tǒng)的真實(shí)狀態(tài)映射到觀測(cè)空間矢门，根據(jù)動(dòng)態(tài)系統(tǒng)很容易確定；

$P ^-$ ：為 $n\times n$ 先驗(yàn)估計(jì)誤差協(xié)方差矩陣灰蛙，由 $P_0$ 遞歸即可得到祟剔；

$P$ ：為 $n×n$ 后驗(yàn)估計(jì)誤差協(xié)方差矩陣，由 $P_0$ 遞歸即可得到摩梧；

$Q$ ： $n\times n$ 過(guò)程噪聲 $\omega_n$ 是的協(xié)方差矩陣物延，當(dāng)動(dòng)態(tài)系統(tǒng)的狀態(tài)轉(zhuǎn)換過(guò)程確定時(shí)， $Q$ 是一個(gè)確定的值仅父，此時(shí)叛薯，可通過(guò)離線測(cè)試確定對(duì)于某個(gè)過(guò)程的最優(yōu) $Q$ 值，一般來(lái)說(shuō)笙纤，當(dāng)狀態(tài)轉(zhuǎn)換過(guò)程為已確定時(shí)耗溜， $Q$ 的取值越小越好。當(dāng) $Q$ 取值逐漸增大時(shí)省容，濾波收斂變慢抖拴，且狀態(tài)變量的擾動(dòng)變大。當(dāng)系統(tǒng)的動(dòng)態(tài)轉(zhuǎn)換過(guò)程是不確定的腥椒，隨時(shí)間變化阿宅，此時(shí) $Q$ 不再是一個(gè)確定值，而是一個(gè)隨時(shí)間變化的變量 $Q(k)$ 笼蛛，此時(shí)的卡爾曼濾波為自適應(yīng)卡爾曼濾波洒放，關(guān)于自適應(yīng)卡爾曼濾波的更詳細(xì)內(nèi)容請(qǐng)參考此文—自適應(yīng)卡爾曼濾波技術(shù)，在此不做進(jìn)一步展開滨砍；

$R$ ： $m\times m$ 測(cè)量噪聲 $v_n$ 協(xié)方差矩陣往湿，跟系統(tǒng)外部環(huán)境的測(cè)量?jī)x器相關(guān)榨为，一般很難獲得改值，根據(jù)平時(shí)的使用經(jīng)歷來(lái)看:

$R$ 取值過(guò)小或過(guò)大都會(huì)使濾波效果變差煌茴；

$R$ 取值越小收斂越快随闺，反之收斂越慢；

一般離線得到合適的 $R$ 值蔓腐，再代入濾波器中矩乐；

$I$ ： $n\times n$ 單位矩陣

$K$ ： $n×m$ 階矩陣，卡爾曼增益

???關(guān)于卡爾曼濾波參數(shù)的更詳細(xì)內(nèi)容回论，由于篇幅有限散罕，只做了簡(jiǎn)單的描述，但是卡爾曼濾波器參數(shù)的確定卻是重中之重傀蓉，直接影響到算法的效果欧漱，要進(jìn)行實(shí)際應(yīng)用的讀者可結(jié)合卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究一文，來(lái)做進(jìn)一步討論葬燎。

發(fā)散現(xiàn)象及平方根濾波

發(fā)散現(xiàn)象

???線形卡爾曼濾波基于線形高斯?fàn)顟B(tài)空間模型的假設(shè)推導(dǎo)的误甚，但是在實(shí)際應(yīng)用時(shí)動(dòng)態(tài)系統(tǒng)不一定完全滿足這些假定，這會(huì)兒導(dǎo)致卡爾曼濾波的不穩(wěn)定谱净，這也叫卡爾曼濾波的發(fā)散現(xiàn)象窑邦，另外在計(jì)算時(shí)如果數(shù)值不太精確也會(huì)產(chǎn)生發(fā)散現(xiàn)象。
平方根濾波

???一個(gè)數(shù)學(xué)上優(yōu)美且計(jì)算可行的壕探，解決卡爾曼濾波發(fā)散問(wèn)題的方法是使用平方根濾波冈钦，其本質(zhì)思想是對(duì)卡爾曼濾波進(jìn)行修正，在算法的每一次循環(huán)中使用數(shù)值穩(wěn)定的正交變換李请。具體而言瞧筛，就是使用喬里斯基分解將卡爾曼濾波的誤差協(xié)方差矩陣轉(zhuǎn)換為其平方根形式。理論推導(dǎo)部分可以參考機(jī)械工業(yè)出版社的《神經(jīng)網(wǎng)絡(luò)與機(jī)器學(xué)習(xí)》第十四章動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)的貝葉斯濾波中的平方根濾波部分导盅，在此不做推導(dǎo)较幌。

最后編輯于：2020.12.02 17:26:10

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市认轨，隨后出現(xiàn)的幾起案子绅络，更是在濱河造成了極大的恐慌，老刑警劉巖嘁字，帶你破解...
沈念sama閱讀 221,576評(píng)論 6贊 515
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件恩急，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡纪蜒，警方通過(guò)查閱死者的電腦和手機(jī)衷恭，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 94,515評(píng)論 3贊 399
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)纯续，“玉大人随珠，你說(shuō)我怎么就攤上這事灭袁。” “怎么了窗看？”我有些...
開封第一講書人閱讀 168,017評(píng)論 0贊 360
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵茸歧，是天一觀的道長(zhǎng)。經(jīng)常有香客問(wèn)我显沈，道長(zhǎng)软瞎，這世上最難降的妖魔是什么？我笑而不...
開封第一講書人閱讀 59,626評(píng)論 1贊 296
?港島之戀（遺憾婚禮）
正文為了忘掉前任拉讯，我火速辦了婚禮涤浇，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘魔慷。我一直安慰自己只锭，他們只是感情好，可當(dāng)我...
茶點(diǎn)故事閱讀 68,625評(píng)論 6贊 397
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開白布院尔。她就那樣靜靜地躺著蜻展，像睡著了一般。火紅的嫁衣襯著肌膚如雪召边。梳的紋絲不亂的頭發(fā)上铺呵，一...
開封第一講書人閱讀 52,255評(píng)論 1贊 308
城市分裂傳說(shuō)
那天，我揣著相機(jī)與錄音隧熙，去河邊找鬼。笑死幻林，一個(gè)胖子當(dāng)著我的面吹牛贞盯，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播沪饺，決...
沈念sama閱讀 40,825評(píng)論 3贊 421
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼躏敢，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼！你這毒婦竟也來(lái)了整葡？” 一聲冷哼從身側(cè)響起件余，我...
開封第一講書人閱讀 39,729評(píng)論 0贊 276
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤，失蹤者是張志新（化名）和其女友劉穎遭居，沒想到半個(gè)月后啼器，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 46,271評(píng)論 1贊 320
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡俱萍，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 38,363評(píng)論 3贊 340
?白月光啟示錄
正文我和宋清朗相戀三年端壳，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片枪蘑。...
茶點(diǎn)故事閱讀 40,498評(píng)論 1贊 352
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡损谦，死狀恐怖岖免，靈堂內(nèi)的尸體忽然破棺而出，到底是詐尸還是另有隱情照捡，我是刑警寧澤颅湘，帶...
沈念sama閱讀 36,183評(píng)論 5贊 350
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布，位于F島的核電站栗精，受9級(jí)特大地震影響闯参，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜术羔，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 41,867評(píng)論 3贊 333
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一赢赊、第九天我趴在偏房一處隱蔽的房頂上張望。院中可真熱鬧级历，春花似錦释移、人聲如沸。這莊子的主人今日做“春日...
開封第一講書人閱讀 32,338評(píng)論 0贊 24
一樁弒父案玩讳，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)。三九已至嚼贡，卻和暖如春熏纯，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背粤策。一陣腳步聲響...
開封第一講書人閱讀 33,458評(píng)論 1贊 272
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工樟澜，沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人叮盘。一個(gè)月前我還...
沈念sama閱讀 48,906評(píng)論 3贊 376
代替公主和親
正文我出身青樓秩贰，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親柔吼。傳聞我的和親對(duì)象是個(gè)殘疾皇子毒费，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 45,507評(píng)論 2贊 359

卡爾曼濾波

概述

隱馬爾可夫

線形卡爾曼濾波

離散線形卡爾曼濾波算法

發(fā)散現(xiàn)象及平方根濾波

推薦閱讀更多精彩內(nèi)容