深入了解卡爾曼濾波器（3）：多元卡爾曼濾波器

聲明： 本文圖文均來源于https://www.kalmanfilter.net/，如有侵權請聯系删除。

本文由微信公衆号【多元卡爾曼濾波器

前面介紹了一維卡爾曼濾波器，相信大家已經對卡爾曼濾波器有了一定的認識，但是在實際應用中，我們通常需要處理有多元狀态資料的系統。比如，對于一個三維空間中的飛機，我們需要一個9維向量來描述其位置、速度和加速度：

[ x y z x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ ] T \begin{bmatrix} x & y & z & \dot{x} & \dot{y} & \dot{z} & \ddot{x} & \ddot{y} & \ddot{z} \end{bmatrix}^{T} [x​y​z​x˙​y˙​​z˙​x¨​y¨​​z¨​]T

假設我們采用恒加速度（constant acceleration）動态模型，那麼在 n n n時刻飛機的狀态可以寫為

{ x n = x n − 1 + x ˙ n − 1 Δ t + 1 2 x ¨ n − 1 Δ t 2 y n = y n − 1 + y ˙ n − 1 Δ t + 1 2 y ¨ n − 1 Δ t 2 z n = z n − 1 + z ˙ n − 1 Δ t + 1 2 z ¨ n − 1 Δ t 2 x ˙ n = x ˙ n − 1 + x ¨ n − 1 Δ t y ˙ n = y ˙ n − 1 + y ¨ n − 1 Δ t z ˙ n = z ˙ n − 1 + z ¨ n − 1 Δ t x ¨ n = x ¨ n − 1 y ¨ n = y ¨ n − 1 z ¨ n = z ¨ n − 1 \begin{cases} x_{n} = x_{n-1} + \dot{x}_{n-1} \Delta t+ \frac{1}{2}\ddot{x}_{n-1} \Delta t^{2}\\ y_{n} = y_{n-1} + \dot{y}_{n-1} \Delta t+ \frac{1}{2}\ddot{y}_{n-1} \Delta t^{2}\\ z_{n} = z_{n-1} + \dot{z}_{n-1} \Delta t+ \frac{1}{2}\ddot{z}_{n-1} \Delta t^{2}\\ \dot{x}_{n} = \dot{x}_{n-1} + \ddot{x}_{n-1} \Delta t\\ \dot{y}_{n} = \dot{y}_{n-1} + \ddot{y}_{n-1} \Delta t\\ \dot{z}_{n} = \dot{z}_{n-1} + \ddot{z}_{n-1} \Delta t\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1}\\ \end{cases} ⎩ ⎨ ⎧​xn​=xn−1​+x˙n−1​Δt+21​x¨n−1​Δt2yn​=yn−1​+y˙​n−1​Δt+21​y¨​n−1​Δt2zn​=zn−1​+z˙n−1​Δt+21​z¨n−1​Δt2x˙n​=x˙n−1​+x¨n−1​Δty˙​n​=y˙​n−1​+y¨​n−1​Δtz˙n​=z˙n−1​+z¨n−1​Δtx¨n​=x¨n−1​y¨​n​=y¨​n−1​z¨n​=z¨n−1​​

在實際應用中，我們通常會使用矩陣的方式來描述多元資料處理的過程。接下來，我們将用矩陣的方式來介紹多元卡爾曼濾波器的幾個方程。

狀态外推方程

狀态外推方程的作用是在目前時刻 n n n基于現有的知識去預測 n + 1 n+1 n+1時刻系統的狀态，是以也叫狀态預測方程或者狀态轉移方程，其矩陣形式的公式如下：

x ^ n + 1 , n = F x ^ n , n + G u n + w n \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}+w_{n}} x^n+1,n​=Fx^n,n​+Gun​+wn​

其中， x ^ n + 1 , n \boldsymbol{\hat{x}_{n+1,n}} x^n+1,n​是預測的 n + 1 n+1 n+1時刻的系統狀态向量； x ^ n , n \boldsymbol{\hat{x}_{n,n}} x^n,n​是估計的 n n n時刻的系統狀态向量； u n \boldsymbol{u_{n}} un​是控制變量（輸入變量），對系統來說是一個可測量的（确定性的）輸入； w n \boldsymbol{w_{n}} wn​是過程噪聲，是會影響系統狀态的不可測量的輸入量； F \boldsymbol{F} F是狀态轉移矩陣； G \boldsymbol{G} G是控制矩陣，也叫輸入轉移矩陣，用于将控制變量映射為狀态變量。

以前面的飛機為例，用狀态向量 x n ^ \hat{x_{n}} xn​^​描述其在三維空間中的位置、速度和加速度

