卡爾曼濾波算法-Kalman Filter Algorithm

1、簡介

1.1 濾波是什麼

所謂了濾波，就是從混合在一起的諸多信號中提取出所需要的信號。

1.2 信號的分類：

（1）確定性信號：可以表示爲確定的時間函數，可確定其在任何時刻的量值。（具有確定的頻譜）

（2）隨機信號：不能用確定的數學關係來描述的，不能預測其任何瞬時值，其變化服從統計規律。（頻率不確定，功率譜確定）

確定性信號的濾波：

可採用低通、高通、帶通和帶阻等模擬濾波器或者計算機通過算法實現--常規濾波。

隨機信號的濾波：

根據有用信號和干擾信號的功率譜設計濾波器--維納濾波(Wiener Filtering) 或者卡爾曼濾波(Kalman Filter)

隨機信號的濾波也可以看做是估計問題。

1.3 kalman filter是什麼

簡單的說卡爾曼濾波器是一個“optical recursive data processing algorithm(最優化迴歸數據處理算法）”。從形式上，卡爾曼濾波器是5個公式。

對於解決大部分問題卡爾曼濾波器是最優、效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航、控制、傳感器數據融合，甚至在雷達系統及導彈追蹤等方面，近年來更被用於計算機圖像處理，例如頭像識別，頭像分割、圖像邊緣檢測等等。

1.4 卡爾曼濾波器的特點：

（1）卡爾曼濾波器處理的對象是隨機信號；

（2）被處理的信號無有用和干擾之分，濾波的目的是要估計出所有被處理的信號（區別於維納濾波）；

（3）系統的白噪聲激勵和測量噪聲並不是需要濾除的對象，它們的統計特性是估計過程中需要利用的信息；（區別於最小二乘法）

（4）算法是遞推的，且使用狀態空間法在時域內設計濾波器，適用於對多維隨機過程；

（5）被估計量既可以是平穩的，也可以是非平穩的；

（6）估計過程中，只需要考慮過程噪聲和測量噪聲及當前時刻系統狀態的統計特性。（花費計算機計算資源少）

1.5 卡爾曼濾波分類：

現行系統的卡爾曼濾波方程；

1）線性離散系統 2）線性連續系統

非線性系統的卡爾曼濾波方程

1）擴展卡爾曼濾波器EKF 2）無跡卡爾曼濾波器UKF

1.6 卡爾曼濾波的基本思想：

卡爾曼濾波就是把統計學應用到了濾波算法上。其算法的核心思想是,根據當前的儀器"測量值" 和上一刻的 "預測量" 和 "誤差",計算得到當前的最優量. 再預測下一刻的量,

裏面比較突出的是觀點是. 把誤差納入計算, 而且分爲預測誤差和測量誤差兩種.通稱爲噪聲.

還有一個非常大的特點是,誤差獨立存在, 始終不受測量數據的影響.

在海圖作業中，假設航海長需要知道當前時刻船的位置，航海長通常以上一時刻的船位基準，根據航向、航速和海流等一系列因素推算當前刻的船位置，觀測和推算這兩個船位一般不重合，無論是通過航向航速等因素計算出來的結果和當前時刻傳感器對船位的測量結果都是不準確的，都會有誤差，航海長需要通過分析和判斷選擇一個可靠的船位，作爲船艦的下一個位置。當前準確位置變成一個根據計算估計出來的位置和傳感器測量出來的位置獲取真實位置的最優估計問題。

上面這種場景便是一個典型的卡爾曼濾波的應用場景，卡爾曼濾波的基本思想是：

以k-1時刻的最優估計 $x_{k-1}$ 爲準，預測k時刻的狀態變化量 $\widehat{x}_{k/k-1}$ ,同時又對該狀態進行觀測，得到觀測變量 $Z_{k}$ ,再在預測和觀測之間進行分析，或者說以觀測量對預測量進行修正，從而得到k時刻的最優狀態估計 $x_{k}$

2、卡爾曼濾波器的基本方程：

2.1、無控制離散型卡爾曼濾波器的基本方程：

系統的狀態方程： $x_{k} = \phi _{k,k-1} * x_{k-1}+\Gamma _{k-1}w_{k-1}$

系統的測量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

這裏：

$w_{k-1}$ 爲過程白噪聲；

$v_{k}$ 位測量噪聲；

$\Gamma$ 爲噪聲驅動矩陣；

$Z_{k}$ 系統測量方程的輸出量（即觀測量），是可以實際測量的量；

如果 $w_{k-1}$ 和 $v_{k}$ 滿足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=0$

$Q_{k}$ 位過程噪聲的協方差矩陣，表示向量元素之間的相關關係，其爲非負定陣；

$R_{k}$ 爲測量噪聲的協方差，表示向量元素之間的相關關係，其爲正定陣；

在實際應用中 $w_{k-1}$ 和 $v_{k}$ 常假設爲滿足高斯分佈，過程噪聲 $w_{k-1}$ 和測量噪聲 $v_{k}$ 假設是相互獨立的（之間沒有關係，無相互影響）

意思是這些噪聲在離散的狀態上是沒有關係的（互相獨立的，每個時刻的噪聲都是獨立的）且服從高斯分佈：

$p(w_{k-1})$ ~

$p(v_{k})$ ~

$Q_{k}=ww^{T}$

$R_{k}=vv^{T}$

$E\left [ w_{k} \right ] = 0$

$E\left [ v_{k} \right ] = 0$

則無控制離散型卡爾曼濾波的基本方程爲；

(1)對k時刻狀態的預測方程：

${\widehat{x}_{k/k-1} =\phi _{ k,k-1}*x_{k-1}$ (1-1)

(2)對k時刻預測的均方誤差的預測方程：

${\widehat{P}_{k/k-1} =\phi _{ k,k-1}*P_{k-1}*\phi _{ k,k-1}^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T}$ (1-2)

(3)濾波增益方程（權重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T} + R_{k} \right ]^{-1}$ (1-3)

