卡爾曼最優預測以及最優濾波方程的相關推導--最優預測篇

        最近期末考試完了,終於有時間靜下心來好好推一下卡爾曼最優預測跟濾波的相關方程了,雖然之前也用過也知道怎麼用,但是也僅僅停留在知道怎麼用能用來幹什麼的境界。老早就想好好理一理卡爾曼的幾個基本方程,也算是來填個坑。

        最早接觸卡爾曼濾波還是在三年前調試慣性MEMS的時候,就那麼十幾幾十行代碼甚至神奇,當時理解起來也只能是人云亦云,什麼陀螺儀是短時測量精準但是存在積分誤差,加速度計短時測量沒有陀螺儀準但是不存在積分誤差,可以用加速度計來修正陀螺儀的積分誤差等等等等......反正那會兒也沒去深究,能用就行了,但是心裏總感覺有一個梗......

        現在有時間也有相關的知識儲備來好好探究一下那幾個方程了,打算用兩篇文章來詳細地推導一下卡爾曼最優預測方程跟卡爾曼最優濾波方程,如果過年回老家有時間精力的話我會寫一個demo來實際評估一下卡爾曼的效果。前排提醒,這兩篇文章不是卡爾曼最優預測跟最優濾波的科普性文章,不會舉什麼養豬、開車的例子,因此想看通俗解釋的請提前繞道,個人儘量做到嚴謹的推導。若發現錯誤,望讀者不吝指正。

一、基礎知識複習

        在正式開篇之前,希望讀者能有最基本的概率論與數理統計、一點點的線性代數、一點點的微積分、億點點的隨機過程以及一些線性系統的狀態空間描述的知識儲備。

        先給出幾個簡單的定義跟定理:

        1.正定對稱矩陣的相關性質:

        (1)若A\in \mathbb{R}^{n \times n}}爲正定對稱矩陣,有A^T=A,且對\forall x\in \mathbb{R}^n,有x^TAx\geqslant 0當且僅當x=0時爲0

        (2)若A\in \mathbb{R}^{n \times n}}爲正定對稱矩陣,則A的各特徵值均大於0

        2.接下來幫大家復(yu)習概率論裏面的相關知識:

        (1)隨機變量X(全稱應寫爲\left \{X(\omega),\omega \in \Omega \right \},\Omega爲樣本空間,一般直接簡寫爲X)的分佈函數爲:

                                                            F_X(x)=P\left \{ X\leqslant x,x\in \mathbb{R} \right \}

X爲連續型隨機變量,則有:

                                                            F_X(x)=\int_{-\infty}^{x}f_X(t)dt

其中f_X(x)爲隨機變量X的概率密度,且有f_X(x)=F_X'{(x)} 

        (2)隨機變量X的數學期望(也稱均值)爲:

                                                           E[X]=m_X=\int_{-\infty }^{+\infty }xdF(x)

X爲離散型隨機變量,則有:

                                                           E[X]=m_X=\sum_{i=0}^{+\infty }x_iP(x_i)

X爲連續型隨機變量,則有:

                                                            E[X]=m_X=\int_{-\infty}^{+\infty}xf(x)dx,數學期望的本質爲一階矩

        (3)隨機變量X的方差定義爲:

                                                            D[X]=E[(X-m_X)^2]=E[X^2]-m_X^2

X爲離散型隨機變量,則:

                                                            D[X]=\sum_{i=0}^{+\infty}(x_i-m_X)^2P(x_i)

X爲連續型隨機變量,則:

                                                             D[X]=\int_{-\infty}^{+\infty}(x-m_X)^2f(x)dx

\sigma _X=\sqrt {D[X]}爲標準差,方差其實是二階中心矩

        (4)對於n維連續型隨機向量\boldsymbol {X}=[X_1,X_2...X_n]^T,其聯合分佈函數爲:

     F_{X_1,X_2,...,X_n}(x_1,x_2,...,x_n)=P\left \{ X_1\leqslant x_1,X_2\leqslant x_2, ...Xn\leqslant x_n, \right \}=\int_{-\infty}^{x_1}\int_{-\infty}^{x_2}...\int_{-\infty}^{x_n}f(x_1,x_2,...x_n)dx_1dx_2...dx_n

其中f(x_1,x_2,...x_n)n維聯合概率密度。n維隨機向量的期望向量(也稱均值向量)爲:

                               E[\boldsymbol {X}]=m_X=E[X_1,X_2,...,X_n]^T=[E[X_1],E[X_2],...,E[X_n]]^T=[m_{X_1},m_{X_2},...,m_{X_n}]^T

方差陣(有的地方也叫協方差矩陣,本質上一樣,但是這裏爲了與下面的不同隨機向量之間做區分記爲方差陣)爲:D[\textbf{X}]=E[(X-m_X)(X-m_X)^T]=E\begin{bmatrix} (X_1-m_{X_1})^2 & (X_1-m_{X_1})(X_2-m_{X_2}) & ... & (X_1-m_{X_1})(X_n-m_{X_n})\\ (X_2-m_{X_2})(X_1-m_{X_1}) & (X_2-m_{X_2})^2 & ... & (X_2-m_{X_2})(X_n-m_{X_n})\\ ... & ... & ... & ... \\(X_n-m_{X_n})(X_1-m_{X_1}) & (X_n-m_X_n})(X_2-m_{X_2}) & ... & (X_n-m_{X_n})^2\end{bmatrix}

