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

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