一種基于羅德里格參數(shù)和二階非線性量測的濾波對準算法
【技術(shù)領域】
[0001] 本發(fā)明涉及慣性導航技術(shù)領域，具體是一種基于羅德里格參數(shù)和二階非線性量測 的濾波對準算法。
【背景技術(shù)】
[0002] 動基座對準能夠有效提高捷聯(lián)慣導系統(tǒng)載體平臺的機動性能，具有很高的軍事應 用價值。動基座對準的研宄內(nèi)容主要包括兩個方面，一是建立大失準角條件下的非線性誤 差模型 [1~3];二是設計相應的非線性濾波估計算法[2~9]。
[0003] 依據(jù)姿態(tài)描述方式的不同，可以得到不同的非線性誤差模型，如基于四元數(shù)非線 性誤差模型 [1]、基于歐拉角非線性誤差模型[2]、基于修正Rodrigues參數(shù)非線性誤差模型
[3] ，以及大方位失準角條件下，基于方位角正余弦函數(shù)的非線性誤差模型[4]等。其中，四 元數(shù)非線性誤差模型無奇異點，使用最為廣泛，但在設計濾波算法時需要考慮其模值約束 的影響。歐拉角姿態(tài)描述法存在奇異點，因此不適用于任意姿態(tài)對準，且基于歐拉角的非 線性誤差模型中含有狀態(tài)量的正余弦函數(shù)，使得誤差模型非線性增大。傳統(tǒng)的基于修正 Rodrigues參數(shù)的誤差模型雖可避免奇異點，但是模型非線性度同樣很大。此外，傳統(tǒng)方法 建立動基座對準非線性誤差模型時，均以動態(tài)載體系和動態(tài)導航系之間的實時姿態(tài)為估計 對象，系統(tǒng)方程及量測方程均建立在動態(tài)的導航坐標系下。以速度為觀測量時，以上誤差模 型只有系統(tǒng)方程為非線性，量測方程則是線性的。
[0004] 在非線性濾波算法的選擇上，常規(guī)EKF濾波算法需要求導計算Jacobian矩陣，且 在處理嚴重非線性問題時，可能出現(xiàn)濾波誤差增大甚至發(fā)散的現(xiàn)象。因此，一類基于sigma 點的非線性濾波算法成為研宄的熱點，如UKF濾波[2'3'4]、改進強跟蹤UKF濾波[5]、粒子濾波 [6]、Gauss-Hermite濾波[7'8]，以及容積卡爾曼濾波（CKF)[9]等?；趕igma點的非線性濾 波算法，用確定性或隨機采樣策略逼近非線性函數(shù)的概率分布，無需對非線性模型求導，且 可以通過優(yōu)化采樣策略，減小計算量并提高非線性函數(shù)概率分布的近似精度?？傮w來講，基 于sigma點的非線性濾波算法可以做到計算量與EKF濾波相當，但是精度優(yōu)于EKF濾波。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于提供一種基于羅德里格參數(shù)和二階非線性量測的濾波對準算 法，以解決上述【背景技術(shù)】中提出的問題。
[0006] 為實現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：
[0007] 一種基于羅德里格參數(shù)和二階非線性量測的濾波對準算法，由慣性系動基座對準 過程、二階非線性量測濾波估計算法、經(jīng)典Rodrigues參數(shù)奇異點及其處理方法，
[0008] 具體描述如下：
[0009] 步驟1，慣性系動基座對準過程
[0010] 慣性系動基座對準算法以實時姿態(tài)陣的鏈式分解為基礎，
[0012] 其中，nt系為實時導航坐標系，即載體時變位置東北天地理坐標系；in為導航慣性 系，與動基座對準開始時刻的n系重合；b為載體坐標系；ib為載體慣性系，與對準開始時刻 的b系重合；式（1)中，是運動的nt系相對于導航慣性系i^勺姿態(tài)陣，由GPS輸出位置 信息解析計算；能利用陀螺輸出進行姿態(tài)跟蹤，所以，慣性系動基座對準過程是對常值姿 態(tài)陣ct的估計；
[0013] 利用牛頓第二定律和哥氏定理，得到慣性系比力方程如下
[0015] 其中，P⑴為t時刻載體對地速度在導航慣性系in內(nèi)的投影；#(0為t時刻載體 所在位置重力加速度在導航慣性系in內(nèi)投影；_T'(〇為t時刻理想比力值；
[0016] 對式（2)兩端分別進行積分，并記
[0019] 利用GPS輸出完成式⑶中"仏)的求解，
[0024] 進一步，在tk_^tk更新周期內(nèi)，假設，為常矢量，導航慣性系in內(nèi)對地速度 為線性函數(shù)，即
[0027] 其中，tG[tH，tk];T=VtH為GPS量測更新周期
[0028] 將式（9)、（10)代入式（7)、（8)中，整理得