方差陣主對角線爲隨機向量各分量的方差,其餘元素爲分量之間的相關矩。特別的,對於兩個隨機變量X,Y,若有E[XY]=0,則稱X,Y正交,後面我們會證明的正交定理就是基於此

        (5)對於二維隨機向量(X,Y),其協方差爲:

                                               COV(X,Y)=E[(X-m_X)(Y-m_Y)]=E[XY]-m_Xm_Y,當\boldsymbol {X},\boldsymbol {Y}均爲n維隨機向量時,稱COV[\boldsymbol {X},\boldsymbol {Y}]\boldsymbol {X},\boldsymbol {Y}的協方差矩陣,且:

COV[\boldsymbol {X},\boldsymbol {Y}]=E[(\boldsymbol {X}-m_X)(\boldsymbol {Y}-m_Y)^T]=E\begin{bmatrix} (X_1-m_{X_1})(Y_1-m_{Y_1})) & (X_1-m_{X_1})(Y_2-m_{Y_2}) & ... & (X_1-m_{X_1})(Y_n-m_{Y_n}) \\ (X_2-m_{X_2})(Y_1-m_{Y_1}) & (X_2-m_{X_2})(Y_2-m_{Y_2}) & ... & (X_2-m_{X_2})(Y_n-m_{Y_n}) \\ ... & ... & ... & ...\\ (X_n-m_{X_n})(Y_1-m_{Y_1}) & (X_n-m_{X_n})(Y_2-m_{Y_2}) & ... & (X_n-m_{X_n})(Y_n-m_{Y_n})\end{bmatrix}

        (6)正態隨機變量在實際工程中廣泛存在,我們所熟知的高斯白噪聲就是一類幅值服從正態分佈,功率譜密度分佈服從均勻分佈的一類噪聲。正態分佈的隨機變量在自然界中也是廣泛存在的,中心極限定理告訴我們,無窮多個獨立同分布甚至不同分佈的隨機變量的疊加的結果近似服從正態分佈,因此單個因素可能影響很小,但是無窮多因素疊加會影響整體分佈。

        正態分佈又稱高斯分佈,其概率密度爲:

                                                                            f(x)=\frac{1}{\sqrt {2 \pi}\sigma}exp\left \{ -\frac{(x-m_X)^2}{2\sigma^2} \right \}

記爲X\sim N(m_X,\sigma ^2),其中m_X=E[X]X的均值(數學期望),\sigma ^2X的方差。

        正態分佈有一個非常重要的性質,也就是正態分佈的線性函數也服從正態分佈。即若Y=aX+b,則Y \sim N(am_X+b,(a\sigma )^2)。這一點非常重要,在實際採樣過程中,由於採樣信號中混雜着零均值的高斯白噪聲,那麼對採樣信號疊加的時候實際有用信號幅值會疊加,但是零均值的高斯白噪聲疊加均值仍然爲零,相當於變相地提高了信噪比。

        (8)對於n維正態隨機向量\boldsymbol {X}=[X_1,X_2,...X_n]^T,其均值向量爲\boldsymbol {\mu}=[m_{X_1},m_{X_2},...m_{X_n}]^T,協方差矩陣爲

                                                C=(c_{ij}), \ c_{ij}= E[(X_i-m_{X_i})(X_j-m_{X_j})]\ ,\ (i,j= 1,2,...,n)

若協方差矩陣非奇異即det(C)\neq 0,則其n維聯合概率密度爲:

                                    f(x_1,x_2,...x_n)=\frac{1}{(2\pi)^{\frac{n}{2}}(det(C))^{\frac{1}{2}}}exp\left \{ -\frac{1}{2}(\boldsymbol {X}-\boldsymbol {\mu})^TC^{-1}(\boldsymbol {X}-\boldsymbol {\mu}) \right \},記爲\boldsymbol {X}\sim N(\boldsymbol {\mu},C)

