卡爾曼濾波器的前言

首先，介紹條件概率，
思考這一個問題：在一個學校里面，學生里面的男生占比0.6，女生占比0.4，男生留長發的概率是0.2，男生短發的概率是0.8，女生留長發的概率是0.9，女生短發的概率是0.1，現在你從背影看到一個留長發的人，判斷他（她）是男生還是女生？
P ( s = 男 生 ) = 0.6 P ( s = 女 生 ) = 0.4 P(s=男生)=0.6 \quad P(s=女生)=0.4 P(s=男生)=0.6P(s=女生)=0.4

P ( o = 短 發 ∣ s = 男 生 ) = 0.8 P ( o = 長 發 ∣ s = 男 生 ) = 0.2 P ( o = 短 發 ∣ s = 女 生 ) = 0.1 P ( o = 長 發 ∣ s = 女 生 ) = 0.9 P(o=短發|s=男生)=0.8 \quad P(o=長發|s=男生)=0.2 \\ P(o=短發|s=女生)=0.1 \quad P(o=長發|s=女生)=0.9 P(o=短發∣s=男生)=0.8P(o=長發∣s=男生)=0.2P(o=短發∣s=女生)=0.1P(o=長發∣s=女生)=0.9

那么
P ( s = 男 生 ∣ o = 長 發 ) = P ( s = 男 生 , o = 長 發 ) P ( o = 長 發 ) = P ( o = 長 發 ∣ s = 男 生 ) P ( s = 男 生 ) P ( o = 長 發 ∣ s = 男 生 ) P ( s = 男 生 ) + P ( o = 長 發 ∣ s = 女 生 ) P ( s = 女 生 ) = 0.2 × 0.6 0.2 × 0.6 + 0.9 × 0.4 = 0.25 P(s=男生|o=長發)=\frac{P(s=男生,o=長發)}{P(o=長發)}=\frac{P(o=長發|s=男生)P(s=男生)}{P(o=長發|s=男生)P(s=男生)+P(o=長發|s=女生)P(s=女生)}=\frac{0.2×0.6}{0.2×0.6+0.9×0.4}=0.25 P(s=男生∣o=長發)=P(o=長發)P(s=男生,o=長發)?=P(o=長發∣s=男生)P(s=男生)+P(o=長發∣s=女生)P(s=女生)P(o=長發∣s=男生)P(s=男生)?=0.2×0.6+0.9×0.40.2×0.6?=0.25

P ( s = 女 生 ∣ o = 長 發 ) = P ( s = 女 生 , o = 長 發 ) P ( o = 長 發 ) = P ( o = 長 發 ∣ s = 女 生 ) P ( s = 女 生 ) P ( o = 長 發 ∣ s = 男 生 ) P ( s = 男 生 ) + P ( o = 長 發 ∣ s = 女 生 ) P ( s = 女 生 ) = 0.9 × 0.4 0.2 × 0.6 + 0.9 × 0.4 = 0.75 P(s=女生|o=長發)=\frac{P(s=女生,o=長發)}{P(o=長發)}=\frac{P(o=長發|s=女生)P(s=女生)}{P(o=長發|s=男生)P(s=男生)+P(o=長發|s=女生)P(s=女生)}=\frac{0.9×0.4}{0.2×0.6+0.9×0.4}=0.75 P(s=女生∣o=長發)=P(o=長發)P(s=女生,o=長發)?=P(o=長發∣s=男生)P(s=男生)+P(o=長發∣s=女生)P(s=女生)P(o=長發∣s=女生)P(s=女生)?=0.2×0.6+0.9×0.40.9×0.4?=0.75
所以，當看到背影是長發的同學，是女生的概率更大一些，有0.75，所以我們猜測這是個女生，
PS：根據上邊這個例子，直觀點的解釋如下，
假設這個學校有1000個學生，按照上面的概率，那么男生約600人，女生約400人，留長發的女生約360人，留短發的女生約40人，留長發的男生約120人，留短發的男生約480人，當看到一個背影長發的同學，這個同學肯定從長發男生和長發女生這個總體選出來的一個，長發群體總共480人（男生120人，女生360人），那么我們肯定推斷這個選出來的背影是長發的同學肯定是女生的概率更大，

下面對這個例子作一些補充，其中 s s s為隱狀態，意思是我們不能直觀看到， o o o是觀測量，是我們我可以直觀看到的，就像上面的例子一樣，同學的真實性別我們觀測不到，是隱狀態，但是我們可以根據背影看到是否為長發，是否為長發就是觀測量，