x ^ n = [ x ^ n y ^ n z ^ n x ˙ ^ n y ˙ ^ n z ˙ ^ n x ¨ ^ n y ¨ ^ n z ¨ ^ n ] T \boldsymbol{\hat{x}_{n}}= \begin{bmatrix} \hat{x}_{n} & \hat{y}_{n} & \hat{z}_{n} & \hat{\dot{x}}_{n} & \hat{\dot{y}}_{n} & \hat{\dot{z}}_{n} & \hat{\ddot{x}}_{n} & \hat{\ddot{y}}_{n} & \hat{\ddot{z}}_{n} \end{bmatrix}^{T} x^n​=[x^n​​y^​n​​z^n​​x˙^n​​y˙​^​n​​z˙^n​​x¨^n​​y¨​^​n​​z¨^n​​]T

假如采用恒加速模型，如果不考慮有控制變量輸入，那麼狀态外推方程為

x ^ n + 1 , n = F x ^ n , n \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}} x^n+1,n​=Fx^n,n​

狀态轉移方程為

F = [ 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 ] \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{matrix} \right] F=⎣ ⎡​100000000​010000000​001000000​Δt00100000​0Δt0010000​00Δt001000​0.5Δt200Δt00100​00.5Δt200Δt0010​000.5Δt200Δt001​⎦ ⎤​

那麼

[ x ^ n + 1 , n y ^ n + 1 , n z ^ n + 1 , n x ˙ ^ n + 1 , n y ˙ ^ n + 1 , n z ˙ ^ n + 1 , n x ¨ ^ n + 1 , n y ¨ ^ n + 1 , n z ¨ ^ n + 1 , n ] = [ 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0.5 Δ t 2 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 Δ t 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 ] [ x ^ n , n y ^ n , n z ^ n , n x ˙ ^ n , n y ˙ ^ n , n z ˙ ^ n , n x ¨ ^ n , n y ¨ ^ n , n z ¨ ^ n , n ] \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \hat{\ddot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n,n}\\ \end{matrix} \right] ⎣ ⎡​x^n+1,n​y^​n+1,n​z^n+1,n​x˙^n+1,n​y˙​^​n+1,n​z˙^n+1,n​x¨^n+1,n​y¨​^​n+1,n​z¨^n+1,n​​⎦ ⎤​=⎣ ⎡​100000000​010000000​001000000​Δt00100000​0Δt0010000​00Δt001000​0.5Δt200Δt00100​00.5Δt200Δt0010​000.5Δt200Δt001​⎦ ⎤​⎣ ⎡​x^n,n​y^​n,n​z^n,n​x˙^n,n​y˙​^​n,n​z˙^n,n​x¨^n,n​y¨​^​n,n​z¨^n,n​​⎦ ⎤​

算得結果

{ x ^ n + 1 , n = x ^ n , n + x ˙ ^ n , n Δ t + 1 2 x ¨ ^ n , n Δ t 2 y ^ n + 1 , n = y ^ n , n + y ˙ ^ n , n Δ t + 1 2 y ¨ ^ n , n Δ t 2 z ^ n + 1 , n = z ^ n , n + z ˙ ^ n , n Δ t + 1 2 z ¨ ^ n , n Δ t 2 x ˙ ^ n + 1 , n = x ˙ ^ n , n + x ¨ ^ n , n Δ t y ˙ ^ n + 1 , n = y ˙ ^ n , n + y ¨ ^ n , n Δ t z ˙ ^ n + 1 , n = z ˙ ^ n , n + z ¨ ^ n , n Δ t x ¨ ^ n + 1 , n = x ¨ ^ n , n y ¨ ^ n + 1 , n = y ¨ ^ n , n z ¨ ^ n + 1 , n = z ¨ ^ n , n \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n} \Delta t^{2} \\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n} \Delta t^{2} \\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{z}}_{n,n} \Delta t^{2} \\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t \\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t \\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n} + \hat{\ddot{z}}_{n,n} \Delta t \\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n} \\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n} \\ \hat{\ddot{z}}_{n+1,n} = \hat{\ddot{z}}_{n,n} \\ \end{cases} ⎩ ⎨ ⎧​x^n+1,n​=x^n,n​+x˙^n,n​Δt+21​x¨^n,n​Δt2y^​n+1,n​=y^​n,n​+y˙​^​n,n​Δt+21​y¨​^​n,n​Δt2z^n+1,n​=z^n,n​+z˙^n,n​Δt+21​z¨^n,n​Δt2x˙^n+1,n​=x˙^n,n​+x¨^n,n​Δty˙​^​n+1,n​=y˙​^​n,n​+y¨​^​n,n​Δtz˙^n+1,n​=z˙^n,n​+z¨^n,n​Δtx¨^n+1,n​=x¨^n,n​y¨​^​n+1,n​=y¨​^​n,n​z¨^n+1,n​=z¨^n,n​​

假如我們有加速度傳感器可以提供飛機的加速度資訊作為系統的輸入，所提供的加速度測量值 u n \boldsymbol{u_{n}} un​為