此時協方差矩陣C爲一個正定對稱矩陣。若協方差矩陣C爲奇異矩陣,C^{-1}不存在,則上面的聯合概率密度公式無意義,此時稱該n維隨機向量服從退化正態分佈(也稱奇異正態分佈)。上面提到正態分佈的線性函數也服從正態分佈,對於多維正態隨機向量,有同樣的性質。即對於n維正態隨機向量\boldsymbol {X}=[X_1,X_2,...X_n]^T服從N(\boldsymbol {\mu},C)的正態分佈,且K\in \mathbb{R}^{m*n}爲任意矩陣,則線性變換\boldsymbol {Y}=K\boldsymbol {X}服從m維正態分佈N(K\boldsymbol {\mu},KCK^T),此性質稱爲正態隨機變量的線性不變性。

        3.坐穩了,下面開始復(yu)習隨機過程的相關知識

        (1)隨機過程的嚴格定義是:對於給定概率空間(\Omega,\mathcal F,P)和指標集T,若對於每一個t\in T,有定義在(\Omega,F,P)上的隨機變量X_t(\omega),\omega \in \Omega與之對應,稱依賴於t的隨機變量族X_t爲隨機過程,記爲\left\{X_t(\omega),t \in T \right \},或記爲\left\{X_t,t \in T \right \}以及\left\{X(t),t \in T \right \},定義比較抽象,這裏舉個簡單的例子,拿我們熟知的電阻的熱噪聲來講,指標集T就是時間,對於\forall t \in T時刻我們對熱噪聲採樣,則實際採樣得到的數據其實是一個隨機變量,其服從正態分佈,因此電阻熱噪聲就是個正態過程,當然指標集T可以是連續的也可以是離散的,可以是時間也可以是其他。常見的隨機過程分類就是正態過程、更新計數過程、平穩過程、馬爾可夫過程等。隨機過程X(t)可以看成一個無窮維的隨機向量,我們可以用有限維分佈函數族來近似的刻畫隨機過程,但是較爲複雜,實際工程不可能採用此方法來分析隨機過程,因此我們經常研究隨機過程的一階矩、二階矩統計特性。

        一階矩其實就是我們在概率論裏所熟知的均值,但是這裏是t的一個函數,即:

                                                                      m_X(t)=E[X(t)]=\int_{-\infty}^{+\infty}xdF_t(x),t \in T

稱爲隨機過程X(t)的均值函數。有了均值函數,我們可以定義隨機過程的方差函數D_X(t)=E[(X-m_X(t))^2],t \in T

        下面給出隨機過程的一些重要的數字特徵:

對於隨機過程\left \{ X(t),t \in T \right \},稱

                                                                         R(s,t)=E[X_sX_t],\ \forall s,t \in T

爲隨機過程的自相關函數,自相關函數是隨機過程中最常用的數字特徵。稱

                                                   C(s,t)=COV(X_s,X_t)=E[(X_s-m_X(s))(X_t-m_X(t))],\ \forall s,t \in T

爲隨機過程的協方差函數,不難得出C(s,t)=R(s,t)-m_X(s)m_X(t)以及D_X(t)=C(t,t)

        (2)就像多維隨機變量一樣,也存在多維隨機過程,給定兩個隨機過程X(t),Y(t),則稱

                                                           C_{XY}(s,t)=COV(X_s,Y_t)=E[(X_s-m_X(s))(Y_t-m_Y(t))]

爲兩隨機過程的互協方差函數,其中m_X(s)=E[X(s)],m_Y(t)=E[Y(t)]。稱

                                                                                      R_{XY}(s,t)=E[X_sY_t]

爲兩個隨機過程的互相關函數,同樣,若對\forall s,t \in TR_{XY}(s,t)=0,則稱隨機過程X(t),Y(t)相互正交

        (3)二階矩過程:對於隨機過程X(t),若對\forall t \in TE[|X_t|^2] < +\infty,則稱該隨機過程爲二階矩過程

        (4)平穩過程:若隨機過程X(t)的任意n維聯合分佈函數不隨時間推移而改變,則稱此隨機過程爲嚴平穩過程,但是由於嚴平穩過程要求過於嚴格,且證明較困難,因而在實際工程技術中應用更多的一類是寬平穩過程。對於二階矩過程X(t),若滿足

         (a)m_X(t)=m_X爲一個常數

         (b)對\forall s,t \in T, R_X(s,t)=R_X(t-s)=R_X(\tau),\tau=t-s只與時間區間長度有關,與起始時間無關

則稱此二階矩過程爲寬平穩過程,一般來講嚴平穩過程不一定是寬平穩過程,寬平穩過程也不一定是嚴平穩過程,但是對於正態過程,嚴平穩與寬平穩等價。此外,正態隨機過程還有一個非常重要的性質就是正態隨機過程經過線性系統之後還是正態隨機過程。本文所講的平穩過程均指寬平穩過程

        *(5)關於平穩過程的均方遍歷性(也稱爲各態歷經性),通俗的來講就是可以僅利用一條樣本軌跡來估計整體的統計特性,雖然實際工程中許多隨機過程都滿足均方遍歷性,但是其定義涉及到均方微積分的相關概念,數學驗證也往往很困難,不過很多時候可以根據實際工程背景來確定。在此就不展開敘述,讀者若有興趣可以翻閱隨機過程相關書籍的相關章節

        *(6)傅里葉變換是一種有效的信號分析方法,同樣我們可以用傅里葉變換來分析平穩過程獲得平穩過程的功率譜。對於平穩過程\left \{ X(t),t \in \mathbb{R} \right \}是均方連續的,若其自相關函數R(\tau)滿足

                                                                                     \int_{-\infty}^{+\infty}|R(\tau)|d\tau < +\infty

則此平穩過程功率譜密度S(\omega)爲:

                                                                                  S(\omega)=\int_{-\infty}^{+\infty}R(\tau)e^{-j\omega \tau}d\tau