下面給出卡爾曼濾波器的兩個式子，
x k = A x k ? 1 + B u k ? 1 + w k ? 1 x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} xk?=Axk?1?+Buk?1?+wk?1?
上式是系統的狀態方程，其中 x k x_{k} xk?是系統在 k k k時刻的系統狀態量， x k : n × 1 x_{k}:n×1 xk?:n×1， x k ? 1 x_{k-1} xk?1?是系統在 k ? 1 k-1 k?1時刻的系統狀態量， x k ? 1 : n × 1 x_{k-1}:n×1 xk?1?:n×1， A A A是系統矩陣， A : n × n A:n×n A:n×n， B B B是輸入矩陣（也叫控制矩陣）， B : n × r B:n×r B:n×r， u k ? 1 u_{k-1} uk?1?是系統在 k ? 1 k-1 k?1時刻的系統輸入， u k ? 1 : r × 1 u_{k-1}:r×1 uk?1?:r×1， w k ? 1 w_{k-1} wk?1?是服從高斯分布的隨機向量，相當于是系統噪聲， w k ? 1 : n × 1 w_{k-1}:n\times 1 wk?1?:n×1， w k ? 1 ～ N p ( 0 , Q ) w_{k-1}\sim N_{p}(0,Q) wk?1?～Np?(0,Q)， Q Q Q是系統噪聲服從的協方差矩陣， Q : n × n Q:n\times n Q:n×n，
y k = H x k + v k y_{k}=Hx_{k}+v_{k} yk?=Hxk?+vk?
上式是系統的觀測方程，其中 x k x_{k} xk?是系統在 k k k時刻的系統狀態量， x k : n × 1 x_{k}:n×1 xk?:n×1， H H H是系統狀態到觀測值（系統中的傳感器資料）的轉換矩陣， H : m × n H:m\times n H:m×n, v k v_{k} vk?是測量噪聲， v k : m × 1 v_{k}:m\times 1 vk?:m×1， v k ～ N p ( 0 , R ) v_{k}\sim N_{p}(0,R) vk?～Np?(0,R)， R R R是測量噪聲服從的協方差矩陣， R : m × m R:m\times m R:m×m， y k y_{k} yk?是系統的觀測值（系統中的傳感器資料），
與上面舉的學校男女生的例子類比，系統的狀態量 x k x_{k} xk?是我們直接觀測不到的，即隱狀態，我們能觀測到的量是傳感器的資料 y k y_{k} yk?，現在我們要推算 k k k時刻系統中的真實狀態值 x k x_{k} xk?，在推測 x k x_{k} xk?的同時，我們已經知道系統過往的測量值 y 1 , y 2 , … , y k y_{1},y_{2},\dots,y_{k} y1?,y2?,…,yk?，系統過往的真實狀態值 x 1 , x 2 , … , x k ? 1 x_{1},x_{2},\dots,x_{k-1} x1?,x2?,…,xk?1?我們是不知道的，即使我們對 x 1 , x 2 , … , x k ? 1 x_{1},x_{2},\dots,x_{k-1} x1?,x2?,…,xk?1?做出了估計，為了更好的展示該程序的邏輯，如下圖所示，

如上圖所示，系統的真實狀態隨著時間逐漸變化，在時間步 k k k對應觀測向量 y k y_{k} yk?（可能系統有多個傳感器，所以每一次可以得到一個觀測向量），重點來了！！！現在，需要要做的是，在已知系統過往的觀測值向量 y 1 , y 2 , … , y k ? 1 y_{1},y_{2},\dots,y_{k-1} y1?,y2?,…,yk?1?和當前觀測值向量 y k y_{k} yk?的前提下，估計當前狀態 x k x_{k} xk?最可能取到的值，其數學運算式為
x ^ k = argmax P ( x k ∣ y 1 , y 2 , … , y k ) \hat{x}_{k}=\text{argmax}P(x_{k}|y_{1},y_{2},\dots,y_{k}) x^k?=argmaxP(xk?∣y1?,y2?,…,yk?)
這里舉個例子，幫助理解上式，如果 P ( x k ∣ y 1 , y 2 , … , y k ) = 1 2 π σ e ? ( x k ? μ ) 2 2 σ 2 P(x_{k}|y_{1},y_{2},\dots,y_{k})=\frac{1}{\sqrt{2\pi}\sigma}e^{\frac{-(x_{k}-\mu)^{2}}{2\sigma^{2}}} P(xk?∣y1?,y2?,…,yk?)=2π ?σ1?e2σ2?(xk??μ)2?，那么讓我們估計一下 x k x_{k} xk?的值，我們肯定將估計值 x ^ k \hat{x}_{k} x^k?取 μ \mu μ，因為在 x k = μ x_{k}=\mu xk?=μ發生的概率最大，其實卡爾曼濾波就做了這件事，計算出 k k k時刻的 P ( x k ∣ y 1 , y 2 , … , y k ) P(x_{k}|y_{1},y_{2},\dots,y_{k}) P(xk?∣y1?,y2?,…,yk?)，這個式子是多元正態分布（為什么呢？下面我就介紹），先說一下，多元正態分布運算式的一般形式，如下所示，
f ( x ) = 1 ( 2 π ) p ∣ Σ ∣ 1 2 e ? 1 2 ( x ? μ ) T Σ ? 1 ( x ? μ ) f(x)=\frac{1}{(\sqrt{2\pi})^{p}{|\Sigma|^{\frac{1}{2}}}}e^{-\frac{1}{2}(x-\mu)^{T}\Sigma^{-1}(x-\mu)} f(x)=(2π ?)p∣Σ∣21?1?e?21?(x?μ)TΣ?1(x?μ)
其中， x x x是隨機向量， x : p × 1 x:p\times 1 x:p×1, Σ \Sigma Σ是隨機向量 x x x的協方差矩陣， Σ : p × p \Sigma :p\times p Σ:p×p， μ \mu μ是隨機向量 x x x的均值， μ : p × 1 \mu:p\times 1 μ:p×1， f ( x ) f(x) f(x)也稱為隨機向量 x x x的概率密度函式，也記作 x ～ N p ( μ , Σ ) x\sim N_{p}(\mu,\Sigma) x～Np?(μ,Σ)，