u n = [ x ¨ n y ¨ n z ¨ n ] \boldsymbol{u_{n}}= \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] un​=⎣ ⎡​x¨n​y¨​n​z¨n​​⎦ ⎤​

那麼狀态外推方程為

x ^ n + 1 , n = F x ^ n , n + G u n , n \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n,n}} x^n+1,n​=Fx^n,n​+Gun,n​

轉移矩陣 F \boldsymbol{F} F和控制矩陣 G \boldsymbol{G} G分别如下：

F = [ 1 0 0 Δ t 0 0 0 1 0 0 Δ t 0 0 0 1 0 0 Δ t 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] F=⎣ ⎡​100000​010000​001000​Δt00100​0Δt0010​00Δt001​⎦ ⎤​

G = [ 0.5 Δ t 2 0 0 0 0.5 Δ t 2 0 0 0 0.5 Δ t 2 Δ t 0 0 0 Δ t 0 0 0 Δ t ] \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] G=⎣ ⎡​0.5Δt200Δt00​00.5Δt200Δt0​000.5Δt200Δt​⎦ ⎤​

那麼

[ x ^ n + 1 , n y ^ n + 1 , n z ^ n + 1 , n x ˙ ^ n + 1 , n y ˙ ^ n + 1 , n z ˙ ^ n + 1 , n ] = [ 1 0 0 Δ t 0 0 0 1 0 0 Δ t 0 0 0 1 0 0 Δ t 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] [ x ^ n , n y ^ n , n z ^ n , n x ˙ ^ n , n y ˙ ^ n , n z ˙ ^ n , n ] + [ 0.5 Δ t 2 0 0 0 0.5 Δ t 2 0 0 0 0.5 Δ t 2 Δ t 0 0 0 Δ t 0 0 0 Δ t ] [ x ¨ n y ¨ n z ¨ n ] \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] ⎣ ⎡​x^n+1,n​y^​n+1,n​z^n+1,n​x˙^n+1,n​y˙​^​n+1,n​z˙^n+1,n​​⎦ ⎤​=⎣ ⎡​100000​010000​001000​Δt00100​0Δt0010​00Δt001​⎦ ⎤​⎣ ⎡​x^n,n​y^​n,n​z^n,n​x˙^n,n​y˙​^​n,n​z˙^n,n​​⎦ ⎤​+⎣ ⎡​0.5Δt200Δt00​00.5Δt200Δt0​000.5Δt200Δt​⎦ ⎤​⎣ ⎡​x¨n​y¨​n​z¨n​​⎦ ⎤​

協方差外推方程

協方差外推方程如下：

P n + 1 , n = F P n , n F T + Q \boldsymbol{P_{n+1,n} = FP_{n,n}F^{T} + Q} Pn+1,n​=FPn,n​FT+Q

其中， P n , n \boldsymbol{P_{n,n}} Pn,n​描述了目前估計值的不确定度，是目前系統狀态的協方差矩陣； P n + 1 , n \boldsymbol{P_{n+1,n}} Pn+1,n​描述了目前預測值的不确定度，是預測的系統狀态的協方差矩陣； F \boldsymbol{F} F是狀态轉移矩陣； Q \boldsymbol{Q} Q是過程噪聲協方差矩陣。

現在我們從頭來推導一下這個方程。

假設過程噪聲為零（ Q = 0 \boldsymbol{Q}=0 Q=0），那麼

P n + 1 , n = F P n , n F T \boldsymbol{P_{n+1,n} = FP_{n,n}F^{T}} Pn+1,n​=FPn,n​FT

由前面的背景知識我們知道系統狀态向量 x \boldsymbol{x} x的協方差矩陣為

C O V ( x ) = E ( ( x − μ x ) ( x − μ x ) T ) COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right) COV(x)=E((x−μx​)(x−μx​)T)

是以

P n , n = E ( ( x ^ n , n − μ x n , n ) ( x ^ n , n − μ x n , n ) T ) \boldsymbol{P_{n,n}} = E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) Pn,n​=E((x^n,n​−μxn,n​​)(x^n,n​−μxn,n​​)T)

根據狀态外推方程

x ^ n + 1 , n = F x ^ n , n + G u ^ n , n \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}} x^n+1,n​=Fx^n,n​+Gu^n,n​

可得

