前言

不知道是不是每個想入局卡爾曼的同學都面臨同樣的問題，敲筆記之前瀏覽了一遍參考的東西，各種角度給出了非常生動的介紹，手里還有一本清華大學張賢達老師《現代信號處理》也從自適應濾波器的角度對卡爾曼濾波器的推導。雖然大模型也能給出示例程序，運行結果也能感受的到算法的魅力，但肉眼凡胎還是要親自學習理解一下，那就學院派先來，筆記一下張老師的推導思路。

從自適應濾波的角度

自適應濾波器發展的歷史中，匹配濾波器和維納濾波器是繞不開的兩座豐碑，匹配濾波器是輸出達到最大的信噪比，一個經典的應用就是通信系統自帶training sequence或pilot信號的解調。wiener濾波器采用最小均方估計誤差的準則，不需要提供先驗的同步信號，應用就更加廣泛了。這個經典的推導再很多年前的webrtc中的噪聲抑制之一：頻域維納濾波 敲過鍵盤。這里有很多關于極點的擴展分析，暫時涉及不到，就不深挖這個坑了。而數字信號處理中，估計誤差$e(n)$定義為期望響應$d(n)$與濾波器輸出$y(n)$之差，即$$y(n)=\sum _{k=0}^{\infty }{w}_k^{?}u(n?k),n=1,2,...$$$$e(n)=d(n)?y(n)$$這種估計誤差在某種統計意義下最小的設計就被稱為最優濾波器，優化準則即令某個代價函數最小化，在ai時代的熵函數盛行之前，最小均方誤差應該是最核心的存在，這就是大名鼎鼎的：MMSE(Minimum Mean Square Error)，簡而言之：設計線性離散時間濾波器的系數$w_k$，使輸出$y(n)$在給定的輸入樣本集合$u(0),u(1),...$的情況下給出期望響應$d(n)$的估計，并使得$e(n)=d(n)?y(n)$的均方誤差$E|e(n){|}^2$為最小。

正交性原理到維納解

從數學上課文還推導了正交性原理，并講述其從數學上作為線性最有濾波理論中令代價函數$J$最小化的充分必要條件之關系。直接寫出定理和引理E{u(n?k)eopt?(n)}=0,k=0,1,2E\{u(n-k)e^*_{opt}(n)\}=0, \ \ \ k=0,1,2E{u(n?k)eopt??(n)}=0,???k=0,1,2E{yopt(n)eopt?(n)}=0E\{y_{opt}(n)e^*_{opt}(n)\}=0E{yopt?(n)eopt??(n)}=0根據前面的推導，可以將定理展開E{u(n?k)[d?(n)?∑i=0∞wopt,ku?(n?i)]}=0,k=0,1,2...E\{u(n-k) [ d^*(n)-\sum_{i=0}^\infty w_{opt,k} u^*(n-i)]\}=0, \ \ \ k=0,1,2...E{u(n?k)[d?(n)?i=0∑∞?wopt,k?u?(n?i)]}=0,???k=0,1,2...∑i=0∞wopt,kE{u(n?k)u?(n?i)}=E{u(n?k)d?(n)},k=0,1,2...\sum_{i=0}^\infty w_{opt,k} E\{u(n-k) u^*(n-i)\}=E\{u(n-k) d^*(n)\}, \ \ \ k=0,1,2...i=0∑∞?wopt,k?E{u(n?k)u?(n?i)}=E{u(n?k)d?(n)},???k=0,1,2...式中，數學期望項E{u(n?k)u?(n?i)}E\{u(n-k) u^*(n-i)\}E{u(n?k)u?(n?i)}代表v濾波器輸入在之后$i?k$的自相關函數$R_{u,u}(i?k)$，即Ru,u(i?k)=E{u(n?k)u?(n?i)}R_{u,u}(i-k)=E\{u(n-k) u^*(n-i)\}Ru,u?(i?k)=E{u(n?k)u?(n?i)}而數學期望項E{u(n?k)d?(n)}E\{u(n-k) d^*(n)\}E{u(n?k)d?(n)}等于v濾波器輸入$u(n?k)$與數學期望$d(n)$在滯后$?k$的互相關函數$R_{u,d}(?k)$ Ru,d(?k)=E{u(n?k)d?(n)}R_{u,d}(-k)=E\{u(n-k) d^*(n)\}Ru,d?(?k)=E{u(n?k)d?(n)}由此得到一個更簡潔的公式：$$\sum _{i=0}^{\infty }{w}_{opt,k}{R}_{u,u}(i?k)\}=R_{u,d}(?k),k=0,1,\mathrm{2...}$$這就是Wiener-Hopf 差分方程，對應M階FIR濾波器，正無窮這個東西就變成了M-1，所有有限長FIR的M個齊次方程為$$\sum _{i=0}^{M?1}{w}_{opt,k}{R}_{u,u}(i?k)\}=R_{u,d}(?k),k=0,1,\mathrm{2...}M?1$$如果定義M個輸入向量$$\mathbf{u}(n)=[u(n),u(n?1),..u(n?M+1){]}^T$$那么自相關矩陣為：R=E{u(n)uH(n)}=[Ru,u(0)Ru,u(1)Ru,u(M?1)Ru,u?(1)Ru,u(0)Ru,u(M?2)?Ru,u?(M?1)Ru,u?(M?2)Ru,u(0)]R=E\{\bold u(n)\bold u^H(n)\} = \begin{bmatrix} R_{u,u}(0) &R_{u,u}(1) & & R_{u,u}(M-1) \\ R_{u,u}^*(1) &R_{u,u}(0)& & R_{u,u}(M-2) \\ & \ddots & \\ R_{u,u}*(M-1)&R_{u,u}^*(M-2)& &R_{u,u}(0) \end{bmatrix} R=E{u(n)uH(n)}=?Ru,u?(0)Ru,u??(1)Ru,u??(M?1)?Ru,u?(1)Ru,u?(0)?Ru,u??(M?2)??Ru,u?(M?1)Ru,u?(M?2)Ru,u?(0)??等式右側則是輸入與期望響應的互相關向量：r=E{u(n)d?(n)}=[Ru,d(0),Ru,d(?1),...,Ru,d(?M+1)]Tr=E\{\bold u(n) d^*(n)\}=[R_{u,d}(0), R_{u,d}(-1),...,R_{u,d}(-M+1)]^Tr=E{u(n)d?(n)}=[Ru,d?(0),Ru,d?(?1),...,Ru,d?(?M+1)]T進一步簡化成矩陣表達：$$R{w}_{opt}=r$$等式兩側左乘以$R^{?1}$，則得到維納濾波器的最有權重解：$$w_{opt}=R^{?1}r$$