同時

                                                                                   R(\tau)=\frac{1}{2\pi}\int_{-\infty}^{+\infty}S(\omega)e^{j\omega \tau}d\omega

即平穩過程\left \{ X(t),t \in \mathbb{R} \right \}的相關函數與功率譜密度構成一對傅里葉變換。稱

                                                                F_X(\omega)=\int_{-\infty}^{+\infty}S_X(u)du,\omega \in \mathbb{R}爲平穩過程\left \{ X(t),t \in \mathbb{R} \right \}的譜函數

        4.線性系統的狀態空間描述

        (1)線性系統的定義在這裏就不給出了,線性系統最重要的性質是滿足齊次跟疊加原理。線性系統又分爲線性時變系統跟線性定常系統。線性時變系統是指系統各結構參數會隨時間變化的線性系統,而線性定常系統則不會隨時間變化,拿我們人體爲例,我們身體各器官各功能從長遠看一定在變化,比如我們的聽力、視力等一定會衰減,這時就是一個時變系統,但是在很短的時間內我們的各項器官功能可以看作不變的,這時可以看成是一個定常系統。

        描述一個動態系統,最常用的兩種方式是傳遞函數描述外部的輸入輸出關係以及狀態空間描述內部各狀態變化規律。傳遞函數的相關介紹在此就不展開,可以參考信號與系統相關章節,本文的重點是系統的狀態空間描述。卡爾曼等人在20世紀60年代將狀態以及狀態空間的概念引入到系統控制理論中,極大地推動了系統的時間域理論的發展,狀態空間描述不僅可以用於分析單輸入單輸出的線性定常、時變系統、還可以分析多輸入多輸出的線性系統、非線性系統,本文所講的卡爾曼濾波就是基於線性系統的狀態空間描述。

        (2)系統的狀態是指一組能完整描述系統的時間域行爲或運動過程的線性無關(數目最少)的變量,當狀態表示爲以各狀態變量爲分量的向量時,稱爲狀態向量,記爲:

                                                                  X(t)=[x_1(t),x_2(t),...,x_n(t)]^T

狀態空間定義爲狀態向量的集合,是以狀態向量的n個線性無關的分量爲基底生成的n維線性空間。這裏可以舉個簡單的例子,還是拿我們人體來說,我們人體本身就是一個複雜的系統,描述我們人體的時候我們經常選取的一些變量可以是身高、體重、性別、年齡....這些變量就可以稱爲我們人體這個系統的狀態。系統的狀態空間描述通常需要兩個過程,即系統的輸入引起系統內部狀態變化的過程以及狀態與輸入導致的輸出變化的過程。輸入引起狀態變化是一個動態過程,通常用微分方程來描述,稱其爲狀態方程。而狀態與輸入導致輸出的變化是一個轉化的過程,通常用代數方程來描述,稱其爲輸出方程或者量測方程。記系統的p個輸入爲:

                                                                  U(t)=[u_1(t),u_2(t),...u_p(t)]^T

系統的q個輸出爲:

                                                                  Y(t)=[y_1(t),y_2(t),...y_q(t)]^T