P n + 1 , n = E ( ( x ^ n + 1 , n − μ x n + 1 , n ) ( x ^ n + 1 , n − μ x n + 1 , n ) T ) = E ( ( F x ^ n , n + G u ^ n , n − F μ x n , n − G u ^ n , n ) ( F x ^ n , n + G u ^ n , n − F μ x n , n − G u ^ n , n ) T ) = E ( F ( x ^ n , n − μ x n , n ) ( F ( x ^ n , n − μ x n , n ) ) T ) = E ( F ( x ^ n , n − μ x n , n ) ( x ^ n , n − μ x n , n ) T F T ) = F E ( ( x ^ n , n − μ x n , n ) ( x ^ n , n − μ x n , n ) T ) F T = F P n , n F T \boldsymbol{P_{n+1,n}} = E \left( \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right) \left( \boldsymbol{\hat{x}_{n+1,n} - \mu_{x_{n+1,n}}} \right)^{T} \right) \\ = E \left( \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right) \left( \boldsymbol{F\hat{x}_{n,n} + G\hat{u}_{n,n} - F\mu_{x_{n,n}} - G\hat{u}_{n,n}} \right)^{T} \right) \\ = E \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \right)^{T} \right) \\ = E \left(\boldsymbol{F} \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \boldsymbol{F^{T}} \right) \\ = \boldsymbol{F} E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \boldsymbol{F^{T}} \\ = \boldsymbol{F P_{n,n} F^{T}} Pn+1,n​=E((x^n+1,n​−μxn+1,n​​)(x^n+1,n​−μxn+1,n​​)T)=E((Fx^n,n​+Gu^n,n​−Fμxn,n​​−Gu^n,n​)(Fx^n,n​+Gu^n,n​−Fμxn,n​​−Gu^n,n​)T)=E(F(x^n,n​−μxn,n​​)(F(x^n,n​−μxn,n​​))T)=E(F(x^n,n​−μxn,n​​)(x^n,n​−μxn,n​​)TFT)=FE((x^n,n​−μxn,n​​)(x^n,n​−μxn,n​​)T)FT=FPn,n​FT

該如何建構過程噪聲協方差矩陣 Q \boldsymbol{Q} Q呢？

假設系統的狀态為位置、速度和加速度，分别用 x x x， v v v和 a a a來表示。對于恒速模型來說，過程噪聲的協方差矩陣為

Q = [ V ( x ) C O V ( x , v ) C O V ( v , x ) V ( v ) ] \boldsymbol{Q} = \left[ \begin{matrix} V(x) & COV(x,v) \\ COV(v,x) & V(v) \\ \end{matrix} \right] Q=[V(x)COV(v,x)​COV(x,v)V(v)​]

我們将用随機加速度方差 σ a 2 \sigma^{2}_{a} σa2​來表示位置、速度的方差和協方差。由前面的背景知識可以知道

V ( v ) = σ v 2 = E ( v 2 ) − μ v 2 = E ( ( a Δ t ) 2 ) − ( μ a Δ t ) 2 = Δ t 2 ( E ( a 2 ) − μ a 2 ) = Δ t 2 σ a 2 V(v) = \sigma^{2}_{v} = E\left(v^{2}\right) - \mu_{v}^{2} = E \left( \left( a\Delta t\right)^{2}\right) - \left(\mu_{a}\Delta t\right)^{2} = \Delta t^{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \Delta t^{2}\sigma^{2}_{a} V(v)=σv2​=E(v2)−μv2​=E((aΔt)2)−(μa​Δt)2=Δt2(E(a2)−μa2​)=Δt2σa2​

V ( x ) = σ x 2 = E ( x 2 ) − μ x 2 = E ( ( 1 2 a Δ t 2 ) 2 ) − ( 1 2 μ a Δ t 2 ) 2 = Δ t 4 4 ( E ( a 2 ) − μ a 2 ) = Δ t 4 4 σ a 2 V(x) = \sigma^{2}_{x} = E\left(x^{2}\right) - \mu_{x}^{2} = E \left( \left( \frac{1}{2}a\Delta t^{2}\right)^{2}\right) - \left(\frac{1}{2}\mu_{a}\Delta t^{2}\right)^{2} = \frac{\Delta t^{4}}{4}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{4}}{4}\sigma^{2}_{a} V(x)=σx2​=E(x2)−μx2​=E((21​aΔt2)2)−(21​μa​Δt2)2=4Δt4​(E(a2)−μa2​)=4Δt4​σa2​

C O V ( x , v ) = C O V ( v , x ) = E ( x v ) − μ x μ v = E ( 1 2 a Δ t 2 a Δ t ) − ( 1 2 μ a Δ t 2 μ a Δ t ) = Δ t 3 2 ( E ( a 2 ) − μ a 2 ) = Δ t 3 2 σ a 2 COV(x,v) = COV(v,x) = E\left(xv\right) - \mu_{x}\mu_{v} = E\left( \frac{1}{2}a\Delta t^{2}a\Delta t\right) - \left( \frac{1}{2}\mu_{a}\Delta t^{2}\mu_{a}\Delta t\right) = \frac{\Delta t^{3}}{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{3}}{2}\sigma^{2}_{a} COV(x,v)=COV(v,x)=E(xv)−μx​μv​=E(21​aΔt2aΔt)−(21​μa​Δt2μa​Δt)=2Δt3​(E(a2)−μa2​)=2Δt3​σa2​

那麼