kalman濾波的提出

kalman濾波是再wiener理論上發展的，“采用頻域設計法是造成Wiener濾波器設計困難的根本原因。因此人們逐漸轉向尋求在時域內直接設計最優濾波器的方法。Kalman在20世紀60年代初提出了Kalman濾波理論。Kalman濾波理論是一種時域方法。它把狀態空間的概念引入隨機估計理論，把信號過程視為白噪聲作用下一個線性系統的輸出，用狀態方程來描述輸入-輸出關系，在估計過程中利用系統狀態方程、觀測方程和白噪聲激勵，即系統過程噪聲和觀測噪聲，利用它們的統計特性形成濾波算法。由于所用的信息都是時域內的量，所以Kalman濾波不但可以對平穩的一維隨機過程進行估計，也可以對非平穩的、多維隨機過程進行估計。同時Kalman濾波算法是遞推的，便于在計算機上實現實時應用，克服了經典Wiener濾波方法的缺點和局限性”。上面這段摘自《Kalman濾波算法詳解及MATLAB實現》相對于wiener解本身的數學方法是非遞推的（lms/rls等方法是等效實現），卡爾曼方法先天的遞推計算，即自適應濾波器哦。這個推導過程與最小二乘法的自適應框架統一。
考慮一離散時間的動態系統，它由描述狀態的過程方程和描述觀測向量的觀測方程共同表示。
（1）過程方程$$\mathbf{x}(n+1)=\mathbf{F}(n+1,n)\mathbf{x}(n)+{\mathbf{v}}_1(n)$$ $M?1維向量$$x(n)$表示系統再離散時間$n$的狀態向量，它是不可觀測的；$M?M$矩陣$\mathbf{F}(n+1,n)$稱為狀態轉移矩陣，描述動態系統在時間n的狀態到n+1的狀態之間的轉移，它應該是已知的；而$M?1$向量$v_1(n)$的過程噪聲向量，描述狀態轉移中間的加性噪聲或誤差。因為這樣一個過程也可以看作是狀態的變遷，所以此方程也稱為狀態轉移方程。
（2）觀測方程$$y(n)=\mathbf{C}(n)\mathbf{x}(n)+{\mathbf{v}}_2(n)$$ $y(n)$代表動態系統在時間n的$N?1$觀測向量；$N?M$矩陣C(n)稱為觀測矩陣（描述狀態經過其作用，變成可觀測數據），要求是已知的；$v_2$表示觀測噪聲向量，其維度與觀測向量相同。
要簡化計算，以下的假設是必須滴： $v_1(n)$過程噪聲向量和$v_2$觀測噪聲向量都是零均值的白噪聲，兩者也是線性無關的。他們的相關矩陣分別為E{v1(n)v1H(k)}={Q1(n),if?n=kO,if?n≠kE\{v_1(n)v_1^H(k)\}=\begin{cases} Q_1(n), & \text{if }n=k \\ O, & \text{if }n\neq k \end{cases}E{v1?(n)v1H?(k)}={Q1?(n),O,?if?n=kif?n=k?E{v2(n)v2H(k)}={Q2(n),if?n=kO,if?n≠kE\{v_2(n)v_2^H(k)\}=\begin{cases} Q_2(n), & \text{if }n=k \\ O, & \text{if }n\neq k \end{cases}E{v2?(n)v2H?(k)}={Q2?(n),O,?if?n=kif?n=k?E{v1(n)v2H(k)}=O,?n,kE\{v_1(n)v_2^H(k)\}=O, \space \forall n,kE{v1?(n)v2H?(k)}=O,??n,k
利用觀測的向量$y(1),...,y(n)$對$n\ge 1$求狀態向量$\mathbf{x}(i)$各個分量的最小二乘估計，那么具體的根據狀態變量索引$i$和觀測向量索引$n$的關系，kalman問題又細分為三個類型：
（1）濾波（$i=n$)：使用n時刻及以前時刻的測量數據，抽取n時刻的信息；
（2）平滑（$1\le i<n$)：因為n以前某個時刻的信息，而且n時刻以后的測量數據也可用，非因果的效果使得平滑更加精確；
（3）預測（i > n)：使用n時刻及其以前時刻的測量數據，提前抽取n+$\tau ,\tau >0$時刻（未來）的信息，這是一種預測。

innovation process新息過程

看到這個概念，我的第一反映這是什么WTF？為什么叫“新息”？在卡爾曼濾波中，新息過程就是這個預測值和實際觀測值之間的差異。用數學符號表示就是：
$$新息=實際觀測值?預測的觀測值$$結合上面的符號，給定觀測值$y(1),...,y(n?1)$，求觀測向量$y(n)$的最小二乘估計，記作$${\hat{y}}_1(n)\stackrel{\text{def}}{=}\hat{y}(n|y(1),...,y(n?1))$$$y(n)$的新息過程定義為$$\alpha (n)=y(n)?{\hat{y}}_1(n)$$$N?1$向量$\alpha (n)$表示觀測數據$y(n)$的新的信息，簡稱新息。新息的性質：
（1）n時刻的新息$\alpha$與過去所有的觀測數據$y(1),...,y(n?1)$正交，即E{α(n)yH(k)}=O,1≤k<nE\{\alpha(n)y^H(k)\}=O,\space 1\leq k <nE{α(n)yH(k)}=O,?1≤k<n
（2）新息過程由彼此正交的隨機向量序列{α(n)}\{\alpha(n)\}{α(n)}組成，E{α(n)αH(k)}=O,1≤k<nE\{\alpha(n)\alpha^H(k)\}=O,\space 1\leq k <nE{α(n)αH(k)}=O,?1≤k<n
（3）表示觀測數據的隨機向量序列{y(1),...,y(n)}\{y(1),...,y(n)\}{y(1),...,y(n)}與表示新息過程的隨機向量序列{α(1),...,α(n)}\{\alpha(1),...,\alpha(n)\}{α(1),...,α(n)}一一對應，即{y(1),...,y(n)}={α(1),...,α(n)}\{y(1),...,y(n)\}=\{\alpha(1),...,\alpha(n)\}{y(1),...,y(n)}={α(1),...,α(n)}以上性質表明：n時刻的新息$\alpha (n)$是一個與n時刻之前的觀測數據{y(1),...,y(n?1)}\{y(1),...,y(n-1)\}{y(1),...,y(n?1)}不相關、具有白噪聲性質的隨機過程，但它卻能夠提供有關$y(n)$的新信息。新息的的相關矩陣R(n)=E{α(n)αH(k)}R(n)=E\{\alpha(n)\alpha^H(k)\}R(n)=E{α(n)αH(k)}在卡爾曼濾波中，并不直接估計觀測數據向量的一步預測${\hat{y}}_1(n)$，而是先計算狀態向量的一步預測：$${\hat{x}}_1(n)\stackrel{\text{def}}{=}x(n|y(1),...,y(n?1))$$然后利用觀測方程$${\hat{y}}_1(n)=C(n){\hat{x}}_1(n)$$帶入新息計算的公式$$\alpha (n)=y(n)?C(n){\hat{x}}_1(n)=C(n)[x(n)?{\hat{x}}_1(n)]+v_2(n)$$此即新息過程的實際計算公式，但首先一步預測的狀態向量估計${\hat{x}}_1(n)$要先求出。定義狀態向量的一步預測誤差$$?(n,n?1)\stackrel{\text{def}}{=}x(n)?{\hat{x}}_1(n)$$$$\alpha (n)=C(n)?(n,n?1)+v_2(n)$$然后將這個式子帶入相關矩陣R(n)=C(n)E{?(n,n?1)?H(n,n?1)}CH(n)+E{v2(n)v2H(n)}=C(n)K(n,n?1)CH(n)+Q2(n)R(n)=C(n)E\{\epsilon(n,n-1)\epsilon^H(n,n-1)\}C^H(n)+E\{v_2(n)v_2^H(n)\}\\ =C(n)K(n,n-1)C^H(n)+Q_2(n)R(n)=C(n)E{?(n,n?1)?H(n,n?1)}CH(n)+E{v2?(n)v2H?(n)}=C(n)K(n,n?1)CH(n)+Q2?(n)K(n,n?1)=E{?(n,n?1)?H(n,n?1)}K(n,n-1)=E\{\epsilon(n,n-1)\epsilon^H(n,n-1)\}K(n,n?1)=E{?(n,n?1)?H(n,n?1)}定義為一步預測狀態誤差的相關矩陣，$Q_2(n)$是觀測噪聲$v_2(n)$的相關矩陣。

kalman濾波算法

新息過程是為了轉入kalman濾波算法的核心問題：如何利用新息過程估計狀態向量的預測？答案是用新息過程序列$\alpha (1),...,\alpha (n)$的線性組合直接構造狀態向量的一步預測：$${\hat{x}}_1(n+1)\stackrel{\text{def}}{=}{\hat{x}}_1(n+1|y(1),...,y(n))=\sum _{k=1}^n{W}_1(k)\alpha (k)$$式子中$W_1(k)$表示與一步預測相對應的權矩陣，且k為離散時間，如何求解這個權矩陣？那就得利用正交性原理了，即最有估計的誤差與已知值正交，這里誤差公式是下一拍的，$$?(n+1,n)=x(n+1)?{\hat{x}}_1(n+1)$$，已知值其實就是新息$\alpha (n)$，由此設計公式E{?(n+1,n)αH(k)}=E{[x(n+1)?x^1(n+1)]αH(k)}=O,k=1,...,.nE\{\epsilon(n+1,n)\alpha^H(k)\}=E\{[x(n+1)-\hat x_1(n+1)]\alpha^H(k)\}=O, \space k=1,...,.nE{?(n+1,n)αH(k)}=E{[x(n+1)?x^1?(n+1)]αH(k)}=O,?k=1,...,.n代入權重表達式,并利用新息的正交性可得E{x(n+1)αH(k)}=W1(k)E{α(k)αH(k)}=W1(k)R(k)E\{x(n+1)\alpha^H(k)\}=W_1(k)E\{\alpha(k)\alpha^H(k)\}=W_1(k)R(k)E{x(n+1)αH(k)}=W1?(k)E{α(k)αH(k)}=W1?(k)R(k)右×一下$R^{?1}(k)$得到了權重的自洽表達W1(k)=E{x(n+1)αH(k)}R?1(k)W_1(k)=E\{x(n+1)\alpha^H(k)\}R^{-1}(k)W1?(k)=E{x(n+1)αH(k)}R?1(k)由此狀態向量的一步預測的最小均方估計表示為x^1(n+1)=∑k=1n?1E{x(n+1)αH(k)}R?1(k)α(k)+E{x(n+1)αH(n)}R?1(n)α(n)\hat x_1(n+1)=\sum_{k=1}^{n-1}E\{x(n+1)\alpha^H(k)\}R^{-1}(k)\alpha(k)+E\{x(n+1)\alpha^H(n)\}R^{-1}(n)\alpha(n)x^1?(n+1)=k=1∑n?1?E{x(n+1)αH(k)}R?1(k)α(k)+E{x(n+1)αH(n)}R?1(n)α(n)利用之前的假設E{v1(n)α(k)}=O,k=0,1,...,nE\{v_1(n)\alpha(k)\}=O, k=0,1,...,nE{v1?(n)α(k)}=O,k=0,1,...,n，結合狀態方程$\mathbf{x}(n+1)=\mathbf{F}(n+1,n)\mathbf{x}(n)+{\mathbf{v}}_1(n)$E{x(n+1)αH(k)}=E{[F(n+1,n)x(n)+v1(n)]αH(k)}=F(n+1,n)E{x(n)αH(k)},k=0,1,...,nE\{x(n+1)\alpha^H(k)\}=E\{[\bold F(n+1,n)\bold x(n)+\bold v_1(n)]\alpha^H(k)\}=\bold F(n+1,n)E\{\bold x(n)\alpha^H(k)\}, \space k=0,1,...,nE{x(n+1)αH(k)}=E{[F(n+1,n)x(n)+v1?(n)]αH(k)}=F(n+1,n)E{x(n)αH(k)},?k=0,1,...,n這樣先化簡最小均方估計的第一項∑k=1n?1E{x(n+1)αH(k)}R?1(k)α(k)=F(n+1,n)∑k=1n?1E{x(n)αH(k)}R?1(k)α(k)=F(n+1,n)x^1(n)\sum_{k=1}^{n-1}E\{x(n+1)\alpha^H(k)\}R^{-1}(k)\alpha(k)=F(n+1,n)\sum_{k=1}^{n-1}E\{x(n)\alpha^H(k)\}R^{-1}(k)\alpha(k)=F(n+1,n)\hat x_1(n)k=1∑n?1?E{x(n+1)αH(k)}R?1(k)α(k)=F(n+1,n)k=1∑n?1?E{x(n)αH(k)}R?1(k)α(k)=F(n+1,n)x^1?(n)最小均方估計的第二項，若定義G(n)=defE{x(n+1)αH(n)}R?1(n)G(n)\stackrel{\text{def}}{=}E\{x(n+1)\alpha^H(n)\}R^{-1}(n)G(n)=defE{x(n+1)αH(n)}R?1(n)那么最終得到的預測公式$$x_1(n+1)==F(n+1,n){\hat{x}}_1(n)+G(n)\alpha (n)$$上面的公式表明n+1時刻的狀態向量的一步預測分為非自適應的部分$F(n+1,n){\hat{x}}_1(n)$和自適應的部分$G(n)\alpha (n)$這里$G(n)$稱為Kalman增益（矩陣）。初學者到這里肯定微醺了，那不妨先記住流程吧

Kalman 自適應濾波器算法

初始條件

x^1(1)=E{x(1)}\hat x_1(1)=E\{x(1)\}x^1?(1)=E{x(1)}
K(1,0)=E{[x(1)?xˉ(1)][x(1)?xˉ(1)]H},xˉ(1)=E{x(1)}K(1,0)=E\{[x(1)-\bar x(1)][x(1)-\bar x(1)]^H\},\space\space\bar x(1)=E\{x(1)\}K(1,0)=E{[x(1)?xˉ(1)][x(1)?xˉ(1)]H},??xˉ(1)=E{x(1)}

輸入觀測向量過程

觀測向量序列={y(1),...,y(n)}觀測向量序列=\{y(1),...,y(n)\}觀測向量序列={y(1),...,y(n)}

已知參數

$$\begin{gathered} 狀態轉移矩陣F(n+1,n) \\ 觀測矩陣C(n) \\ 過程噪聲向量的相關矩陣Q_1(n) \\ 觀測噪聲向量的相關矩陣Q_2(n) \end{gathered}$$

計算：n=1，2，3，…

$$\begin{array}{rl}G(n)& =F(n+1,n)K(n,n?1)C^H(n)[C(n)K(n,n?1)C^H(n)+Q_2(n){]}^{?1}\\ \alpha (n)& =y(n)?C(n){\hat{x}}_1(n)\\ {\hat{x}}_1(n+1)& =F(n+1,n){\hat{x}}_1(n)+G(n)\alpha (n)\\ P(n)& =K(n,n?1)?F^{?1}(n+1,n)G(n)C(n)K(n,n?1)\\ K(n+1,n)& =F(n+1,n)P(n)F^H(n+1,n)+Q_1(n)\end{array}$$
上面就是清華專著的推導，Kalman濾波器的估計性能，使濾波后的狀態估計誤差的相關矩陣P(n)的跡最小化，即Kalman濾波器是狀態向量x(n)的線性最小方差估計。

參考

卡爾曼濾波個人學習筆記 (一)
卡爾曼濾波(一)：初始篇
卡爾曼濾波算法原理講解(搬運的文章，備份留作學習用)
卡爾曼濾波：從入門到精通
圖解卡爾曼濾波(Kalman Filter)
The Kalman Filter
Kalman Filter 卡爾曼濾波
A geometric interpretation of the covariance matrix
How a Kalman filter works, in pictures
卡爾曼濾波原理及應用——MATLAB仿真
Kalman濾波算法詳解及MATLAB實現