則對於連續時間線性時變系統可以描述爲:

                                                                    \left\{ \begin{array}{ll} \dot{X(t)} = A(t)X(t)+B(t)U(t) \\ Y(t)=C(t)X(t)+D(t)U(t) \end{array} \right.

其中A(t)爲與時間相關的n\times n維矩陣,稱爲系統矩陣;B(t)n \times p維輸入矩陣;C(t)q \times n維輸出矩陣;D(t)q \times p維前饋矩陣。它們均由系統的參數或結構決定。特別的,當系統的各項參數結構不隨時間變化時,則A(t),B(t),C(t),D(t)退化爲與時間無關的常數矩陣,此時系統變爲線性定常系統,其狀態空間描述可寫爲:

                                                                    \left\{ \begin{array}{ll} \dot{X(t)} = AX(t)+BU(t) \\ Y(t)=CX(t)+DU(t) \end{array} \right.

        (3)我們實際經常使用的數字系統本質上是離散的線性系統,此時可以用差分方程的形式給出離散時間線性時變系統的狀態空間描述,即:

                                                                    \left\{ \begin{array}{ll} X(k+1) = G(k)X(k)+H(k)U(k) \\ Y(k)=C(k)X(k)+D(k)U(k) \end{array} \right.

同樣,對於離散時間線性定常系統的狀態空間描述爲:

                                                                    \left\{ \begin{array}{ll} X(k+1) = GX(k)+HU(k) \\ Y(k)=CX(k)+DU(k) \end{array} \right.

        5.相關定理的證明:

        在推導卡爾曼最優預測以及最優濾波方程時,經常用到正交定理,下面我們來證明一下正交定理。

        在討論線性最小方差估計的時候,通常使用觀測值Z的線性函數來表示X的估計值,即:

                                                                     \hat{X}=aZ+b,式中a,b爲待定的常數,

則估計誤差方差要最小,即:

                                                               J=E[(X-\hat{X})^2]=E[(X-(aZ+b))^2]=min

J達到最小時,應滿足:

                                                                 \frac{\partial J}{\partial a}=0,即\frac{\partial J}{\partial a}=-2E\left \{ [X-(aZ+b)]Z \right \}=0

式中

                                                                                  X-(aZ+b)=\overline X

爲估計誤差,立即可得到:

                                                                                            E[\overline XZ]=0

即估計誤差與觀測值的乘積的期望爲0,也就是估計誤差與觀測值正交。

        充分性的證明在此略去,僅需證明在估計誤差與觀測值正交的條件下E[(X-(aZ+b))^2]確實是方差最小的。

        同樣我們可以證明估計誤差\overline X\widehat{X}也是正交的。由於線性最小方差估計是無偏估計,即有:

                                                                                        E[\overline X]=0

則我們有:

                                                      E[\overline X \widehat X]=E[\overline X(aZ+b)]=aE[\overline XZ]+bE[\overline X]=0,即\overline X\hat{X}正交。

        以上對所需的數學基礎知識以及線性系統的相關知識做了一些複習,下一節我們將正式進入線性系統的卡爾曼濾波部分的學習。

二、問題的提法

        在實際系統中,信號總是受到各種隨機噪聲的干擾,因此引起我們系統各狀態變化的因素裏面還應加上一項干擾項,此時系統的狀態方程應改寫爲:\dot{X(t)} = A(t)X(t)+B(t)U(t) +F(t)W(t)。同時我們的各傳感器、ADC等量測得出的信號其實也都是受到隨機噪聲干擾的信號,在沒有前饋通道的情況下,量測方程實際上應寫爲Z(t)=H(t)X(t)+V(t)(這裏用符號區別於之前講的量測方程,其實本質上都是對狀態的反映)。上兩式中,W(t)爲干擾向量,Z(t)爲觀測向量,V(t)爲觀測噪聲向量。爲了實現最優控制,我們需要實際的狀態向量X(t),但是狀態向量受到隨機噪聲的干擾,因此需要估計狀態向量X(t),並且需要狀態向量的估計值\hat{X(t)}儘可能的接近實際狀態向量。但是在大家實際應用的基本上都是離散的數字系統,因此我們主要來研究線性離散系統的卡爾曼濾波。

        我們給出卡爾曼濾波問題的提法:

        對於線性離散的隨機系統的卡爾曼濾波問題可以描述爲:

設有線性離散隨機系統狀態空間描述如下:

        \left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k)+G(k+1,k)U(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right. \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.1)

        式中:

        X(k)n維狀態向量;

        U(k)r維輸入控制向量;

        W(k)p維隨機干擾向量;

        Z(k)m維量觀測向量;

        V(k)m維隨機觀測噪聲向量;

        \Phi(k+1,k),G(k+1,k),\Gamma(k+1,k),H(k)分別爲n\times n,n \times r,n \times p,m \times n維矩陣。

        並且假定:

        (1)W(k),V(k)均爲零均值的白噪聲或高斯白噪聲序列,且W(k),V(k)相互獨立,即有:

        \ \ \ \ \ \ \ \ E[W(k)]=E[V(k)]=0,COV(W(k),V(k))=0,\\ \ \ \ \\ COV(W(k),W(j))=\left\{ \begin{array}{ll} Q_k \ \ \ & k=j \\ 0 \ \ \ \ & k\neq j \end{array} \right. \ \ \ ,COV(V(k),V(j))=\left\{ \begin{array}{ll} R_k \ \ \ & k=j \\ 0 \ \ \ \ & k\neq j \end{array} \right.

        其中,Q_kW(k)的方差陣,是一個非負定對稱矩陣,R_kV(k)的方差陣,是一個正定對稱矩陣。

        (2)狀態向量X(k)的初始時刻均值爲E[X(0)]=\boldsymbol{ \mu_0 },方差陣P_0=E[(X(0)-\boldsymbol{\mu_0})(X(0)-\boldsymbol{\mu_0})^T]

        (3)X(0)W(k),V(k)不相關,即COV(X(0),W(k))=COV(X(0),V(k))=0

這些假定是合理的,因爲通常我們所接觸到的噪聲都是高斯白噪聲,都是正態隨機過程,經過採樣之後成爲高斯白噪聲序列,且系統噪聲與量測噪聲分屬不同部分一般都是不相關的,且實際有用信號也跟噪聲是不相關的。

        對於式(2.1)所描述的系統,已經得到觀測序列Z(0),Z(1),...,Z(k),要求找出X(j)的線性最優估計\hat{X(j|k)},使得估計值\hat{X(j|k)}X(j)之間的誤差X(j|k)=X(j)-\hat{X(j|k)}的方差E \left \{[X(j)-\hat{X(j|k)}]^T[X(j)-\hat{X(j|k)}] \right \}最小,同時要求估計值\hat{X(j|k)}是觀測序列Z(0),Z(1),...,Z(k)的線性函數,且估計是無偏的,即E[\hat{X(j|k)}]=E[X(j)]

        根據jk的大小,估計問題可以分爲三類:

        (a)j > k 稱爲預測問題

        (b)j = k 稱爲濾波問題

        (c)j < k 稱爲平滑問題

這裏僅研究預測以及濾波問題。

三、線性離散系統的卡爾曼最優預測方程

        通常在推導卡爾曼最優預測方程時,我們均忽略控制輸入信號的作用,即輸入爲0,這樣我們可以得到系統的狀態空間描述爲:

                                                    \left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right.\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.1)

給定觀測序列Z(0),Z(1),...,Z(k),我們需要找出X(k+1)的最優估計\hat{X(k+1|k)},稱此估計爲一步最優估計,使得估計誤差:

                                                                 X(k+1|k)=X(k+1)-\hat{X(k+1|k)}

的方差最小,也就是使得:

                                                E\left \{ [X(k+1)-\hat{X(k+1|k)}]^T [X(k+1)-\hat{X(k+1|k)}]\right \}=min

當我們已經獲得觀測序列Z(0),Z(1),...,Z(k-1)後,假定我們已經找到了狀態向量X(k)的一個最優線性預測估計\hat{X(k|k-1)}。在我們還沒有獲得觀測值Z(k)的情況下,先對k+1時刻的狀態向量X(k+1)做出估計,觀察狀態方程,由於W(k)爲零均值的白噪聲序列,我們無法做具體預測,其最優估計爲0,因此只能把

                                                                \hat{X(k+1|k-1)} = \Phi(k+1,k)\hat{X(k|k-1)}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.2)

當作狀態向量X(k+1)的一個預測估計。

        則當\hat{X(k|k-1)}X(k)的最優線性估計時,我們所得到的\hat{X(k+1|k-1)}也是X(k+1)的最優線性估計,下面給出證明:

證明:由X(k+1|k-1)=X(k+1)-\hat{X(k+1|k-1)}

                                          =\Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) -\Phi(k+1,k)\hat{X(k|k-1)}

                                          =\Phi(k+1,k)(X(k)-\hat{X(k|k-1)})+ \Gamma(k+1,k)W(k)

                                          =\Phi(k+1,k)X(k|k-1)+ \Gamma(k+1,k)W(k)

由於\hat{X(k|k-1)}X(k)的最優線性估計,根據正交定理,估計誤差X(k|k-1)應正交於Z(0),Z(1),...,Z(k-1),同時,由於W(k)均值爲零且與Z(0),Z(1),...,Z(k-1)相互獨立,即有E[(W(k)Z(i))]=E[W(k)]E[Z(i)]=0,\ \ i=1,2,...,k-1,因此W(k)Z(0),Z(1),...,Z(k-1)也正交,根據線性代數的相關知識,我們可以得出X(k|k-1)的線性變換\Phi(k+1,k)X(k|k-1)W(k)的線性變換\Gamma(k+1,k)W(k)的疊加也應與Z(0),Z(1),...,Z(k-1)正交,即X(k+1|k-1)也與Z(0),Z(1),...,Z(k-1)正交。所以,在獲取到觀測向量Z(k)之前,\hat{X(k+1|k-1)}X(k+1)的最優線性估計。

        在獲取到觀測向量Z(k)之後,我們就可以利用觀測值Z(k)來修正\hat{X(k+1|k-1)}

        由於\hat{X(k|k-1)}X(k)的最優線性估計,則通過量測方程,且由於V(k)也是零均值的白噪聲序列且無法預測,我們可以得到k時刻觀測向量Z(k)的最優線性估計爲:

                                                                              \hat{Z(k|k-1)}=H(k)\hat{X(k|k-1)} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.3)

        若獲取到的觀測值Z(k)恰好等於預測估計\hat{Z(k|k-1)},則新的觀測值Z(k)並沒有提供任何有用的信息,無法用於修正\hat{X(k+1|k-1)}。關於這一點,只需要證明估計誤差X(k+1|k-1)\hat{Z(k|k-1)}正交即可,下面給出證明:

證明:根據正交定理,我們有E[(X(k|k-1))(\hat{X(k|k-1})^T]=0

接下來我們計算E[(X(k+1|k-1))(\hat{Z(k|k-1})^T]

                     =E[(\Phi(k+1,k)X(k|k-1)+ \Gamma(k+1,k)W(k))(H(k)\hat{X(k|k-1)})^T]

                     =E[\Phi(k+1,k) X(k|k-1) {\hat{X(k|k-1)}}^TH(k)^T+ \Gamma(k+1,k)W(k)){\hat{X(k|k-1)}}^TH(k)^T]

                     =\Phi(k+1,k)E[X(k|k-1) {\hat{X(k|k-1)}}^T]H(k)^T+ \Gamma(k+1,k)E[W(k)){\hat{X(k|k-1)}}^T]H(k)^T

不難看出上式中右側第一項與第二項均爲0,因此可以得出誤差X(k+1|k-1)\hat{Z(k|k-1)}正交。

因此,若獲取到的觀測值Z(k)恰好等於預測估計\hat{Z(k|k-1)},則\hat{X(k+1|k-1)}就是X(k+1)的最優線性估計。但是在實際中,一般觀測值Z(k)不可能恰好等於預測估計\hat{Z(k|k-1)},但是我們可以分析一下誤差的來源:

Z(k)=H(k)X(k)+V(k) 

             =H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)