Q = σ a 2 [ Δ t 4 4 Δ t 3 2 Δ t 3 2 Δ t 2 ] \boldsymbol{Q} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] Q=σa2​[4Δt4​2Δt3​​2Δt3​Δt2​]

這樣求矩陣 Q \boldsymbol{Q} Q比較麻煩，我們可以通過下面的方式快速求出來。

如果動态模型不包含控制輸入，那麼我們可以直接通過狀态轉移矩陣将加速度的随機方差 σ a 2 \sigma^{2}_{a} σa2​映射到動态模型中。定義矩陣 Q a \boldsymbol{Q}_{a} Qa​為:

Q a = [ 0 0 0 0 0 0 0 0 1 ] σ a 2 \boldsymbol{Q}_{a} = \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \sigma^{2}_{a} Qa​=⎣ ⎡​000​000​001​⎦ ⎤​σa2​

那麼過程噪聲協方差矩陣 Q \boldsymbol{Q} Q可由下面的公式得到

Q = F Q a F T \boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F^{T}} Q=FQa​FT

由運動模型可知狀态轉移矩陣為

F = [ 1 Δ t Δ t 2 2 0 1 Δ t 0 0 1 ] \boldsymbol{F} = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] F=⎣ ⎡​100​Δt10​2Δt2​Δt1​⎦ ⎤​

則

如果動态模型包含控制輸入，那麼過程噪聲協方差矩陣 Q \boldsymbol{Q} Q可由下面的公式得到：

Q = G σ a 2 G T \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} Q=Gσa2​GT

其中 G \boldsymbol{G} G為控制矩陣（或者叫輸入轉移矩陣）。由運動模型可知

G = [ Δ t 2 2 Δ t ] \boldsymbol{G} = \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] G=[2Δt2​Δt​]

那麼

Q = G σ a 2 G T = σ a 2 G G T = σ a 2 [ Δ t 2 2 Δ t ] [ Δ t 2 2 Δ t ] = σ a 2 [ Δ t 4 4 Δ t 3 2 Δ t 3 2 Δ t 2 ] \boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} = \sigma^{2}_{a}\boldsymbol{G}\boldsymbol{G^{T}} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \frac{\Delta t^{2}}{2} & \Delta t \\ \end{matrix} \right] = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] Q=Gσa2​GT=σa2​GGT=σa2​[2Δt2​Δt​][2Δt2​​Δt​]=σa2​[4Δt4​2Δt3​​2Δt3​Δt2​]

狀态更新方程

狀态更新方程的表達式如下所示：

x ^ n , n = x ^ n , n − 1 + K n ( z n − H x ^ n , n − 1 ) \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )} x^n,n​=x^n,n−1​+Kn​(zn​−Hx^n,n−1​)

其中 x ^ n , n \boldsymbol{\hat{x}_{n,n}} x^n,n​是 n n n時刻估計的系統狀态向量； x ^ n , n − 1 \boldsymbol{\hat{x}_{n,n-1}} x^n,n−1​是在 n − 1 n-1 n−1時刻預測的系統狀态向量； K n \boldsymbol{K_{n}} Kn​是卡爾曼增益； z n \boldsymbol{z_{n}} zn​是測量值； H H H為觀測矩陣。

在前面測量金條重量的例子中我們已經介紹了狀态更新方程，在這裡就不做更多介紹了。在多元的情況下，我們需要注意的是矩陣的次元。舉個例子，假如系統的狀态是一個5維的向量，但是隻有第1、3、5維是可測量的：

x ( n ) = [ x 1 x 2 x 3 x 4 x 5 ] z ( n ) = [ z 1 z 3 z 5 ] \boldsymbol{x(n)} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ x_{3}\\ x_{4}\\ x_{5}\\ \end{matrix} \right] \boldsymbol{z(n)} =\left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right] x(n)=⎣ ⎡​x1​x2​x3​x4​x5​​⎦ ⎤​z(n)=⎣ ⎡​z1​z3​z5​​⎦ ⎤​

那麼觀測矩陣應該是一個 3 × 5 3 \times 5 3×5的矩陣：

H = [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] \boldsymbol{H}= \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] H=⎣ ⎡​100​000​010​000​001​⎦ ⎤​

那麼觀測殘差 ( z n − H x ^ n , n − 1 ) \left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right) (zn​−Hx^n,n−1​)為