卡爾曼濾波器的推導

首先，為了便于閱讀，我把系統的狀態方程和觀測方程和系統中狀態轉移圖放在這里，每個量的含義都在上文有解釋，這里不再贅述，
x k = A x k ? 1 + B u k ? 1 + w k ? 1 x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} xk?=Axk?1?+Buk?1?+wk?1?

y k = H x k + v k y_{k}=Hx_{k}+v_{k} yk?=Hxk?+vk?

首先，假設我們這個系統是個電阻電容系統，在時間 t = 1 t=1 t=1時，系統剛上電，此時系統中的觀測值為 y 1 y_{1} y1?，在觀測值 y 1 y_{1} y1?發生的前提下，我們假設
P ( x 1 ∣ y 1 ) ～ N n ( μ ^ 1 , Σ ^ 1 ) P(x_{1}|y_{1})\sim N_{n}(\hat{\mu}_{1},\hat{\Sigma}_{1}) P(x1?∣y1?)～Nn?(μ^?1?,Σ^1?)
事實上，這個是我們必須做出的假設，假設中的引數也容易寫，因為系統剛上電， μ ^ 1 \hat{\mu}_{1} μ^?1?直接取作系統的初始狀態值就行了，因為系統中是存在不確定性（噪聲），如果噪聲很小，我們 Σ ^ 1 \hat{\Sigma}_{1} Σ^1?就可以方差取小一些，反之，取大一些，這個算是，我們使用卡爾曼濾波器的一個超引數，是需要我們提前做出假設的，PS：肯定有人會問，如果我們開始這個假設和系統真實情況 P ( x 1 ∣ y 1 ) P(x_{1}|y_{1}) P(x1?∣y1?)不太符合怎么辦，其實問題不大，后面再迭代程序中，這個初始假設中存在的不合理性，會慢慢榷訓，后面仿真，會對這個初始假設做進一步說明，

然后，在時間 t = 2 t=2 t=2的時候，我們需要計算 P ( x 2 ∣ y 1 , y 2 ) P(x_{2}|y_{1},y_{2}) P(x2?∣y1?,y2?)，