Z(k|k-1)=Z(k)-\hat{Z(k|k-1)}

                        =H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)-H(k)\hat{X(k|k-1)}

                        =H(k)[X(k|k-1)]+V(k)

從上式我們不難看出誤差主要來源於X(k)的估計誤差X(k|k-1)以及量測噪聲V(k)。因此我們可以考慮用量測誤差Z(k|k-1)來修正我們之前的預測值\hat{X(k+1|k-1)}使其變爲\hat{X(k+1|k)}。由於採用的是線性估計方法,通常我們使用加權的方法來修正,也就是在(3.2)後加上一項與Z(k|k-1)成正比的修正項K(k)Z(k|k-1),如此,我們可以得到:

                                                      \hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)Z(k|k-1)

即:

                                      \hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)(Z(k)-H(k)\hat{X(k|k-1)}) \ \ \ \ \ \ \ \ \ \ (3.4)

式中K(k)爲一個待定的矩陣,稱爲最優增益矩陣或加權矩陣。式(3.4)可進一步寫成:

                                    \hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)H(k)X(k|k-1)+K(k)V(k) \ \ \ \ \ \ \ \ \ (3.5)

要讓\hat{X(k+1|k)}X(k+1)的最優線性估計,那麼估計誤差:

                                                           X(k+1|k)=X(k+1)-\hat{X(k+1|k)}