( z n − H x ^ n , n − 1 ) = [ z 1 z 3 z 5 ] − [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] [ x ^ 1 x ^ 2 x ^ 3 x ^ 4 x ^ 5 ] = [ ( z 1 − x ^ 1 ) ( z 3 − x ^ 3 ) ( z 5 − x ^ 5 ) ] \left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right) = \left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right] - \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{1}\\ \hat{x}_{2}\\ \hat{x}_{3}\\ \hat{x}_{4}\\ \hat{x}_{5}\\ \end{matrix} \right] = \left[ \begin{matrix} (z_{1} - \hat{x}_{1})\\ (z_{3} - \hat{x}_{3})\\ (z_{5} - \hat{x}_{5})\\ \end{matrix} \right] (zn​−Hx^n,n−1​)=⎣ ⎡​z1​z3​z5​​⎦ ⎤​−⎣ ⎡​100​000​010​000​001​⎦ ⎤​⎣ ⎡​x^1​x^2​x^3​x^4​x^5​​⎦ ⎤​=⎣ ⎡​(z1​−x^1​)(z3​−x^3​)(z5​−x^5​)​⎦ ⎤​

此時卡爾曼增益的次元應該是 5 × 3 5 \times 3 5×3。

協方差更新方程

協方差更新方程的表達式如下：

P n , n = ( I − K n H ) P n , n − 1 ( I − K n H ) T + K n R n K n T \boldsymbol{ P_{n,n} = \left( I - K_{n}H \right) P_{n,n-1} \left( I - K_{n}H \right)^{T} + K_{n}R_{n}K_{n}^{T} } Pn,n​=(I−Kn​H)Pn,n−1​(I−Kn​H)T+Kn​Rn​KnT​

其中， P n , n \boldsymbol{P_{n,n} } Pn,n​為目前狀态估計值的協方差矩陣； P n , n − 1 \boldsymbol{P_{n,n-1} } Pn,n−1​是目前狀态的先驗估計（基于前一個狀态的預測）的協方差矩陣； K n \boldsymbol{K_{n}} Kn​為卡爾曼增益； H H H為觀測矩陣； R n R_{n} Rn​為測量噪聲的協方差矩陣。

接下來，我們将對這個公式進行詳細推導。推導過程中會用到下面幾個公式：

方程	說明
x ^ n , n = x ^ n , n − 1 + K n ( z n − H x ^ n , n − 1 ) \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )} x^n,n​=x^n,n−1​+Kn​(zn​−Hx^n,n−1​)	狀态更新方程
z n = H x n + v n \boldsymbol{z_{n} = Hx_{n} + v_{n}} zn​=Hxn​+vn​	測量方程， v n \boldsymbol{v_{n}} vn​為測量噪聲
P n , n = E ( e n e n T ) = E ( ( x n − x ^ n , n ) ( x n − x ^ n , n ) T ) \boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right) Pn,n​=E(en​enT​)=E((xn​−x^n,n​)(xn​−x^n,n​)T)	狀态協方差矩陣
R n = E ( v n v n T ) \boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right) Rn​=E(vn​vnT​)	測量噪聲協方差矩陣

對于每一個估計，估計誤差 e n \boldsymbol{e_{n}} en​為

e n = x n − x ^ n , n = x n − x ^ n , n − 1 − K n ( z n − H x ^ n , n − 1 ) = x n − x ^ n , n − 1 − K n ( H x n + v n − H x ^ n , n − 1 ) = x n − x ^ n , n − 1 − K n H x n − K n v n + K n H x ^ n , n − 1 = x n − x ^ n , n − 1 − K n H ( x n − x ^ n , n − 1 ) − K n v n = ( I − K n H ) ( x n − x ^ n , n − 1 ) − K n v n \boldsymbol{e_{n}} = \boldsymbol{x_{n} - \hat{x}_{n,n}} \\ = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n} \left( z_{n} - H \hat{x}_{n,n-1} \right)} \\ = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n} \left( Hx_{n} + v_{n} - H \hat{x}_{n,n-1} \right)} \\ = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}Hx_{n} - K_{n}v_{n} + K_{n}H \hat{x}_{n,n-1}} \\ = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}H\left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} \\ = \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} en​=xn​−x^n,n​=xn​−x^n,n−1​−Kn​(zn​−Hx^n,n−1​)=xn​−x^n,n−1​−Kn​(Hxn​+vn​−Hx^n,n−1​)=xn​−x^n,n−1​−Kn​Hxn​−Kn​vn​+Kn​Hx^n,n−1​=xn​−x^n,n−1​−Kn​H(xn​−x^n,n−1​)−Kn​vn​=(I−Kn​H)(xn​−x^n,n−1​)−Kn​vn​

再由上式來推導估計值的協方差矩陣 P n , n \boldsymbol{P_{n,n}} Pn,n​

( x n − x ^ n , n − 1 ) (\boldsymbol{ x_{n} - \hat{x}_{n,n-1}}) (xn​−x^n,n−1​)是先驗估計與真實值之間的誤差，它與目前時刻的測量噪聲 v n \boldsymbol{ v_{n} } vn​是不相關的。有前面的背景知識我們知道，兩個互相獨立變量乘積的期望值為零，是以

