2010-第四章-非線性系統(tǒng)的Kalman濾波

4.1擴展的卡爾曼濾波方程前面講的Kalman濾波要求系統(tǒng)狀態(tài)方程和觀測方程都是線性的。然而，許多工程系統(tǒng)往往不能用簡單的線性系統(tǒng)來描述。例如，導(dǎo)彈控制問題，測軌問題和慣性導(dǎo)航問題的系統(tǒng)狀態(tài)方程往往不是線性的。因此，有必要研究非線性濾波問題。對于非線性模型的濾波問題，理論上還沒有嚴格的濾波公式。一般情況下，都是將非線性方程線性化，而后，利用線性系統(tǒng)Kalman濾波基本方程。這一節(jié)我們就給出非線性系統(tǒng)的Kalman濾波問題的處理方法。為了方便描述，下面僅限于討論下列情況的非線性模型(3.2.8.1)(3.2.8.2)式中，是狀態(tài)向量，是觀測向量，和是噪聲；是的非線性函數(shù)，具有一階連續(xù)導(dǎo)數(shù)；是的非線性函數(shù)，具有一階連續(xù)導(dǎo)數(shù)。和都是均值為零的白噪聲序列，其統(tǒng)計特性如下，，另外，已知初始條件，即的統(tǒng)計特性。下面僅介紹推廣的Kalman濾波方法，即圍繞濾波值的線性化濾波方法，這種方法是先將非線性模型線性化，而后應(yīng)用線性系統(tǒng)的Kalman濾波基本公式。由系統(tǒng)狀態(tài)方程(3.2.8.1)可得(2.3.8.3)(2.3.8.4)(2.3.8.5)則狀態(tài)方程為(3.2.8.6)初始值為。同基本Kalman濾波模型相比，在已知求得前一步濾波值的條件下，狀態(tài)方程(3.2.8.6)中增加了非隨機的外作用項。把觀測方程的圍繞進行泰勒展開，略去二次以上項，可得(3.2.8.7)令則觀測方程為(3.2.8.8)應(yīng)用Kalman濾波基本方程可得(3.2.8.9)即(3.2.8.10)式中(3.2.8.11)即(3.2.8.12)方程(3.2.8.13)稱為估計誤差方差陣的遞推方程(3.2.8.13)(3.2.8.14)式中濾波值和濾波誤差方差陣的初始值為，(3.2.8.15)推廣的Kalman濾波的優(yōu)點是不必預(yù)先計算標(biāo)稱軌道。注意推廣的Kalman濾波只要在濾波誤差及一步預(yù)測誤差較小時才適用。4.2強跟蹤濾波基本理論本小節(jié)引入自1990年以來發(fā)展起來的一個有強跟蹤濾波器理論[2-3]。 考慮如下一大類非線性系統(tǒng)的狀態(tài)估計問題 (3.2.9.1) (3.2.9.2)其中，整數(shù)為離散時間變量，為狀態(tài)向量，為輸入向量，為輸出向量。非線性函數(shù),具有關(guān)于狀態(tài)的一階連續(xù)偏導(dǎo)數(shù)。為已知的矩陣。系統(tǒng)噪聲和測量噪聲勻是高斯白噪聲，并具有如下的統(tǒng)計特性 (3.2.9.3)(3.2.9.4)(3.2.9.5)(3.2.9.6)其中,為對稱的非負定陣,為對稱正定陣。 初始狀態(tài)為高斯分布的隨機向量，且滿足統(tǒng)計特性(3.2.9.7)(3.2.9.8)并且有與統(tǒng)計獨立。 系統(tǒng)(3.2.9.1)-(3.2.9.2)的狀態(tài)估計問題可以首先選擇在3.2.7節(jié)引入的擴展Kalman濾波器(ExtendedKalmanFilter——EKF)進行解決(3.2.9.9)其中，為狀態(tài)的一步預(yù)報值。(3.2.9.10)增益陣(3.2.9.11)預(yù)報誤差協(xié)方差陣(3.2.9.12)狀態(tài)估計誤差協(xié)方差陣(3.2.9.13)殘差序列(3.2.9.14)式(3.2.9.11)中(3.2.9.15)式(3.2.9.12)中(3.2.9.16) 式(3.2.9.9)—(3.2.9.16)就是著名的擴展Kalman濾波器的遞推公式。此時輸出殘差序列的協(xié)方差陣為(3.2.9.17) 當(dāng)系統(tǒng)模型(3.2.9.1)-(3.2.9.2)具有足夠的精度，并且濾波器的初始值選擇得當(dāng)時，上述的EKF可以給出比較準(zhǔn)確的狀態(tài)估計值。 然而，通常的情況是，系統(tǒng)模型(3.2.9.1)-(3.2.9.2)具有模型不確定性，即此模型與其所描述的非線性系統(tǒng)不能完全匹配，造成模型不確定性的主要原因有： 1)模型簡化。對于比較復(fù)雜的系統(tǒng)，若要精確描述其行為，通常需要較高維數(shù)的狀態(tài)變量，甚至無窮維的變量。這對系統(tǒng)狀態(tài)的重構(gòu)造成了極大不便。因此，通常人們都要使用模型簡化的辦法，使用較少的狀態(tài)變量來描述系統(tǒng)的主要特征，忽略掉實際系統(tǒng)某些較不重要的因素。也就是存在所謂的未建模動態(tài)。這些未建模動態(tài)在某些特殊條件下有可能被激發(fā)起來，造成模型與實際系統(tǒng)之間較大的不匹配[2-3]。 2)噪聲統(tǒng)計特性不準(zhǔn)確。即所建模型的噪聲統(tǒng)計特性與實際過程噪聲的統(tǒng)計特性有較大差異。所建模型噪聲的統(tǒng)計特性一般過于理想。實際系統(tǒng)在運行過程中，可能會受到強電磁干擾等隨機因素的影響，造成實際系統(tǒng)的統(tǒng)計特性發(fā)生較大的變動。 3)對實際系統(tǒng)初始狀態(tài)的統(tǒng)計特性建模不準(zhǔn)。 4)實際系統(tǒng)的參數(shù)發(fā)生變動。由于實際系統(tǒng)部件老化、損壞等原因，使得系統(tǒng)的參數(shù)發(fā)生變動(緩變或突變)，造成原模型與實際系統(tǒng)不匹配。 一個很遺憾的事實是，EKF關(guān)于模型不確定性的魯棒性很差，造成EKF會出現(xiàn)狀態(tài)估計不準(zhǔn)，甚至發(fā)散等現(xiàn)象[2-3]。 此外，EKF在系統(tǒng)達到平穩(wěn)狀態(tài)時，將喪失對突變狀態(tài)的跟蹤能力。這是EKF類濾波器(包括卡爾曼濾波器在內(nèi))的另一大缺陷。造成這種情況的主要原因是，當(dāng)系統(tǒng)達平穩(wěn)狀態(tài)時，EKF的增益陣將趨于極小值。這時，若系統(tǒng)狀態(tài)發(fā)生突變，預(yù)報殘差將隨之增大。然而，此時的增益陣仍將保持為極小值，不會隨的增大而相應(yīng)地增大。因此，由式(3.2.9.9)(3.2.9.9)得知，EKF將喪失對突變狀態(tài)的跟蹤能力。從這個意義上可以認為，EKF類濾波器是一種開環(huán)濾波器，因為這類濾波器的增益陣不會隨濾波效果自適應(yīng)地進行調(diào)整，以始終保持對系統(tǒng)狀態(tài)的準(zhǔn)確跟蹤能力。這一現(xiàn)象對定常線性隨機系統(tǒng)將更加直觀一些。此時，只需用通常的Kalman濾波器(KF)進行狀態(tài)估計。而KF的增益陣可以根據(jù)線性系統(tǒng)的參數(shù)(A,B,C)離線計算出來，然后存儲在計算機中在線應(yīng)用。這時，如果系統(tǒng)狀態(tài)發(fā)生突變，濾波器的增益陣當(dāng)然不會隨之變動，因此，KF也就喪失了對突變狀態(tài)的跟綜能力。所以說,KF也是一種開環(huán)濾波器。 為了克服EKF存在的上述缺陷，迫切需要有一種性能更加優(yōu)越的濾波器。為此，人們提出如下強跟蹤濾波器的新概念。定義3.2.9.1稱一個濾波器為強跟蹤濾波器(StrongTrackingFilter，STF)，若它與通常的濾波器相比，具有以下優(yōu)良的特性： 1）較強的關(guān)于模型不確定性的魯棒性。 2）極強的關(guān)于突變狀態(tài)的跟蹤能力。甚至在系統(tǒng)達平穩(wěn)狀態(tài)時，仍保持對緩變狀態(tài)與突變狀態(tài)的跟蹤能力。 3）適中的計算復(fù)雜性。 顯然，特性1）和2)就是為了克服EKF的上述兩大缺陷而提出來的。特性3)是為了使得STF便于實時應(yīng)用。 關(guān)于系統(tǒng)(3.2.9.1)-(3.2.9.2)的一類強跟蹤濾波器應(yīng)具有如下的一般結(jié)構(gòu)(3.2.9.18)其中(3.2.9.19)(3.2.9.20) 現(xiàn)在面臨的難點就是要在線確定時變增益陣，使得此濾波器具有強跟蹤濾波器的所有特性。為此，提出一個正交性原理。正交性原理 使得濾波器(3.2.9.18)為強跟蹤濾波器的一個充分條件是在線選擇一個適當(dāng)?shù)臅r變增益陣，使得 1)(3.2.9.21)2)(3.2.9.22)其中，條件2)要求不同時刻的殘差序列處處保持相互正交，這也是正交性原理這一名稱的由來．條件1)實際上就是原來的EKF的性能指標(biāo)． 說明此正交性原理的一個淺顯的例子是，早已證明，當(dāng)模型與實際系統(tǒng)完全匹配時，Kalman濾波器(KF)的輸出殘差序列是不自相關(guān)的高斯白噪聲序列,因此，式(3.2.9.22)是滿足的。而式(3.2.9.21)就是KF的性能指標(biāo)，因此當(dāng)然也是滿足的。 當(dāng)由于模型不確定性的影響，造成濾波器的狀態(tài)估計值偏離系統(tǒng)的狀態(tài)時，必然會在輸出殘差序列的均值與幅值上表現(xiàn)出來。這時，若我們在線調(diào)整增益陣,強迫(3.2.9.22)式仍然成立，使得殘差序列仍然保持相互正交，則可以強迫強跟蹤濾波器保持對實際系統(tǒng)狀態(tài)的跟蹤。這也是＂強跟蹤濾波器＂一詞的由來。 此正交性原理具有很強的物理意義。它說明當(dāng)存在模型的不確定性時，應(yīng)在線調(diào)整增益陣，使得輸出殘差始終具有類似高斯白噪聲的性質(zhì)。這也表明已經(jīng)將輸出殘差中的一切有效信息提取出來。 當(dāng)不存在模型的不確定性時，強跟蹤濾波器正常運行，式(3.2.9.22)已自然滿足，不起調(diào)節(jié)作用。此時的強跟蹤濾波器就退化為通常的基于性能指標(biāo)(3.2.9.21)的EKF。注釋3.2.9.1 此正交性原理的核心是式(3.2.9.22)2)(3.2.9.22)當(dāng)用其它的性能指標(biāo)取代式(3.2.9.21)后，就可以得到另外一些變形的類似的正交性原理．因此，當(dāng)在原有的濾波器上附加上條件(3.2.9.22)后，就可以改造原來的濾波器，使其具有強跟蹤濾波器的性質(zhì)．注釋3.2.9.2 對非線性系統(tǒng)，實際應(yīng)用此正交性原理時，式(3.2.9.21)與(3.2.9.22)很難精確滿足。這時,只需使其近似滿足即可，以減少計算量，保持強跟蹤濾波器具有良好的實時性。3.2.9.3一種帶次優(yōu)漸消因子的擴展卡爾曼濾波器(SFEKF) 為了使得濾波器具有強跟蹤濾波器的優(yōu)良性能，一個自然的想法是采用時變的漸消因子對過去的數(shù)據(jù)漸消，減弱老數(shù)據(jù)對當(dāng)前濾波值的影響。這可以通過實時調(diào)整狀態(tài)預(yù)報誤差的協(xié)方差陣以及相應(yīng)的增益陣來達到。為此，我們修改上面的EKF中的公式(3.2.9.2.12)為：(3.2.9.3.1)其中，為時變的漸消因子。(3.2.9.2.9)—(3.2.9.2.11)式,(3.2.9.3.1)式,(3.2.9.2.13)—(3.2.9.2.17)式構(gòu)成了一種帶次優(yōu)漸消因子的擴展卡爾曼濾波器，簡記為：SFEKF(SuboptimalFadingExtendedKalmanFilter),參見[17,18]。我們之所以稱其為次優(yōu)漸消因子，是因為我們通常采用次優(yōu)的算法來求取,以提高算法的實時性。3.2.9.1一個有用的定理 現(xiàn)在的目標(biāo)是應(yīng)用上一節(jié)的正交性原理來確定時變次優(yōu)漸消因子,進而也就確定了時變增益陣。首先，我們給出一個有用的定理。定理3.2.9.1 令：,其中，為由上面的SFEKF得到的狀態(tài)估計值。若成立，對，就有下式成立：(3.2.9.3.2)證明：略因此，有3.2.9.2次優(yōu)漸消因子的確定 根據(jù)正交性原理，為了使SFEKF具有強跟蹤濾波器的性質(zhì)，在每一采樣時刻，應(yīng)在線確定其增益陣，強迫使式(3.2.9.2.22)保持成立[17,18]，即(3.2.9.3.13) 由定理3.2.9.1與(3.2.9.3.13)式得知，當(dāng)在線選擇適當(dāng)?shù)臅r變增益陣,使得(3.2.9.3.14)時，則正交性原理的(3.2.9.2.22)式必然成立。為此，可以令(3.2.9.3.15)并定義,(3.2.9.3.16)其中，。 由此可知，(3.2.9.3.14)的符合程度可以通過求解下面的性能指標(biāo)來衡量：(3.2.9.3.17) 由性能指標(biāo)(3.2.9.3.17)求解可采用任何一元無約束非線性規(guī)劃方法，在這里我們首先給出一種梯度方法。算法3.2.9.1梯度方法 次優(yōu)漸消因子可由下面的迭代公式得到[19]：(3.2.9.3.18) 初始值：.式(3.2.9.3.18)中，為迭代步長，此參數(shù)的選擇需要一定的技巧，以使快速衰減．其中，為迭代步數(shù)．(3.2.9.3.18)式中的可由下列公式得到： (3.2.9.3.19)其中(3.2.9.3.20)(3.2.9.3.21)(3.2.9.3.22) (3.2.9.3.23) (3.2.9.3.24) (3.2.9.3.25) 式(3.2.9.3.23)中，殘差的協(xié)方差陣的實際值在的迭代求解中是未知的，它可以