[0032] 利用捷聯(lián)慣導姿態(tài)、速度二子樣更新算法實現(xiàn)對式（4)中)^化)的求解；進一步， 考慮陀螺儀隨機常值漂移eb和加速度計隨機常值零偏V6的影響，推導得
[0036] 其中，#為姿態(tài)誤差角；汾。仏)為加速度計慣性系比力積分誤差；
[0037]由式（2)、（3)、⑷和式（13)可得，求解常值姿態(tài)陣q的觀測方程為
[0039] 進一步，用經(jīng)典Rodrigues參數(shù)法來等價描述姿態(tài)陣，記對應Rodrigues參數(shù) 為1，則二者滿足凱萊變換關系式，即
[0041] 將式（17)代入式（16)，整理得，
[0043] 其中，#^ 匕）；wv包含慣性器件測量噪聲的積分和隨機擾動的積分，且有
[0045] 式（18)即是與姿態(tài)陣q等價的Rodrigues參數(shù)1的觀測方程，若能估計出 Rodrigues參數(shù)1，則依據(jù)式（17)得到<^ . ?
[0046] 綜上，慣性系動基座對準選取如下15維狀態(tài)
[0048] 由上述推導，系統(tǒng)方程及量測方程分別為，
[0051] 利用式（21)、式（22)設計濾波算法實現(xiàn)對Rodrigues參數(shù)1的估計，進而得到姿 態(tài)陣q,通過式（1)即實現(xiàn)動基座對準；
[0052] 步驟2,二階非線性量測濾波估計算法
[0053] 式（21)描述的系統(tǒng)方程為線性，式（22)描述的量測方程為非線性，但僅是狀態(tài)量 的二階非線性函數(shù)，能用有限階Taylor級數(shù)展開描述，即
[0055] 其中，Xk(l為Taylor級數(shù)展開點；
ei是第i個分量為1，其余元素為 0的3維單位向量；Hk為非線性函數(shù)h的雅克比陣；Di為非線性函數(shù)h的二階偏導數(shù)陣；Tr為矩陣求跡函數(shù)，且有
[0059] 其中，h=IXh2h3]T;
[0060] 同時，對于二階非線性函數(shù)，其二階偏導數(shù)陣為常值矩陣，故由式（21)、（22)、 (25)，知DiS15維常值對稱陣，其中非零元素僅有
[0061] Di(2,6) = 1，D2(l，6) = -1，D3(l，5) = 1，
[0062] Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -1
[0063] Di(6,2) = 1，D2(6,1) =-1，D3(5,1) = 1，
[0064] Di(5,3) =-1，D2(4,3) = 1，D3(4,2) =-1 (26)
[0065] 步驟2. 1，濾波時間更新算法
[0066] 式（21)系統(tǒng)方程為線性，采用標準卡爾曼濾波算法完成時間更新，得到狀態(tài)量和 估計誤差方差陣的一步預測，即和用狀態(tài)一步預測結(jié)果;代替式（23)中Xk(l， 建立起當前觀測量與狀態(tài)一步預測關系式，進而設計量測更新算法對一步預測結(jié)果 進行校正，得到當前時刻的狀態(tài)最優(yōu)估計值下面推導基于式（23)二階泰勒級數(shù)量測方 ? 程的濾波量測更新算法；
[0067] 步驟2. 2,濾波量測更新算法
[0068] 量測更新形式定義為與線性卡爾曼濾波量測更新一致，假設k時刻狀態(tài)估計結(jié)果 為
[0070] 其中，Lk為引入的補償項，和最佳增益Kk一樣均為待定值；Lk和Kk的確定原則分 別為使為無偏估計和使的均方誤差陣Pk的跡最??；
[0071] 定義狀態(tài)估計誤差
[0074]由式（23)、式（24)、式（27)、式（28)整理得
[0076] 要使&為無偏估計，即要求&期望為零；假設時間更新為無偏估計，則對式 (29)右端取期望并令結(jié)果為零，得補償項Lk為
[0078] 其中，E[ ?]表示對括號內(nèi)變量求期望；
[0079] 將式（30)代入式（29)中，整理得
[0081]其中，
[0083]由于E[足]=〇，E[wv]= 0,且^與A不相關，故由式（31)、式（32)得Pk為
[0085] 其中
[0086]Ak=E[AAt] (34)
[0087] 式（32)中，A為3維列向量，從而Ak為3階方陣；利用式（32)，經(jīng)過推導得Ak第 i行第j列元素為
[0089] 式（33)中協(xié)方差陣更新方式與標準卡爾曼濾波形式一致，從而式（27)中最佳增 益陣Kk，
[0090] 考察量測更新方程式（27)、（33)、（36)，基于二階泰勒級數(shù)的量測更新算法與標準 卡爾曼濾波在形式上完全一致，僅增加了對Lk、Ak的計算；而由式（30)、（35)知，Lk、Ak的 求解簡單；考慮到DiS常值稀疏矩陣，將Lk、Ak描述為僅與Pk/H相關的形式，如此進一步 減小在線計算量；
[0091] 步驟3,經(jīng)典Rodrigues參數(shù)奇異點及其處理方法
[0092] 經(jīng)典Rodrigues參數(shù)是最少參數(shù)姿態(tài)描述方法之一，存在奇異點，一種等價表示 方法為
[0094] 其中，u為兩坐標系之