E ( ( I − K n H ) ( x n − x ^ n , n − 1 ) ( K n v n ) T ) = 0 E ( K n v n ( x n − x ^ n , n − 1 ) T ( I − K n H ) T ) = 0 {E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} }\right) = 0} \\ {E \left( \boldsymbol{ K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) = 0} E((I−Kn​H)(xn​−x^n,n−1​)(Kn​vn​)T)=0E(Kn​vn​(xn​−x^n,n−1​)T(I−Kn​H)T)=0

那麼

P n , n = E ( ( I − K n H ) ( x n − x ^ n , n − 1 ) ( x n − x ^ n , n − 1 ) T ( I − K n H ) T ) + E ( K n v n v n T K n T ) = ( I − K n H ) E ( ( x n − x ^ n , n − 1 ) ( x n − x ^ n , n − 1 ) T ) ( I − K n H ) T + K n E ( v n v n T ) K n T \boldsymbol{P_{n,n}} = E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) + E \left({\boldsymbol{ K_{n}v_{n} v_{n}^{T} K_{n}^{T} }}\right) \\ = \boldsymbol{ \left( I - K_{n}H \right)} {E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right)} \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} { E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) } \boldsymbol{ K_{n}^{T} } \\ Pn,n​=E((I−Kn​H)(xn​−x^n,n−1​)(xn​−x^n,n−1​)T(I−Kn​H)T)+E(Kn​vn​vnT​KnT​)=(I−Kn​H)E((xn​−x^n,n−1​)(xn​−x^n,n−1​)T)(I−Kn​H)T+Kn​E(vn​vnT​)KnT​

由

E ( ( x n − x ^ n , n − 1 ) ( x n − x ^ n , n − 1 ) T ) = P n , n − 1 E ( v n v n T ) = R n {E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right) = \boldsymbol{P_{n,n-1}}} \\ { E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) = \boldsymbol{R_{n}}} E((xn​−x^n,n−1​)(xn​−x^n,n−1​)T)=Pn,n−1​E(vn​vnT​)=Rn​

可得

P n , n = ( I − K n H ) P n , n − 1 ( I − K n H ) T + K n R n K n T \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} {\boldsymbol{ P_{n,n-1}} } \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} { \boldsymbol{ R_{n} } } \boldsymbol{ K_{n}^{T} } Pn,n​=(I−Kn​H)Pn,n−1​(I−Kn​H)T+Kn​Rn​KnT​

好了，終于把這個公式推導完了。

卡爾曼增益

卡爾曼增益的表達式如下：

K n = P n , n − 1 H T ( H P n , n − 1 H T + R n ) − 1 \boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} } Kn​=Pn,n−1​HT(HPn,n−1​HT+Rn​)−1

其中， K n \boldsymbol{K_{n}} Kn​為卡爾曼增益； P n , n − 1 \boldsymbol{P_{n,n-1} } Pn,n−1​是目前狀态的先驗估計（基于前一個狀态的預測）的協方差矩陣； H \boldsymbol{H} H為觀測矩陣； R n \boldsymbol{R_{n}} Rn​為測量噪聲的協方差矩陣。

在開始推導卡爾曼增益之前，讓我們先對協方差更新公式再做一些變換：

P n , n = ( I − K n H ) P n , n − 1 ( I − K n H ) T + K n R n K n T = ( I − K n H ) P n , n − 1 ( I − ( K n H ) T ) + K n R n K n T = ( I − K n H ) P n , n − 1 ( I − H T K n T ) + K n R n K n T = ( P n , n − 1 − K n H P n , n − 1 ) ( I − H T K n T ) + K n R n K n T = P n , n − 1 − P n , n − 1 H T K n T − K n H P n , n − 1 + K n H P n , n − 1 H T K n T + K n R n K n T = P n , n − 1 − P n , n − 1 H T K n T − K n H P n , n − 1 + K n ( H P n , n − 1 H T + R n ) K n T \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} {\boldsymbol{ \left( I - K_{n}H \right)^{T}}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} {\boldsymbol{ \left( I - \left( K_{n}H \right)^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ = {\boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}}} {\boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ = {\boldsymbol{ \left( P_{n,n-1} - K_{n}H P_{n,n-1} \right)}} \boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \\ = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{K_{n}H P_{n,n-1}H^{T}K_{n}^{T} + K_{n} R_{n} K_{n}^{T} } } \\ = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + {\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } } Pn,n​=(I−Kn​H)Pn,n−1​(I−Kn​H)T+Kn​Rn​KnT​=(I−Kn​H)Pn,n−1​(I−(Kn​H)T)+Kn​Rn​KnT​=(I−Kn​H)Pn,n−1​(I−HTKnT​)+Kn​Rn​KnT​=(Pn,n−1​−Kn​HPn,n−1​)(I−HTKnT​)+Kn​Rn​KnT​=Pn,n−1​−Pn,n−1​HTKnT​−Kn​HPn,n−1​+Kn​HPn,n−1​HTKnT​+Kn​Rn​KnT​=Pn,n−1​−Pn,n−1​HTKnT​−Kn​HPn,n−1​+Kn​(HPn,n−1​HT+Rn​)KnT​

