一種基於SINS/GPS組合導航的卡爾曼濾波數值優化方法與流程

2023-04-23 14:31:16

本發明涉及組合導航系統數據濾波的技術領域，具體涉及一種基於SINS/GPS組合導航的卡爾曼濾波數值優化方法。

背景技術：
在SINS/GPS組合導航系統的數據濾波環節，往往包含對「有色噪聲」的處理，這時已經不符合經典Kalman濾波的適用條件，為了仍然能夠使用最優估計技術，需要進行狀態擴充。但是隨著狀態維數的增加，計算量急劇膨脹，數值穩定性下降，甚至會出現「維數災難」。對於這個問題，目前一般的方法是，利用平方根信息濾波SRIF、U-D分解濾波、SVD濾波等分解濾波優化算法來減少計算量和增強數值魯棒性，並利用並行算法提高濾波實時性。但是，分解算法的推導過程需要用到複雜的矩陣理論知識，在工程應用上有一定的難度；而且分解濾波算法複雜度仍為O(n3)，隨著狀態維數的進一步增加，實時性也受到了挑戰。並行Kalman濾波雖然大幅提升了實時性，但是其解耦過程基於量測更新強制滯後一個濾波周期來實現，這會帶來一定的精度損害，在多次迭代後可能會引起數值不穩定問題。此外，作為一類通用的優化算法，上述幾種濾波器都沒有基於SINS/GPS組合導航系統的特點進行專門設計，國內外也鮮有研究利用系統的數值特徵對Kalman濾波過程進行深入的優化。