根據數學歸納法，假設 t = k ? 1 t=k-1 t=k?1時， P ( x k ? 1 ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( μ ^ k ? 1 , Σ ^ k ? 1 ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) P(xk?1?∣y1?,y2?,…,yk?1?)～Nn?(μ^?k?1?,Σ^k?1?)，在 t = k t=k t=k時，
P ( x k ∣ y 1 , y 2 , … , y k ) = P ( x k , y k ∣ y 1 , y 2 , … , y k ? 1 ) P ( y k ∣ y 1 , y 2 , … , y k ? 1 ) P(x_{k}|y_{1},y_{2},\dots,y_{k})=\frac{P(x_{k},y_{k}|y_{1},y_{2},\dots,y_{k-1})}{P(y_{k}|y_{1},y_{2},\dots,y_{k-1})} P(xk?∣y1?,y2?,…,yk?)=P(yk?∣y1?,y2?,…,yk?1?)P(xk?,yk?∣y1?,y2?,…,yk?1?)?
因為 P ( x k ? 1 ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( μ ^ k ? 1 , Σ ^ k ? 1 ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) P(xk?1?∣y1?,y2?,…,yk?1?)～Nn?(μ^?k?1?,Σ^k?1?)，且 x k = A x k ? 1 + B u k ? 1 + w k ? 1 x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} xk?=Axk?1?+Buk?1?+wk?1?，則有
P ( x k ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( A μ ^ k ? 1 + B u k ? 1 , A Σ ^ k ? 1 A T + Q ) P(x_{k}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(A\hat{\mu}_{k-1}+Bu_{k-1},A\hat{\Sigma}_{k-1}A^{T}+Q) P(xk?∣y1?,y2?,…,yk?1?)～Nn?(Aμ^?k?1?+Buk?1?,AΣ^k?1?AT+Q)

因為 P ( x k ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( A μ ^ k ? 1 + B u k ? 1 , A Σ ^ k ? 1 A T + Q ) P(x_{k}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(A\hat{\mu}_{k-1}+Bu_{k-1},A\hat{\Sigma}_{k-1}A^{T}+Q) P(xk?∣y1?,y2?,…,yk?1?)～Nn?(Aμ^?k?1?+Buk?1?,AΣ^k?1?AT+Q)，且 y k = H x k + v k y_{k}=Hx_{k}+v_{k} yk?=Hxk?+vk?，則有
P ( y k ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( H ( A μ ^ k ? 1 + B u k ? 1 ) , H ( A Σ ^ k ? 1 A T + Q ) H T + R ) P(y_{k}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(H(A\hat{\mu}_{k-1}+Bu_{k-1}),H(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}+R) P(yk?∣y1?,y2?,…,yk?1?)～Nn?(H(Aμ^?k?1?+Buk?1?),H(AΣ^k?1?AT+Q)HT+R)
因為 P ( x k ∣ y 1 , y 2 , … , y k ? 1 ) P(x_{k}|y_{1},y_{2},\dots,y_{k-1}) P(xk?∣y1?,y2?,…,yk?1?)和 P ( y k ∣ y 1 , y 2 , … , y k ? 1 ) P(y_{k}|y_{1},y_{2},\dots,y_{k-1}) P(yk?∣y1?,y2?,…,yk?1?)的分布知道了，可以得到 P ( x k , y k ∣ y 1 , y 2 , … , y k ? 1 ) P(x_{k},y_{k}|y_{1},y_{2},\dots,y_{k-1}) P(xk?,yk?∣y1?,y2?,…,yk?1?)的分布，該分布仍然是多元正態分布，分布為
P ( x k , y k ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n + p ( [ A μ ^ k ? 1 + B u k ? 1 H ( A u ^ k ? 1 + B u k ? 1 ) ] , [ A Σ ^ k ? 1 A T + Q ( A Σ ^ k ? 1 A T + Q ) H T H ( A Σ ^ k ? 1 A T + Q ) T H ( A Σ ^ k ? 1 A T + Q ) H T + R ] ) P(x_{k},y_{k}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n+p}( \left[ \begin{array}{c} A\hat{\mu}_{k-1}+Bu_{k-1}\\ H(A\hat{u}_{k-1}+Bu_{k-1}) \end{array} \right], \left[ \begin{array}{cc} A\hat{\Sigma}_{k-1}A^{T}+Q & (A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}\\ H(A\hat{\Sigma}_{k-1}A^{T}+Q)^{T} & H(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}+R \end{array} \right]) P(xk?,yk?∣y1?,y2?,…,yk?1?)～Nn+p?([Aμ^?k?1?+Buk?1?H(Au^k?1?+Buk?1?)?],[AΣ^k?1?AT+QH(AΣ^k?1?AT+Q)T?(AΣ^k?1?AT+Q)HTH(AΣ^k?1?AT+Q)HT+R?])

知道上面 P ( x k , y k ∣ y 1 , y 2 , … , y k ? 1 ) P(x_{k},y_{k}|y_{1},y_{2},\dots,y_{k-1}) P(xk?,yk?∣y1?,y2?,…,yk?1?)這個概率分布函式后，如何求 P ( x k ∣ y 1 , y 2 , … , y k ) P(x_{k}|y_{1},y_{2},\dots,y_{k}) P(xk?∣y1?,y2?,…,yk?),下面給定引理：
( u v ) n + m ～ N n + m ( [ μ u μ v ] , [ Σ u Σ u v Σ v u Σ v ] ) \begin{pmatrix} u\\ v\\ \end{pmatrix}_{n+m}\sim N_{n+m}( \left[ \begin{array}{c} \mu_{u}\\ \mu_{v} \end{array} \right], \left[ \begin{array}{cc} \Sigma_{u} & \Sigma_{uv}\\ \Sigma_{vu} & \Sigma_{v} \end{array} \right]) (uv?)n+m?～Nn+m?([μu?μv??],[Σu?Σvu??Σuv?Σv??])
P ( u ∣ v ) ～ N n ( μ u + Σ u v Σ v ? 1 ( v ? μ v ) , Σ u ? Σ u v Σ v ? 1 Σ u v T ) P(u|v)\sim N_{n}(\mu_{u}+\Sigma_{uv}\Sigma^{-1}_{v}(v-\mu_{v}),\Sigma_{u}-\Sigma_{uv}\Sigma^{-1}_{v}\Sigma^{T}_{uv}) P(u∣v)～Nn?(μu?+Σuv?Σv?1?(v?μv?),Σu??Σuv?Σv?1?ΣuvT?)

對于 P ( x k , y k ∣ y 1 , y 2 , … , y k ? 1 ) P(x_{k},y_{k}|y_{1},y_{2},\dots,y_{k-1}) P(xk?,yk?∣y1?,y2?,…,yk?1?)，根據以上引理，可以得到
P ( x k ∣ y 1 , y 2 , … , y k ) ～ ( A μ ^ k ? 1 + B u k ? 1 + ( A Σ ^ k ? 1 A T + Q ) H T [ H ( A Σ ^ k ? 1 A T + Q ) H T + R ] ? 1 [ y k ? H ( A μ ^ k ? 1 + B u k ? 1 ) ] , ( A Σ ^ k ? 1 A T + Q ) ? ( A Σ ^ k ? 1 A T + Q ) H T [ H ( A Σ ^ k ? 1 A T + Q ) H T + R ] ? 1 H ( A Σ ^ k ? 1 A T + Q ) T ) P(x_{k}|y_{1},y_{2},\dots,y_{k})\sim(A\hat{\mu}_{k-1}+Bu_{k-1}+(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}[H(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}+R]^{-1}[y_{k}-H(A\hat{\mu}_{k-1}+Bu_{k-1})],(A\hat{\Sigma}_{k-1}A^{T}+Q)-(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}[H(A\hat{\Sigma}_{k-1}A^{T}+Q)H^{T}+R]^{-1}H(A\hat{\Sigma}_{k-1}A^{T}+Q) ^{T}) P(xk?∣y1?,y2?,…,yk?)～(Aμ^?k?1?+Buk?1?+(AΣ^k?1?AT+Q)HT[H(AΣ^k?1?AT+Q)HT+R]?1[yk??H(Aμ^?k?1?+Buk?1?)],(AΣ^k?1?AT+Q)?(AΣ^k?1?AT+Q)HT[H(AΣ^k?1?AT+Q)HT+R]?1H(AΣ^k?1?AT+Q)T)

上式看起來有點繁瑣，我們令 x ^ k ￣ = A μ ^ k ? 1 + B u k ? 1 \hat{x}_{\overline{k}}=A\hat{\mu}_{k-1}+Bu_{k-1} x^k?=Aμ^?k?1?+Buk?1?， P k ￣ = A Σ ^ k ? 1 A T + Q P_{\overline{k}}=A\hat{\Sigma}_{k-1}A^{T}+Q Pk?=AΣ^k?1?AT+Q， K k = P k ￣ H T [ H P k ￣ H T + R ] ? 1 K_{k}=P_{\overline{k}}H^{T}[HP_{\overline{k}}H^{T}+R]^{-1} Kk?=Pk?HT[HPk?HT+R]?1,則 P ( x k ∣ y 1 , y 2 , … , y k ) P(x_{k}|y_{1},y_{2},\dots,y_{k}) P(xk?∣y1?,y2?,…,yk?)可以被重寫成

P ( x k ∣ y 1 , y 2 , … , y k ) ～ ( x ^ k ￣ + K k ( y k ? H x ^ k ￣ ) , P k ￣ ? K k H P k ￣ ) P(x_{k}|y_{1},y_{2},\dots,y_{k})\sim(\hat{x}_{\overline{k}}+K_{k}(y_{k}-H\hat{x}_{\overline{k}}),P_{\overline{k}}-K_{k}HP_{\overline{k}}) P(xk?∣y1?,y2?,…,yk?)～(x^k?+Kk?(yk??Hx^k?),Pk??Kk?HPk?)

此時，我們知道 P ( x k ∣ y 1 , y 2 , … , y k ) ～ ( x ^ k ￣ + K k ( y k ? H x ^ k ￣ ) , P k ￣ ? K k H P k ￣ ) P(x_{k}|y_{1},y_{2},\dots,y_{k})\sim(\hat{x}_{\overline{k}}+K_{k}(y_{k}-H\hat{x}_{\overline{k}}),P_{\overline{k}}-K_{k}HP_{\overline{k}}) P(xk?∣y1?,y2?,…,yk?)～(x^k?+Kk?(yk??Hx^k?),Pk??Kk?HPk?)，為了形式上和 P ( x k ? 1 ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( μ ^ k ? 1 , Σ ^ k ? 1 ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) P(xk?1?∣y1?,y2?,…,yk?1?)～Nn?(μ^?k?1?,Σ^k?1?)一致，我們記
μ ^ k = x ^ k ￣ + K k ( y k ? H x ^ k ￣ ) \hat{\mu}_{k}=\hat{x}_{\overline{k}}+K_{k}(y_{k}-H\hat{x}_{\overline{k}}) μ^?k?=x^k?+Kk?(yk??Hx^k?)

Σ ^ k = P k ￣ ? K k H P k ￣ = ( I ? K k H ) P k ￣ \hat{\Sigma}_{k}=P_{\overline{k}}-K_{k}HP_{\overline{k}}=(I-K_{k}H)P_{\overline{k}} Σ^k?=Pk??Kk?HPk?=(I?Kk?H)Pk?

那么有 P ( x k ∣ y 1 , y 2 , … , y k ) ～ N n ( μ ^ k , Σ ^ k ) P(x_{k}|y_{1},y_{2},\dots,y_{k})\sim N_{n}(\hat{\mu}_{k},\hat{\Sigma}_{k}) P(xk?∣y1?,y2?,…,yk?)～Nn?(μ^?k?,Σ^k?)，所以在當前時間 t = k t=k t=k時，在發生了測量值 y 1 , y 2 , … , y k y_{1},y_{2},\dots,y_{k} y1?,y2?,…,yk?的情況下，系統真實狀態 x k x_{k} xk?為 μ ^ k \hat{\mu}_{k} μ^?k?的概率最大，我們也說 μ ^ k \hat{\mu}_{k} μ^?k?是系統在 t = k t=k t=k時真實狀態 x k x_{k} xk?的最優估計，

總結一下上面的推導，一共五個式子，首先，我們知道 t = k ? 1 t=k-1 t=k?1時系統的狀態 x k ? 1 x_{k-1} xk?1?符合 P ( x k ? 1 ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( μ ^ k ? 1 , Σ ^ k ? 1 ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) P(xk?1?∣y1?,y2?,…,yk?1?)～Nn?(μ^?k?1?,Σ^k?1?)，即系統在 t = k ? 1 t=k-1 t=k?1時的真實狀態 x k ? 1 x_{k-1} xk?1?的最優估計是 μ ^ k ? 1 \hat{\mu}_{k-1} μ^?k?1?，現在根據 P ( x k ? 1 ∣ y 1 , y 2 , … , y k ? 1 ) ～ N n ( μ ^ k ? 1 , Σ ^ k ? 1 ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k-1})\sim N_{n}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) P(xk?1?∣y1?,y2?,…,yk?1?)～Nn?(μ^?k?1?,Σ^k?1?)可以推算出 t = k t=k t=k時系統的狀態 x k x_{k} xk?符合 P ( x k ? 1 ∣ y 1 , y 2 , … , y k ) ～ N n ( μ ^ k , Σ ^ k ) P(x_{k-1}|y_{1},y_{2},\dots,y_{k})\sim N_{n}(\hat{\mu}_{k},\hat{\Sigma}_{k}) P(xk?1?∣y1?,y2?,…,yk?)～Nn?(μ^?k?,Σ^k?)，
計算公式為
x ^ k ￣ = A μ ^ k ? 1 + B u k ? 1 \hat{x}_{\overline{k}}=A\hat{\mu}_{k-1}+Bu_{k-1} x^k?=Aμ^?k?1?+Buk?1?

P k ￣ = A Σ ^ k ? 1 A T + Q P_{\overline{k}}=A\hat{\Sigma}_{k-1}A^{T}+Q Pk?=AΣ^k?1?AT+Q

K k = P k ￣ H T [ H P k ￣ H T + R ] ? 1 K_{k}=P_{\overline{k}}H^{T}[HP_{\overline{k}}H^{T}+R]^{-1} Kk?=Pk?HT[HPk?HT+R]?1

μ ^ k = x ^ k ￣ + K k ( y k ? H x ^ k ￣ ) \hat{\mu}_{k}=\hat{x}_{\overline{k}}+K_{k}(y_{k}-H\hat{x}_{\overline{k}}) μ^?k?=x^k?+Kk?(yk??Hx^k?)

Σ ^ k = P k ￣ ? K k H P k ￣ = ( I ? K k H ) P k ￣ \hat{\Sigma}_{k}=P_{\overline{k}}-K_{k}HP_{\overline{k}}=(I-K_{k}H)P_{\overline{k}} Σ^k?=Pk??Kk?HPk?=(I?Kk?H)Pk?

談點對卡爾曼濾波器的理解，卡爾曼濾波器的實際做的就是時刻 t = k ? 1 t=k-1 t=k?1，在系統觀測量 y 1 , y 2 , … , y k ? 1 y_{1},y_{2},\dots,y_{k-1} y1?,y2?,…,yk?1?已知的情況下，根據系統狀態 x k ? 1 x_{k-1} xk?1?的條件概率密度函式，來計算在 t = k t=k t=k時，且系統觀測量 y 1 , y 2 , … , y k y_{1},y_{2},\dots,y_{k} y1?,y2?,…,yk?已知的情況下，系統狀態 x k x_{k} xk?的條件概率密度函式，如下圖所示，
圖中的系統狀態為 x x x， x : 2 × 1 x:2\times 1 x:2×1，即系統狀態有兩個分量 x ( 1 ) x^{(1)} x(1)和 x ( 2 ) x^{(2)} x(2)，系統的條件密度函式由服從 N 2 ( μ ^ k ? 1 , Σ ^ k ? 1 ) N_{2}(\hat{\mu}_{k-1},\hat{\Sigma}_{k-1}) N2?(μ^?k?1?,Σ^k?1?)的多元高斯分布變成了另一個服從 N 2 ( μ ^ k , Σ ^ k ) N_{2}(\hat{\mu}_{k},\hat{\Sigma}_{k}) N2?(μ^?k?,Σ^k?)的多元高斯分布，

卡爾曼濾波器的相關基礎知識

筆者是自動化專業的，關于卡爾曼濾波器中的狀態方程和觀測方程，我就沒多說，默認讀者都是會的，如果大家不是很清楚，推薦閱讀劉豹的《現代控制理論》第一章，

關于卡爾曼濾波器最難的也就是概率推算，這一塊基本上是多元正態分布的知識，僅僅學習過概率論的同學應該看著很費解，推薦閱讀高惠璇的《應用多元統計分析》第二章，從中可以系統的了解多元正態分布的定義和相關性質，本文的推導使用的知識都能從中找到對應的推導和證明，

關于和卡爾曼濾波器的相關知識很接近的知識是HMM（Hidden Markov Model）和粒子濾波，推薦看B站的徐亦達老師的卡爾曼濾波器部分，HMM和粒子濾波部分，講的真的很好（建立在讀者有扎實的數學基礎層面上）
B站徐亦達老師卡爾曼濾波器講解視頻

卡爾曼濾波器的仿真

下面放上MATLAB仿真程式，

%%% kalman filter %%%
% the state space function is as follows
% x_{k} = Ax_{k-1}+Bu_{k-1}+ w_{k-1}
% where Cov(ww^{T}) = Q . Matrix size: A: n×n  B: n×p  
% the measurement function is as follows
% z_{k} = Hx_{k} + v_{k}
% where Cov(vv^{T}) = R . Matrix size: H: m×n

% k=1:   P(x_{1}|y_{1})=N(posterior probability{u_{1}},posterior probability{sigma_{1}})
% k=2:   P(x_{2}|y_{1})=N(prior probability{u_{2}},prior probability{sigma_{2}}) 
%        P(x_{2}|y_{1},y_{2})=N(posterior probability{u_{2}},posterior probability{sigma_{2}})
% k=t:   P(x_{k}|y_{1},y_{2},...,y_{k})=N(prior probability{u_{k}},prior probability{sigma_{k}}) 
%        P(x_{k}|y_{1},y_{2},...,y_{k})=N(posterior probability{u_{k}},posterior probability{sigma_{k}})

A = [0, 0.1, 0; 0, 0.2, 0; 0, 0, 0.3];
B = [0.1; 0.1; 0.1];
H = [1, 0, 0;0, 1, 0;0, 0, 1];
Q = [0.01, 0, 0; 0, 0.01, 0; 0, 0, 0.01]; % 注意這里我仿真設定的協方差矩陣中，噪聲的每一個分量都是不相關的，實際中不同噪聲分量可以相關，
R = [0.09, 0, 0;0, 0.09, 0;0, 0, 0.09]; % 注意這里我仿真設定的協方差矩陣中，噪聲的每一個分量都是不相關的，實際中不同噪聲分量可以相關，

posterior_probability_u=[0;0;0]; % 時間步為1時的初始化狀態期望
posterior_probability_sigma=[0,0,0;0,0,0;0,0,0]; % 初始時刻t=1時,初始化狀態協方差矩陣
real_state=zeros(3,1000);
estimate_state=zeros(3,1000);
input_u=ones(1,1000);
measurement_value=zeros(3,1000);
estimate_state(:,1)=posterior_probability_u; 
t=150;
for i = 2:t
    a=normrnd(0,0.1,[3,1]);
    real_state(:,i)=A*real_state(:,i-1)+B*input_u(:,i-1)+a;
    measurement_value(:,i)=H*real_state(:,i)+normrnd(0,0.3,[3,1]);
    prior_probability_u=A*estimate_state(:,i-1)+B*input_u(:,i-1);
    prior_probability_sigma=A*posterior_probability_sigma*A'+Q;
    kalman_gain=prior_probability_sigma*H'/(H*prior_probability_sigma*H'+R);
    estimate_state(:,i)=prior_probability_u+kalman_gain*(measurement_value(:,i)-H*prior_probability_u);
    posterior_probability_sigma=(eye(3,3)-kalman_gain*H)*prior_probability_sigma;
end
figure(1);
plot(real_state(1,1:t),'r');
hold on;
plot(measurement_value(1,1:t),'y');
hold on;
plot(estimate_state(1,1:t),'b');
legend('real state','mesurement value','estimate state');

figure(2);
plot(real_state(2,1:t),'r');
hold on;
plot(measurement_value(2,1:t),'y');
hold on;
plot(estimate_state(2,1:t),'b');
legend('real state','mesurement value','estimate state');

上面程式中的 t = 1 t=1 t=1時， P ( x 1 ∣ y 1 ) ～ N 3 ( μ ^ 1 , Σ ^ 1 ) P(x_{1}|y_{1})\sim N_{3}(\hat{\mu}_{1},\hat{\Sigma}_{1}) P(x1?∣y1?)～N3?(μ^?1?,Σ^1?)中的 μ ^ 1 \hat{\mu}_{1} μ^?1?對應的是posterior_probability_u， Σ ^ 1 \hat{\Sigma}_{1} Σ^1?對應的是posterior_probability_sigma，這是我們事前根據自己對系統的了解設定的，在上述程式中的系統的真實初始值 x 1 = [ 0 ; 0 ; 0 ] x_{1}=[0;0;0] x1?=[0;0;0],當我們把posterior_probability_u初始化為 [ 0 ; 0 ; 0 ] [0;0;0] [0;0;0]時，對于系統狀態的第一個分量 x ( 1 ) x^{(1)} x(1)仿真結果如下圖，

上圖中的紅線是系統的真實狀態值（因為我是在仿真，我計算出來了系統真實值，在實際工程中，我們是不知道系統狀態的真實值的），黃線是系統中的測量值（可以認為是傳感器傳回來的資料），藍線是卡爾曼濾波器估計出來的系統狀態最優值，
這個仿真是我根據實際情況可能出現的情況給出的仿真，在使用陀螺儀加速度計這類傳感器時，傳感器測量噪聲比較大，所以黃線波動幅度很大，紅線按道理波動可以再小一點，這里我把系統狀態方程中的噪聲設定的有點大，在不少的控制系統中，由于反饋的作用，系統中的噪聲會相對小一些，所以從圖中可以看出來，估計出來的狀態最優值波動還是較小的，和系統真實狀態值比較貼近，傳感器的噪聲被很大程度抑制了，如果只使用傳感器的讀數作為系統狀態值，那么可以看到兩者差距還是很大的，
如果傳感器噪聲很小，我們完全可以不用卡爾曼濾波器，因為測量值基本就是系統真實狀態值了，這很容易理解，為了驗證一下，我把程式中的 R R R的改為R = [0.0001, 0, 0;0, 0.0001, 0;0, 0, 0.0001],仿真結果如下，紅線和黃線基本重合，反而卡爾曼濾波器估計出來的最優值不如測量值靠譜，
再回到系統初始設定的問題，在 t = 1 t=1 t=1時，系統初始 P ( x 1 ∣ y 1 ) ～ N 3 ( μ ^ 1 , Σ ^ 1 ) P(x_{1}|y_{1})\sim N_{3}(\hat{\mu}_{1},\hat{\Sigma}_{1}) P(x1?∣y1?)～N3?(μ^?1?,Σ^1?)中的 μ ^ 1 \hat{\mu}_{1} μ^?1?對應的是posterior_probability_u，在上述程式中的系統的真實初始值 x 1 = [ 0 ; 0 ; 0 ] x_{1}=[0;0;0] x1?=[0;0;0],當我們把posterior_probability_u初始化為 [ 1 ; 1 ; 1 ] [1;1;1] [1;1;1]時，這個設定和系統真實情況出入很大，對于系統狀態的第一個分量 x ( 1 ) x^{(1)} x(1)仿真結果如下圖，

可以看出來即使初始假設和系統真實情況出入比較大，關于系統狀態的最優估計還是很快回歸到系統真實狀態值附近，這是因為有測量值幫助修正最優估計，所以允許在初始假設時，出現與系統真實狀態分布的偏差，

卡爾曼濾波器的總結

卡爾曼濾波器利用了兩條線，第一條線是系統的狀態方程，狀態方程保證了系統狀態變數的一個基本物理變換程序，即使有系統噪聲的存在，也可以根據系統的時刻 t = k ? 1 t=k-1 t=k?1的狀態值得到 t = k t=k t=k時，系統狀態值大概值（概率分布），第二條線是我們有系統的傳感器傳回來的測量值（大多時候傳感器的測量噪聲比較大），根據這兩條線，來計算系統最可能的狀態值（即最優估計 μ ^ k \hat{\mu}_{k} μ^?k?），