必定與觀測序列Z(0),Z(1),...,Z(k)均正交。

                                                   X(k+1) = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k)

則估計誤差:

                  X(k+1|k)=X(k+1)-\hat{X(k+1|k)}

                                      = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k)-\Phi(k+1,k)\hat{X(k|k-1)}- \\ {\ \ \ \ \ \ \ \ K(k)H(k)X(k|k-1)-K(k)V(k)}

                                      = \Phi(k+1,k)(X(k)- \hat{X(k|k-1)})+ \Gamma(k+1,k)W(k)-\\ {\ \ \ \ \ \ \ \ K(k)H(k)X(k|k-1)- K(k)V(k)}

                                      = (\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k)

觀察上式右邊已正交於Z(0),Z(1),...,Z(k-1),因而X(k+1|k)也正交於Z(0),Z(1),...,Z(k-1),因此,只要X(k+1|k)還正交於Z(k),那麼\hat{X(k+1|k)}就是X(k+1)的最優線性估計。因此我們可以根據最優線性估計的條件來反算出最優增益矩陣K(k)。(學過現代控制論的同學可能注意到,上式右側第一項的係數是不是非常眼熟(矩陣(A-LC))?其實就是觀測器的系統矩陣。)

        由X(k+1|k)Z(k)正交,即:

                                                                      E[(X(k+1|k))(Z(k))^T]=0

也就是:

            E\left \{ [(\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)- K(k)V(k)][H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)]^T \right \}

        =E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1){\hat{X(k|k-1)}^T}H(k)^T]+ \\ {\ \ \ \ \ \ \ \ E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1){X(k|k-1)^T}H(k)^T}]}+ \\ {\ \ \ \ \ \ \ \ E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1)V(k)^T H(k)^T] }+ \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){\hat{X(k|k-1)}^T}H(k)^T]} + \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){X(k|k-1)}^T}H(k)^T]} + \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){V(k)^T}H(k)^T]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){\hat{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){V(k)^T}]}

        =(\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){\hat{X(k|k-1)}^T}]H(k)^T+ \\ {\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){X(k|k-1)^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)V(k)^T]H(k)^T }+ \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){\hat{X(k|k-1)}^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){X(k|k-1)}^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){V(k)^T}]H(k)^T} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){\hat{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){V(k)^T}]}=0

觀察上式右側,不難看出第1、3、4、5、6、7、8項均爲0,因此可以進一步化簡爲:

                            {(\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){X(k|k-1)^T}]H(k)^T - \\ K(k)E[V(k){V(k)^T}]=0}

上式中E[X(k|k-1){X(k|k-1)^T}]爲誤差方差陣,簡記爲P(k|k-1),且又E[V(k){V(k)^T}]=R_k,則:

                                             (\Phi(k+1,k)-K(k)H(k))P(k|k-1))H(k)^T - K(k)R_k=0

即:

                               \Phi(k+1,k)P(k|k-1))H(k)^T-K(k)(H(k))P(k|k-1)H(k)^T+R_k)=0

因此可以計算出最優增益矩陣:

                                        K(k)=\Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}\ \ \ \ \ \ \ \ \ \ \ \ (3.6)

上式中引入了誤差方差陣P(k|k-1),下面開始計算他的迭代式:

由其定義式可知:

                                   P(k+1|k)=E[X(k+1|k){X(k+1|k)^T}]

                                                       =E[((\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k)) \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k))^T}]

                                                       =E[{((\Phi(k+1,k)-K(k)H(k))X(k|k-1)X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} +{\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))X(k|k-1)W(k)^T\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))X(k|k-1)V(k)^TK(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)W(k)^T\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)V(k)^TK(k)^T} - \\ {\ \ \ \ \ \ \ \ K(k)V(k))X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} - \\ {\ \ \ \ \ \ \ \ K(k)V(k))W(k)^T\Gamma(k+1,k)^T}+ \\ {\ \ \ \ \ \ \ \ K(k)V(k))V(k)^TK(k)^T}]

                                                       =((\Phi(k+1,k)-K(k)H(k))P(k|k-1)(\Phi(k+1,k)-K(k)H(k))^T +{\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)W(k)^T]\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)V(k)^T]K(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)X(k|k-1)^T](\Phi(k+1,k)-K(k)H(k))^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)W(k)^T]\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)V(k)^TK(k)^T]} - \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))X(k|k-1)^T](\Phi(k+1,k)-K(k)H(k))^T} - \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))W(k)^T]\Gamma(k+1,k)^T}+ \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))V(k)^T]K(k)^T}

不難看出上式右側第2、3、4、6、7、8項均爲0,因此可以進一步化簡爲:

                                      P(k+1|k)=(\Phi(k+1,k)-K(k)H(k))P(k|k-1)(\Phi(k+1,k)-K(k)H(k))^T + {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)R_kK(k)^T}

進一步展開得:

                                    P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T - \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(k+1,k)P(k|k-1)H(k)^TK(k)^T}- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k))P(k|k-1)\Phi(k+1,k)^T} +\\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k))P(k|k-1)H(k))^TK(k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)R_kK(k)^T} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.7)

觀察上式中右側第4項以及第6項,我們有:

                                   K(k)H(k))P(k|k-1)H(k))^TK(k)^T} + K(k)R_kK(k)^T

                               =K(k)[H(k)P(k|k-1)H(k)^T + R_k]K(k)^T

(3.6)式代入上式右側中前面的K(k)有:

                                    K(k)[H(k)P(k|k-1)H(k)^T + R_k]K(k)^T

                          ={\Phi(k+1,k)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}[H(k)P(k|k-1)H(k)^T+R_k]K(k)^T}

                                =\Phi(k+1,k)P(k|k-1)H(k)^TK(k)^T

將上式代回(3.7)式可得:

                                     P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k)P(k|k-1)\Phi(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T}

(3.6)式代入上式中的K(k)有:

                    \begin{array}{ll} P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k]}^{-1}{H(k)P(k|k-1)\Phi(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} \end{array} \ \ \ \ \ \ \ (3.9)

由此得到誤差方差陣的遞推公式。

        最後將式(3.4)(3.6)(3.9)綜合起來可以得到卡爾曼最優預測方程如下:

最優預測估計方程:

        \hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)(Z(k)-H(k)\hat{X(k|k-1)})

最優增益矩陣方程:

        K(k)=\Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}

誤差方差陣遞推方程:

        \begin{array}{ll} P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k]}^{-1}{H(k)P(k|k-1)\Phi(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} \end{array}

至此推導完成。

四、線性離散系統的卡爾曼最優預測的算法實現

(1)給定t_0時刻的初始值:

                                                     \hat{X(0|0_-)}=E[X(0)]=\mu_0

估計誤差的方差陣爲:

                                                  P(0|0_-)=E[(X(0)-\mu_0)(X(0)-\mu_0)^T]

(2)計算t_0時刻的最優增益矩陣K(0)

                                         K(0)=\Phi(1,0)P(0|0_-)H(0)^T[H(0)P(0|0_-)H(0)^T+R_0]^{-1}

(3)此時應獲取到新的觀測值Z(0),結合新觀測值,計算X(1)的最優預測估計:

                                          \hat{X(1|0)}=\Phi(1,0)\hat{X(0|0_-)}+K(0)[Z(0)-H(0)\hat{X(0|0_-)}]

(4)更新估計誤差方差陣:

                             \begin{array}{ll} P(1|0)=\Phi(1,0)P(0|0_-)\Phi(1,0)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(1,0)P(0|0_-)H(0)^T [H(0)P(0|0_-)H(0)^T + R_0]}^{-1}{H(0)P(0|0_-)\Phi(1,0)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(1,0)Q_0\Gamma(1,0)^T} \end{array}

(5)獲取到新的估計誤差方差陣之後可以計算t_1時刻的最優增益矩陣K(1)

(6)根據新的觀測值Z(1),結合K(1),計算X(2)的最優預測估計\hat{X(2|1)}

......

依次迭代。

五、總結

        第四節中的算法實現可以看出卡爾曼最優預測所需要存儲的數據很少,僅僅是更新後的最優增益矩陣、誤差方差陣以及前一時刻的最優狀態預測估計值,不需要存儲其他過去時刻的值,極大地減少了存儲量跟運算量,因此便於實時處理。

        正是由於卡爾曼等人提出的線性系統的狀態空間描述以及相關的估計、濾波算法使得二十世紀六十年代以來航空航天事業、制導、定位、火控等迅猛發展。在此對老爺子致以崇高的敬意。

        寫到這裏最優預測篇也該結束了,即將開啓下一篇--最優濾波篇。

發佈了3 篇原創文章 · 獲贊 5 · 訪問量 1893
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章