卡爾曼濾波器是最優濾波器，是以我們需要尋求能最小化估計方差的卡爾曼增益。為了最小化估計方差，我們需要最小化狀态協方差矩陣 P n , n \boldsymbol{P_{n,n}} Pn,n​的主對角線，也就是最小化它的迹 t r ( P n , n ) tr(\boldsymbol{P_{n,n}}) tr(Pn,n​)。為了求得 t r ( P n , n ) tr(\boldsymbol{P_{n,n}}) tr(Pn,n​)的極小值，我們需要求迹 t r ( P n , n ) tr(\boldsymbol{P_{n,n}}) tr(Pn,n​)關于卡爾曼增益 K n \boldsymbol{K_{n}} Kn​的導數，并設導數為零。

t r ( P n , n ) = t r ( P n , n − 1 ) − t r ( P n , n − 1 H T K n T ) − t r ( K n H P n , n − 1 ) + t r ( K n ( H P n , n − 1 H T + R n ) K n T ) = t r ( P n , n − 1 ) − 2 t r ( K n H P n , n − 1 ) + t r ( K n ( H P n , n − 1 H T + R n ) K n T ) tr\left( \boldsymbol{P_{n,n}} \right) = tr\left( \boldsymbol{P_{n,n-1}}\right) - {tr\left( \boldsymbol{ P_{n,n-1}H^{T}K_{n}^{T} }\right) - tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \\ = tr\left( \boldsymbol{P_{n,n-1}}\right) - { 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) tr(Pn,n​)=tr(Pn,n−1​)−tr(Pn,n−1​HTKnT​)−tr(Kn​HPn,n−1​)+tr(Kn​(HPn,n−1​HT+Rn​)KnT​)=tr(Pn,n−1​)−2tr(Kn​HPn,n−1​)+tr(Kn​(HPn,n−1​HT+Rn​)KnT​)

求導數并令其為零：

d ( t r ( P n , n ) ) d K n = d ( t r ( P n , n − 1 ) ) d K n − d ( 2 t r ( K n H P n , n − 1 ) ) d K n + d ( t r ( K n ( H P n , n − 1 H T + R n ) K n T ) ) d K n = 0 − 2 ( H P n , n − 1 ) T + 2 K n ( H P n , n − 1 H T + R n ) = 0 \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} = {\frac{d\left( tr\left( \boldsymbol{P_{n,n-1}}\right) \right)}{d\boldsymbol{K_{n}}}} - { \frac{d\left( 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right) \right) }{d\boldsymbol{K_{n}}} } + {\frac{ d\left( tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \right) }{d\boldsymbol{K_{n}}}} \\ = {0} - { 2 \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } + {\boldsymbol{2K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } \\ = 0 dKn​d(tr(Pn,n​))​=dKn​d(tr(Pn,n−1​))​−dKn​d(2tr(Kn​HPn,n−1​))​+dKn​d(tr(Kn​(HPn,n−1​HT+Rn​)KnT​))​=0−2(HPn,n−1​)T+2Kn​(HPn,n−1​HT+Rn​)=0

這裡用到了兩個計算公式：

d d A ( t r ( A B ) ) = B T d d A ( t r ( A B A T ) ) = 2 A B {\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{AB} \right) \right) = \boldsymbol{B}^{T} } \\ {\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{ABA^{T}} \right) \right) = 2\boldsymbol{AB} } dAd​(tr(AB))=BTdAd​(tr(ABAT))=2AB

由上面的導數為零，可得

( H P n , n − 1 ) T = K n ( H P n , n − 1 H T + R n ) { \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } = {\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } (HPn,n−1​)T=Kn​(HPn,n−1​HT+Rn​)

是以可求得卡爾曼增益 K n \boldsymbol{K_{n}} Kn​：

K n = ( H P n , n − 1 ) T ( H P n , n − 1 H T + R n ) − 1 = P n , n − 1 T H T ( H P n , n − 1 H T + R n ) − 1 = P n , n − 1 H T ( H P n , n − 1 H T + R n ) − 1 \boldsymbol{K_{n}} = \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \\ = \boldsymbol{ P_{n,n-1}^{T} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \\ = \boldsymbol{ P_{n,n-1} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } Kn​=(HPn,n−1​)T(HPn,n−1​HT+Rn​)−1=Pn,n−1T​HT(HPn,n−1​HT+Rn​)−1=Pn,n−1​HT(HPn,n−1​HT+Rn​)−1

因為協方差矩陣是對稱矩陣，是以 P n , n − 1 T = P n , n − 1 \boldsymbol{ P_{n,n-1}^{T}} = \boldsymbol{ P_{n,n-1}} Pn,n−1T​=Pn,n−1​。