(4)濾波估計方程（k時刻的最優估計值）：

${x}_{k} ={\widehat{x}_{k/k-1}} + K_{k}\left [ Z_{k} -H_{k}* {\widehat{x}_{k/k-1}\right ]$ (1-4)

(5)濾波均方誤差更新矩陣（k時刻最優均方誤差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (1-5)

參數解析：

1、 ${x}_{k-1}$ 和 ${x}_{k}$ : 分別表示 k-1 時刻和 k 時刻的後驗狀態估計值，是濾波的結果之一，即更新後的結果，也叫最優估計（估計的狀態，根據理論，我們不可能知道每時刻狀態的確切結果所以叫估計）。

2、 ${\widehat{x}_{k/k-1}$ : k 時刻的先驗狀態估計值，是濾波的中間計算結果，即根據上一時刻（k-1時刻）的最優估計預測的k時刻的結果，是預測方程的結果。

3、 ${P}_{k-1}$ 和 ${P}_{k}$ ：分別表示 k-1 時刻和 k 時刻的後驗估計協方差（即的協方差，表示狀態的不確定度），是濾波的結果之一。

4、 ${\widehat{P}_{k/k-1}$ ：k 時刻的先驗估計協方差（ ${\widehat{x}_{k/k-1}$ 的協方差），是濾波的中間計算結果。

5、：是狀態變量到測量（觀測）的轉換矩陣，表示將狀態和觀測連接起來的關係，卡爾曼濾波里爲線性關係，它負責將 m 維的測量值轉換到 n 維，使之符合狀態變量的數學形式，是濾波的前提條件之一。

6、 $Z_{k}$ ：測量值（觀測值），是濾波的輸入。

7， $K_{k}$ ：濾波增益矩陣，是濾波的中間計算結果，卡爾曼增益，或卡爾曼係數。

8、 $\phi _{ k,k-1}$ ：狀態轉移矩陣，實際上是對目標狀態轉換的一種猜想模型。例如在機動目標跟蹤中，狀態轉移矩陣常常用來對目標的運動建模，其模型可能爲勻速直線運動或者勻加速運動。當狀態轉移矩陣不符合目標的狀態轉換模型時，濾波會很快發散。

9、 $Q_{k}$ ：過程激勵噪聲的協方差（系統過程的協方差），表示向量元素之間的相關關係。該參數被用來表示狀態轉換矩陣與實際過程之間的誤差。因爲我們無法直接觀測到過程信號，所以 Q 的取值是很難確定的。是卡爾曼濾波器用於估計離散時間過程的狀態變量，也叫預測模型本身帶來的噪聲。狀態轉移協方差矩陣

10、 $R_{k}$ ：測量噪聲的協方差，表示向量元素之間的相關關係。濾波器實際實現時，測量噪聲協方差 R一般可以觀測得到，是濾波器的已知條件。

11、 $Z_{k} -H_{k}* {\widehat{x}_{k/k-1}$ ：實際觀測和預測觀測的殘差，和卡爾曼增益一起修正先驗（預測），得到後驗。

2.2、帶有控制的離散型卡爾曼濾波基本方程

系統的狀態方程： $x_{k} = \phi _{k,k-1}* x_{k-1}+\Gamma _{k-1}w_{k-1} + B_{k-1}u_{k-1}$

系統的測量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

參數解析：

1、 $u_{k-1}$ 表示系統的控制輸入；

2、 $B_{k-1}$ 是系統參數，表示系統控制輸入k-1時刻到k時刻，系統輸入轉化爲狀態的轉換矩陣；

如果 $w_{k-1}$ 和 $v_{k}$ 滿足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=0$

$Q_{k}$ 位過程噪聲的協方差，其爲非負定陣；

$R_{k}$ 爲測量噪聲的協方差，其爲正定陣；

則帶控制離散型卡爾曼濾波的基本方程爲；

(1)對k時刻狀態的預測方程：

${\widehat{x}_{k/k-1}} =\phi _{ k,k-1}*x_{k-1} +B_{k-1}u_{k-1}$ (2-1)

(2)對k時刻預測的均方誤差的預測方程：

${\widehat{P}_{k/k-1}} =\phi _{ k,k-1}*P_{k-1}*\phi _{ k,k-1}^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T}$ (2-2)

(3)濾波增益方程（權重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T}+R_{k} \right ]^{-1}$ (2-3)

(4)濾波估計方程（k時刻的最有估計值）：

${x}_{k} ={\widehat{x}_{k/k-1}}+ K_{k}\left [ Z_{k} - H_{k}*\widehat{x}_{k/k-1}\right ]$ (2-4)

(5)濾波均方誤差更新矩陣（k時刻最優均方誤差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (2-5)

2.3、線性離散型卡爾曼濾波方程的一般形式：

系統的狀態方程： $x_{k} = \phi _{k,k-1}* x_{k-1}+\Gamma _{k-1}w(k-1) + B_{k-1}u_{k-1}$

系統的測量方程： $Z_{k}=H_{k}*x_{k} + v_{k}$

如果 $w_{k-1}$ 和 $v_{k}$ 滿足

$E\left [ w_{k} \right ] = 0,Cov\left [ w_{k},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k},v_{j} \right ]=S_{k}\delta _{kj}$

$Q_{k}$ 位過程噪聲的協方差，其爲非負定陣；

$R_{k}$ 爲測量噪聲的協方差，其爲正定陣；

引入矩陣 $J_{k}=\Gamma _{k-1}S_{k}R_{k}^{-1}$ ，對狀態方程進行等效變換：

$x_{k} =\phi_{k,k-1}^{*}x_{k-1} +B_{k-1}u_{k-1} + J_{k-1}Z_{k-1}+w_{k-1}^{*};$

$Z_{k}=H_{k}x_{k}+v_{k}$

其中：

$\phi_{k,k-1}^{*} = \phi_{k,k-1} -J_{k-1}H_{k-1};$

$w_{k-1}^{*}=\Gamma _{k-1}w_{k-1}-J_{k-1}v_{k-1}$

$E\left [ w_{k}^{*} \right ] = 0,Cov\left [ w_{k}^{*},w_{j} \right ] = Q_{k}\delta _{kj};$

$E\left [ v_{k} \right ] = 0,Cov\left [ v_{k},v_{j} \right ] =R_{k}\delta _{kj};$

$Cov\left [ w_{k}^{*},v_{j} \right ]=0$

$Q_{k}$ 位過程噪聲的協方差，其爲非負定陣；

$R_{k}$ 爲測量噪聲的協方差，其爲正定陣；

則線性離散型卡爾曼濾波器的一般形式基本方程爲；

(1)對k時刻狀態的預測方程：

${\widehat{x}_{k/k-1} =\phi _{ k,k-1}^{*}*x_{k-1} +B_{k-1}u_{k-1}+ J_{k-1}Z_{k-1}$ (3-1)

(2)對k時刻協方差的預測方程：

${\widehat{P}_{k/k-1} =\phi _{ k,k-1}^{*}*P_{k-1}*(\phi _{ k,k-1}^{*})^{T} + \Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T} - J_{k-1}S_{k-1}^{T}\Gamma_{k-1}^{T}$ (3-2)

(3)濾波增益方程（權重）：

$K_{k}={\widehat{P}_{k/k-1}} * H_{k}^{T}\left [ H_{k} * {\widehat{P}_{k/k-1}} * H_{k}^{T} + R_{k} \right ]^{-1}$ (3-3)

(4)濾波估計方程（k時刻的最有估計值）：

${x}_{k} ={\widehat{x}_{k/k-1}}+B _{k-1}*u_{k-1} +J_{k-1}Z_{k-1}+ H_{k}\left [ Z_{k} -H_{k}* {\widehat{x}_{k/k-1} - H_{k}B_{k-1}u_{k-1}\right ]$ (3-4)

(5)濾波均方誤差更新矩陣（k時刻最優均方誤差）：

${P}_{k} =\left [ I - K_{k}*H_{k} \right ]*\widehat{P}_{k/k-1}$ (3-5)

Kalman濾波是一種遞歸過程，主要有兩個更新過程：時間更新和觀測更新，其中時間更新主要包括狀態預測和協方差預測，主要是對系統的預測，而觀測更新主要包括計算卡爾曼增益、狀態更新和協方差更新，因此整個遞歸過程上面五個方面的計算：1）狀態預測；2）協方差預測；3）卡爾曼增益；4）狀態更新；5）協方差更新；

1、式(3-1)即線性隨機差分方程(Linear Stochastic Difference equation)，其利用k-1時刻系統狀態最優估計 $x_{k-1}$ 給出當前k時刻系統狀態的估計值 ${\widehat{x}_{k/k-1}}$ ， $x_{k-1}$ 和 ${\widehat{x}_{k/k-1}}$ 都是n*1矩陣， $\phi _{ k,k-1}^{*}$ 是k-1時刻到k時刻的系統轉換矩陣，大小位n*n矩陣， $u_{k-1}}$ 是系統輸入，大小爲m*1， $B_{k-1}}$ 位將輸入轉化爲系統狀態的矩陣，大小爲n*m，線性隨機差分方程假設上一狀態與當前狀態有某種線性關係，比如恆溫環境的溫度，勻速運動的速度等，但是因爲現實環境的複雜，這種線性關係不是完全平滑的，也就是會有一些擾動，式中用 $w_{k-1} = J_{k-1}Z_{k-1}$ 表示擾動，該擾動是系統過程噪聲，主要是從上一狀態進入到當前狀態時外界因素的干擾，通常假定服從高斯白噪聲分佈。這一公式的主要含義是： $x_{k-1}$ 是上一狀態的最優結果， ${\widehat{x}_{k/k-1}$ 是利用上一狀態預測得到的結果， $Z_{k-1}$ 是上一時刻的觀測結果，這公式主要預測當前時刻系統狀態結果。

2、式(3-2)

協方差預測：

$P_{k-1}$ 是上一狀態 $x_{k-1}$ 對應的協方差， ${\widehat{P}_{k/k-1}$ 是 $x_{k}$ 時刻對應的協方差的預測值， $Q_{k-1}$ 是系統過程噪聲（假設爲高斯白噪聲）的協方差矩陣；

這個公式主要含義：對系統結果對應的協方差進行更新；

3、式(3-3)

$H_{k}$ 爲觀測矩陣， $R_{k}$ 爲測量噪聲（同樣假設爲高斯白噪聲）對應的協方差矩陣；主要含義：用於狀態更新和協方差更新中，因爲前兩個預測我們得到了當前狀態的預測結果，然後再得到當前狀態的觀測值，通過預測值和觀測值，獲得卡爾曼增益 $K_{k}$

4、式(3-4)

主要含義：利用當前狀態的觀測值和預測值，及增益，計算k時刻下的當前狀態的最優狀態估計 ${x}_{k}$ ，也就是狀態更新。

5、式(3-5)

其中，爲單位矩陣；

主要含義：前面已經更新了狀態，爲了保證遞推下去，還要更新當前狀態對應的協方差矩陣，用於下一輪計算的公式（3-2）中的 $P_{k-1}$ ；

2.4、離散型卡爾曼濾波基本方程使用要點：

（1）濾波初值的選擇

卡爾曼濾波是一種遞推算法，啓動時必須先給初值 $\widehat{x}_{0},P_{0}$ ;

情況一：一般情況下，取 $\widehat{x}_{0} = E[x_{0}], P_{0}=Cov\left [ x_{0} \right ]$ ，卡爾曼濾波是無偏的，即濾波穩定，但是實際上這樣的初值很難得到；

情況二：如果系統是一致完全隨機可控和一致完全隨機可觀測的，則卡爾曼濾波器一定是一致漸進穩定的，此時隨意的選取濾波初值不影響最終估計值（大多數情況下）。

（2）估計均方誤差的等價形式及選用

$P_{k}=(I-K_{k}H_{k})\widehat{P}_{k/k-1}$ ; (1)

$P_{k}=(I-K_{k}H_{k})\widehat{P}_{k/k-1}(I-K_{k}H_{k}) + K_{k}R_{k}K_{k}^{T};$ (2)

$P_{k}=(\widehat{P}_{k/k-1})^{-1}+H_{k}^{T}R_{k}^{-1}H_{K};$ (3)

公式（1）形式簡單，計算量小，但是累積誤差容易使協方差矩陣失去非負正定性甚至對稱性，所以實際中常使用公式（2）；

如果在濾波初值對估計量的統計特性缺乏瞭解，選取濾波初值隨意，則宜採用公式（3）.

（3）卡爾曼濾波的基本方程只適用於系統方程和測量方程均爲離散的情況，但實際的物理系統一般都是連續的，動力學特性用連續微分方程來描述，所以在使用基本方程之前，需要對系統方程和測量方程進行離散化處理。連續系統的離散化處理包括對過程白噪聲的等效離散化處理。

3、連續系統的卡爾曼濾波基本方程

通過對實際的物理系統進行分析後得到的系統模型一般爲連續型的。連續型卡爾曼濾波方程可在離散型卡爾曼濾波器基本方程基礎上推導出來，基本思路：將連續系統離散化，應用離散型卡爾曼濾波器的基本方程和導數概念推導出連續型濾波方程。

採用遞推算法是離散型卡爾曼濾波的最大優點，算法可由計算機執行，不必存儲時間過程中的大量測量信息，連續型卡爾曼濾波則根據連續時間過程中的測量值，採用求解矩陣微分方程的方法估計系統狀態變量的時間連續值，因此算法失去了遞推性。

連續系統的狀態空間表達式爲：

其中：

$E[w(t)]=0,Cov[w(t),w(\gamma )]=Q(t)\delta (t-\gamma );$

$E[v(t)]=0,Cov[v(t),v(\gamma )]=R(t)\delta (t-\gamma );$

$Cov[w(t),v(\gamma )]=S(t)\delta (t-\gamma );$

Q(t)爲非負定矩陣；R(t)爲正定矩陣

與連續系統模型等效的離散系統的數學模型：

$x(t+\Delta t)=\Phi (t+\Delta t,t)x(t)+\Psi (t+\Delta t,t)u(t)+\Gamma (t+\Delta t,t)W(t);$

$z(t+\Delta t)=H(t+\Delta t)x(t+\Delta t)+V(t+\Delta t);$

其中： $\Phi (t+\Delta t,t)=I+A(t)\Delta t+o(\Delta t);$

$\Psi (t+\Delta t,t)=B(t)\Delta t+o(\Delta t);$

$\Gamma (t+\Delta t,t)=G(t)\Delta t+o(\Delta t);$

是零均值分段常值白噪聲過程，其協方差爲：

$Cov[W(t),W(\gamma )]=\frac{Q(t)}{\Delta t}\delta _{kj};Cov[V(t),V(\gamma )]=\frac{R(t)}{\Delta t}\delta _{ij};$

$Cov[W(t),V(\gamma )]=\frac{S(t)}{\Delta t}\delta _{ij};t=t_{0}+k\Delta t,\gamma =t_{0}+j\Delta t;k,j=0,1,2$

進入矩陣J來除去過程噪聲與測量噪聲的相關性

$J(t)=\Gamma (t+\Delta t/t)S(t)R^{-1}(t)$

(1)狀態的一步預測方程：

$\widehat{x}(t+\Delta t/t)= \Phi (t+\Delta t, t)x(t)+\Psi (t + \Delta t,t)u(t)+J(t)(z(t)-H(t)x(t))$

(2)協方差預測方程：

$\widehat{P}(t+\Delta t/t)=[\Phi (t+\Delta t,t)-J(t)H(t)]P(t)[\Phi (t+\Delta t,t)-J(t)H(t)]^{T} + \Gamma (t+\Delta t,t)\frac{Q(t)}{\Delta t}\Gamma ^{T}(t+\Delta t,t)-J(t)\frac{R(t)}{\Delta t}J^{T}(t)$

(3)濾波增益方程（權重）：

$K(t+\Delta t)=\widehat{P}(t+\Delta t /t)H^{T}(t+\Delta t)[H(t+\Delta t/t)\widehat{P}(t+\Delta t/t)H^{T}(t+\Delta t)+\frac{R(t)}{\Delta t}]^{-1}$

(4)濾波估計方程（K時刻的最優值）：

$x(t+\Delta t)=x(t+\Delta t/t) + K(t+\Delta t)z(t+\Delta t/t)=x(t)+\left \{A(t)x(t)+B(t)u(t)+G(t)S(t)R^{-1}(t)[z(t)-H(t)x(t)] \right \}\Delta t + H(t+\Delta t) \left \{ z(t+\Delta t)-H(t+\Delta t)[x(t)+[A(t)x(t)+B(t)u(t)+B(t)u(t)+G(t)S(t)R^{-1}(t)(z(t)-H(t)x(t))]\Delta t] \right \}$

將其變形求極限

$x(t)=A(t)x(t)+B(t)u(t)+K_{s}(t)[z(t)-K(t)x(t)]$

$K_{s}=K(t)+G(t)S(t)R^{-1}(t)$

(5)濾波協方差更新矩陣（K時刻的最優協方差誤差）：

$P(t+\Delta t)=[I-K(t+\Delta t)H(t+ \Delta t)]P(t+\Delta t/t)$

$P(t)=A(t)P(t)+P(t)A^{T}(t)+G(t)Q(t)G^{T}(t)-K_{s}(t)R^{-1}(t)K^{T}_{s}(t)$

普通卡爾曼濾波是在線性高斯情況下利用最小均方誤差獲得目標的動態估計，適用於過程和測量都屬於線性系統，且誤差符合高斯分佈的系統，但是實際上很多系統都存在一定的非線性，表現在過程方程（狀態方程）是非線性的，或者觀測與狀態之間的關係（測量方程）是非線性的。這種情況下就不能使用一般的卡爾曼濾波了，解決的方法是將非線性關係線性近似，將其轉化爲線性問題。

對於非線性問題線性化常用的兩個途徑：

（1）將非線性環節線性化，對高階項採用忽略或者逼近措施；（EKF）

（2）用採樣方法近似非線性分佈；（UKF）

4、擴展卡爾曼濾波器（EKF）

非線性系統模型：

其中：

$E[w(t)]=0,Cov[w(t),w(\gamma )]=Q(t)\delta (t-\gamma );$

$E[v(t)]=0,Cov[v(t),v(\gamma )]=R(t)\delta (t-\gamma );$

$Cov[w(t),v(\gamma )]=0$

假設在t時刻已經獲得系統狀態x的濾波估計 $\widehat{x}(t)$ ，將和在 $\widehat{x}(t)$ 附近線性化，即非線性系統將隨時在新估計的結果附近進行線性化。

將和在 $x(t)=\widehat{x}(t)$ 附近展開成泰勒級數，忽略二階以上的高階項，則獲得線性化方程爲:

$x(t)=f(\widehat{x}(t),t)+\frac{\partial f(\widehat{x}(t),t)}{\partial \widehat{x}(t)}[x(t)-\widehat{x}(t)]+g(\widehat{x}(t),t)w(t)$

$z(t)=h(\widehat{x}(t),t)+\frac{H(\widehat{x}(t),t)}{\partial \widehat{x}(t)}[x(t)-\widehat{x}(t)] +v(t)$

將其變形，取 $F(t)=\frac{\partial f(\widehat{x}(t),t)}{\partial \widehat{x}(t)};K(t)=\frac{\partial H(\widehat{x}(t),t)}{\partial \widehat{x}(t)};G(t)=g(\widehat{x}(t),t)$