技術實現要素：
本發明要解決的技術問題是：克服現有算法的不足，從數值計算的角度出發，提出一種工程上簡單易行的，利用SINS/GPS系統數字特徵進行離線推導優化和實時並行處理的高效率無損卡爾曼濾波優化算法。本發明解決上述技術問題採用的技術方案為：一種基於SINS/GPS組合導航的卡爾曼濾波數值優化方法，其特徵在於包括下列步驟：步驟(1)、建立一種基於位置-速度測量組合的SINS/GPS組合導航鬆耦合閉環Kalman濾波模型，用3階方陣展開表示模型中的參數矩陣，包括狀態係數An、狀態轉移矩陣φn、系統噪聲係數Gn、量測矢量係數Hn、系統噪聲方差Qn、量測噪聲方差Rn、誤差協方差時間更新值Pn(-)、誤差協方差量測更新值Pn(+e)及Kalman濾波增益Kn；步驟(2)、利用係數矩陣An的稀疏性、特殊子分塊和中間變量在Matlab環境中使用符號運算功能，對式按3階子分塊展開計算，離線推導出離散濾波器轉移矩陣φn的簡化計算形式；步驟(3)、利用步驟(2)推導得到的φn的分塊稀疏性、特殊子分塊及噪聲矩陣ΓnQn的對稱性，在Matlab環境中離線推導出的簡化計算形式；步驟(4)、利用φn的分塊稀疏性、特殊子分塊、中間變量以及協方差矩陣Pn(-)的對稱性，在Matlab環境中對式按3階子分塊展開計算，推導出Pn(-)時間更新的簡化計算形式；步驟(5)、利用系統量測方程係數矩陣Hn的分塊稀疏性、特殊子分塊，在Matlab環境中對式進行離線公式推導，得到濾波增益Kn的簡化計算形式；步驟(6)、利用Hn的分塊稀疏性、特殊子分塊、協方差矩陣Pn(-)的對稱性，在Matlab環境中對式Pn(+e)＝(I-KnHn)Pn(-)按3階子矩陣展開計算，離線推Pn(+e)量測更新的簡化計算形式；步驟(7)、細分Pn(+e)和Pn(-)的計算過程，並將耗時矩陣Pn(-)劃分「有用」、「無用」信息，通過在子過程層級分析對參數的依賴關係，獲得一種新的數值解耦機制，在此基礎上實現對濾波過程的無損並行處理。進一步的，所述的步驟(2)對3的計算優化具體包含如下步驟：步驟(21)、利用係數矩陣An的稀疏性及其特殊子分塊，離線推導出的簡化計算形式並作為中間變量保存，推導結果表明具有分塊稀疏性，而第3列分塊Si3、對角線上的分塊S55、S66為特殊子分塊；步驟(22)、利用係數矩陣An、的稀疏性及其特殊子分塊，將2按3階分塊展開計算，離線推導出的簡化計算形式並作為中間變量保存，指出具有分塊稀疏性，而第3列分塊Ti3、對角線上的分塊T55、T66為特殊子分塊；步驟(23)、在步驟(21)、(22)的計算過程中引入數值為O3×3的子矩陣符號A31、S34、S35，以保持計算形式的連貫性，減少實時計算程序的編程複雜度。進一步的，所述的步驟(3)對ΓnQn的計算優化具體包含如下步驟：步驟(31)、利用噪聲驅動矩陣G(t)的稀疏性，按3階分塊展開計算Q1n＝G(t)Q(t)G(t)T，離線推導出Q(t)的離散化形式，指出Q1n具有分塊對角形式；步驟(32)、利用Q1n的對角形式、φn的稀疏性以及ΓnQnΓTn的對稱性，按離線推導出ΓnQn的簡化計算形式。進一步的，所述的步驟(5)對Kn的計算優化具體包含如下步驟：步驟(51)、利用Hn的稀疏性，按3階分塊展開計算其結果的逆矩陣保存為中間變量D；步驟(52)、利用Hn的稀疏性以及Pn(-)的對稱性，按離線推導出Kn的簡化計算形式。進一步的，所述的步驟(7)並行優化處理具體包含如下步驟：步驟(71)、將Pn(+e)的計算過程按Pn(+e)＝Pn(-)-KnHnPn(-)細分，KnHnPn(-)代替Pn(+e)建立新的量測更新；步驟(72)、定義Pn(-)的2、3分塊列定義為「有用」信息，其餘為「無用」信息，「無用」信息的更新過程與新量測更新過程存在數值解耦關係，這兩個過程實時並行處理；步驟(73)、將Pn(-)「有用」信息的計算過程按細分，子過程更新與ΓnQn更新存在數值解耦關係，實時並行處理。本發明與現有技術相比的優點在於：(1)本發明充分利用了SINS/GPS的數字特徵進行離線優化，其實時計算效率優於流行的通用濾波優化算法。(2)與通用的分解濾波優化算法相比，本發明沒有複雜的矩陣理論推導，更易於工程實現。(3)本發明的數值解耦過程僅通過系統的數字特徵推導獲得，沒有流行並行濾波算法中將量測更新強制滯後一個濾波周期的處理，因此具有更高的濾波精度。(4)本發明對濾波計算的優化是純數值離線優化和實時並行優化，能夠在通用優化算法的基礎上進行二次優化，以達到更高計算效率的目的。附圖說明圖1為本發明數值優化的對象——閉環Kalman濾波周期更新過程。圖2為本發明提出的Kalman濾波數值優化過程。圖3為本發明提出的無損解耦Kalman濾波並行處理機制。具體實施方式下面結合圖1和圖2具體說明本發明的實施過程及主要處理方法。該方法的具體步驟如下：步驟1.採用間接法濾波，直接給出系統狀態方程：式1-1其中狀態誤差x(t)、系統白噪聲ω(t)、係數矩陣A(t)、G(t)為：式1-2式1-3式1-4式1-5式中，γx、γy、γz分別為平臺誤差角東、北、天方向分量；δvx、δvy、δvz為東、北、天方向速度誤差分量；δL、δλ、δh為經度、緯度、高度誤差；εc、εr、▽a分別為隨機常值漂移矢量、隨機馬爾科夫過程漂移矢量以及加速度計隨機漂移矢量；ωg、ωr、ωa分別為陀螺儀隨機漂移白噪聲、一階馬爾科夫驅動白噪聲和加速度計隨機漂移白噪聲；為基體姿態矩陣；O3×3為零矩陣；I3×3為單位矩陣；從數值角度指出，其他分塊中Ai3僅第1列向量非零、AIMUii為對角矩陣。直接給出系統的白噪聲方陣Q(t)為：式1-6其中Qii為對角矩陣。採用位置、速度組合模式定義濾波器的觀測矢量zobs(t)：式1-7直接給出量測方程：式1-8其中係數矩陣H(t)、觀測白噪聲n(t)為：式1-9aH13＝diag{Rn,RecosL,1}式1-9bn(t)＝(Nn,Ne,Nh,Mn,Me,Mu)T式1-10直接給出測量白噪聲協方差陣R(t)：式1-11直接給出系統狀態方程(式1-1)及觀測方程(式1-8)的離散形式：xn＝φnxn-1+Bnucn+Γnωn式1-12zn＝Hnxn+nn式1-13其中ucn為控制矢量，φn誤差轉移矩陣，Bn為控制係數矩陣，Γn為狀態噪聲驅動矩陣。直接給出有控制的閉環Kalman濾波迭代計算過程：式1-14a式1-14bPn(+e)＝(I-KnHn)Pn(-)式1-14cucn＝-Knzobsn式1-14d式中，～表示Kalman濾波器估計(或預測)的參數值；(+e)表示tn時刻估計復位後的參數值；(-)表示tn時刻估計前的參數值；(+c)表示tn時刻控制作用校正後的參數值。總結Kalman濾波的實時計算過程，如圖1所示，主要包括參數的離散化，時間更新和量測更新過程。涉及多次高階矩陣的乘法運算，計算量大，需要進行計算效率的優化。步驟2.φn計算的優化。φn的計算依據下式進行：式2-1計算主要圍繞18階方陣An進行，將An按3階方陣分塊展開，在Matlab環境中利用符號運算功能，推導得到的簡化形式，如式2-2、式2-3。式2-2式2-3其中Sij、Tij寫成易於計算機編程的形式為：式2-4a式2-4bSi5＝Si4,i,j＝1,2式2-4c式2-4d式2-4e式2-4f式2-4g式2-5a式2-5bT55＝(AIMU22S55)Sim式2-5cT66＝(AIMU33S66)Sim式2-5d式2-4、式2-5中涉及A31、S34、S35的計算項為O3×3，涉及Ai3、Si3、AIMUii、S55、S66等特殊分塊的計算項為簡化計算項，用Sim表示。其中Ai3、Si3、Ti3具有如下矩陣形式：式2-6對於任意3階方陣Xi3有以下性質：式2-7式2-7可用於式2-4、式2-5和後續計算的簡化。將An、代入式2-1可得：式2-8其中：式2-9a式2-9b指出φ13、φ23也具有式3.1-3的形式，φ33可分解為具有式3.1-3的形式矩陣和I3×3的和；類似地，知φ55、φ66為對角陣。綜上所述，通過使用3階分塊展開推導，在φn的離線推導結果中直接得到約佔一半數量的O3×3、I3×3分塊；對於其他分塊的計算，可以利用特殊分塊Ai3、Si3、Ti3和Ajj、Sjj、Tjj的性質進行簡化；引入中間變量減少大量的重複計算。統計結果表明離線優化減少了φn更新90％的浮點乘法和94％的加法運算，同時內存花銷減少69％。步驟3.Qn計算的優化。直接計算ΓnQn，作二階近似有：式3-1其中：式3-2式中：式3-3顯然，Q1n和ΓnQn均為對稱矩陣，對ΓnQn僅需計算上三角部分，把Q1n代入ΓnQn可得：式3-4其中：式3-5a式3-5b式3-5c式3-5d式3-5e對ΓnQn計算的優化主要是利用稀疏矩陣G(t)、對角陣分塊Q22、Q33φ55、φ66的特殊性以及ΓnQn的對稱性進行離線推導。特別地，式3-5c中Qj-3,j-3φjj、式3-5d中Qn55、式3-5e中Qn66仍為對角陣，可用於簡化計算。統計結果表明，本發明減少了ΓnQn更新94％(96％)的乘法(加法)運算和88％的內存花銷。步驟4.Pn(-)計算的優化。為推導方便，將Pn(-)、Pn(+e)統一用Pn表示(實際編程中，同一個濾波周期中的Pn(-)、Pn(+e)也只用Pn的不同狀態表示)，Pn按3×3階矩陣分塊，寫成：式4-1Pn(-)為對稱矩陣，則有：式4-2先不考慮ΓnQn，式1-14a寫成：式4-3代入式2-8、式4-1得：式4-4a式4-4bPn,66＝(φ66Pn-1,66φ66)Sim式4-4c式中Temp是為減少重複計算在計算機中保存的中間量、Rep表示已在Temp中計算過的重複項，實質是的計算結果。對Temp的計算可利用的特殊性進行優化。考慮ΓnQn，把式3-4、式4-1代入式1-14a得：Pn,ij＝Pn,ij+Qn,ij,i,j＝1,2,3,5,6；i≥j式4-5綜上所述，Pn(-)的計算分為式3-4和式4-3兩個步驟，優化過程主要利用φn的分塊離散性、φi3及φii(i＝5,6)的特殊性以及引入中間量避免重複計算項。統計結果表明，對Pn(-)的優化減少了69％的計算量和44％的內存開銷。步驟5.Kn計算的優化。把式1-9、式1-11和式4-1代入到式1-14b的求逆部分可得：式5-1令式5-2容易證明DT＝D，寫成：式5-3則有：式5-4記為：式5-5有：式5-6從上述推導過程可知，採用分塊矩陣進行推導優化後，Kn的計算只需要執行式5-1、式5-2、式5-6三個相對簡單的過程，利用Hn的稀疏性，避免了式1-14b中5次高階矩陣乘法。對優化結果的統計表明，本發明減少了Kn更新89％的計算量和61％的內存花銷。步驟6.Pn(+e)計算的優化。將式1-14c改寫成：Pn(+e)＝Pn(-)-KnHnPn(-)式6-1主要計算量在於KnHnPn(-)，因為Pn(+e)、及Pn(-)都是對稱矩陣，則KnHnPn(-)也是對稱矩陣，僅需計算上三角部分。記KnHnPn(-)為ΔPn：式6-2代入式1-9、式4-1及式5-5得：ΔPij＝Ki1H13Pn,3j+Ki2Pn,2j,i,j＝1,2,...,6；i≤j式6-3則有：Pn,ij＝Pn,ij-ΔPij,i,j＝1,2,...,6；i≤j式6-4從推導過程可以看到，利用Pn(+e)的對稱性以及Hn分塊稀疏性，推導出Pn(+e)簡潔的計算形式(式6-3、式6-4)，佔主要計算量的式6-3中僅需Kn與Pn(+e)第2、3分塊行的作少量3階矩陣乘法，減少大量不必要計算。與常規算法作比較，減少了85％的計算量和61％的內存開銷。步驟7.無損解耦並行優化。從優化結果看，Pn(-)的計算量仍約佔整個濾波過程的一半，成為進一步提高計算效率的瓶頸。考慮對Pn(-)的更新實施並行處理，以提高實時性。從上述Kalman濾波迭代計算推導過程可知，除Pn(+e)外，其它參量的更新或與Pn(-)無關或僅與第2、3分塊行和2、3分塊列有關。因此定義Pn(-)第2、3分塊行(列)為「有用」信息，其餘分塊是「無有」信息。更進一步，因為Pn(-)是對稱矩陣，因此「有用」分塊實際上只有第2、3分塊列。「無用」分塊用「×」表示，則Pn(-)可寫成：式3.6-1在量測更新中，僅Pn(+e)與「無用」數據耦合。將Pn(+e)的更新細分成ΔPij(式6-3)的更新和Pn(-)累加(式6-4)兩個過程，並用ΔPij代替Pn(+e)，與式1-14b、式1-14d構成新的量測更新過程。因此，獲得Pn(-)「無用」信息更新與新量測更新的數值解耦，可以將這兩個過程並行處理，以解決Pn(-)計算耗時的瓶頸問題。類似地，對Pn(-)「有用」分塊的更新細分成按式4-3更新和按式4-5更新兩個計算步驟，顯然，第一個步驟中的計算與時間更新中ΓnQn的計算是可以進行並行處理。綜上所述，使用並行處理優化後的Kalman濾波執行過程如圖3所示。與無並行優化處理相比，圖3所示的並行優化效率提高約25％。更重要的是本發明的解耦過程是利用SINS/GPS組合導航的數字特點，通過嚴格的數字推導獲得，與流行的並行處理方法不同，不需要作「強制滯後」處理，是一種無損的並行處理方法。本發明未詳細闡述的部分屬於本領域公知技術。