一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法
【專利摘要】本發(fā)明公開了一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法。包括以下幾個步驟：采集陀螺與星敏感器的輸出數(shù)據(jù)；確定衛(wèi)星姿態(tài)估計系統(tǒng)的狀態(tài)變量和量測量；在k時刻進行標(biāo)準(zhǔn)的容積卡爾曼率波時間更新和量測更新，得到一步狀態(tài)預(yù)測方差、一步量測預(yù)測方差及互協(xié)方差；利用多重次漸消因子對一步狀態(tài)預(yù)測方差進行校正；重新進行容積卡爾曼濾波量測更新，求得k+1時刻的狀態(tài)估計和狀態(tài)估計方差；姿態(tài)估計非線性離散系統(tǒng)的結(jié)束時刻為M，若k+1＝M，則輸出k+1時刻的狀態(tài)估計的姿態(tài)四元數(shù)及陀螺漂移，完成姿態(tài)估計，若k+1＜M，令k＝k+1則重復(fù)步驟三至步驟五。本發(fā)明具有高估計精度和強魯棒性的優(yōu)點。
【專利說明】一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于衛(wèi)星姿態(tài)估計的【技術(shù)領(lǐng)域】，特別涉及一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法。
【背景技術(shù)】
[0002]衛(wèi)星姿態(tài)估計技術(shù)是航天技術(shù)的關(guān)鍵技術(shù)之一，由陀螺與星敏感器組成的衛(wèi)星姿態(tài)估計系統(tǒng)由于測姿精度高、可靠性好以及自主性強等優(yōu)點得到了廣泛的應(yīng)用。針對該姿態(tài)估計系統(tǒng)，四元數(shù)由于計算簡單，無三角函數(shù)的運算，同時又能避免歐拉角的奇異性問題，因此被作為系統(tǒng)的姿態(tài)描述參數(shù)。為提高姿態(tài)估計的精度以及姿態(tài)估計系統(tǒng)的適應(yīng)能力和魯棒性，非線性濾波算法提供了強有力的基礎(chǔ)保障。擴展卡爾曼濾波(ExtendedKalman Filter, EKF)由于其結(jié)構(gòu)簡單、易于實現(xiàn)等優(yōu)點被廣泛應(yīng)用于姿態(tài)估計中。但是，EKF存在理論上的局限性:1)模型線性化引入了截斷誤差，導(dǎo)致濾波精度下降，同時要計算雅克比矩陣，計算復(fù)雜；2)模型失配、未知干擾或狀態(tài)突變等情況下，魯棒性差；3)四元數(shù)作為狀態(tài)變量存在范數(shù)約束，影響濾波精度。
[0003]為了克服EKF算法的局限性，無跡卡爾曼濾波算法(Unscented KalmanFilter, UKF)被提出，該濾波算法的核心思想是利用UT變換(UnscentedTransformation,UT)產(chǎn)生一組確定性的Sigma點來近似非線性函數(shù)的后驗均值和方差，精度能夠達到二階。隨著非線性濾波算法的深入研究，在UKF算法的基礎(chǔ)上，Ienkaran Arasaratnam和SimonHaykin在2009年提出了一種新的非線性濾波一容積卡爾曼濾波算法(Cubature Kalmanfilter, CKF)。CKF算法也是基于最優(yōu)的高斯濾波框架，采用三階球面相徑容積規(guī)則來近似非線性函數(shù)的均值和方差，可以保證在理論上以三階多項式逼近任何非線性高斯?fàn)顟B(tài)的后驗均值和方差，相比較于UKF，具有實現(xiàn)簡單，在高維情況下濾波精度高，收斂性好等優(yōu)點。與EKF類似，CKF算法不足也在于在狀態(tài)突變或模型不準(zhǔn)確的情況下魯棒性差，跟蹤能力差，同時也沒有考慮狀態(tài)變量存在約束的情況。為了提高濾波算法在模型參數(shù)變化或系統(tǒng)發(fā)生突變時的跟蹤能力，一種強跟蹤擴展卡爾曼濾波(StrongTrackingEKF)被提出。此后,有學(xué)者將強跟蹤思想與CKF算法結(jié)合，提出了強跟蹤CKF(STCKF)算法，但該算法只是通過引入單重次漸消因子對預(yù)測誤差協(xié)方差陣進行調(diào)整，雖然具有好的跟蹤能力，但是對復(fù)雜的多變量系統(tǒng)，無法保證對每個變量都具有好的跟蹤能力，同時也沒有考慮狀態(tài)變量存在約束的情況。

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

[0004]本發(fā)明的目的是提供具有高估計精度和強魯棒性的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法。
[0005]本發(fā)明是通過以下技術(shù)方案實現(xiàn)的:
[0006]一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，包括以下幾個步驟:
[0007]步驟一:采集陀螺與星敏感器的輸出數(shù)據(jù)；
[0008]步驟二:利用輸出數(shù)據(jù)確定衛(wèi)星姿態(tài)估計系統(tǒng)的狀態(tài)變量和量測量；
[0009]步驟三:在k時刻進行標(biāo)準(zhǔn)的容積卡爾曼率波時間更新和量測更新，得到一步狀態(tài)預(yù)測方差、一步量測預(yù)測方差及互協(xié)方差；
[0010]步驟四:利用多重次漸消因子對一步狀態(tài)預(yù)測方差進行校正；
[0011]步驟五:利用矯正后的一步狀態(tài)預(yù)測方差，重新進行容積卡爾曼濾波量測更新，求得k+1時刻的狀態(tài)估計和狀態(tài)估計方差；
[0012]步驟六:姿態(tài)估計非線性離散系統(tǒng)的結(jié)束時刻為M，若k+1 = M，則輸出k+Ι時刻的狀態(tài)估計的姿態(tài)四元數(shù)及陀螺漂移，完成姿態(tài)估計，若k+l〈M，令k = k+Ι則重復(fù)步驟三至步驟五。
[0013]本發(fā)明一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法還可以包括:
[0014]1、狀態(tài)變量％ =[fj fijJ , qk為姿態(tài)四元數(shù)，Pk為陀螺漂移，
[0015]建立衛(wèi)星姿態(tài)估計系統(tǒng)的狀態(tài)方程:
[0016]Xm = f{xk 為)+ Wi
[0017]其中:
【權(quán)利要求】
1.一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于，包括以下幾個步驟: 步驟一:采集陀螺與星敏感器的輸出數(shù)據(jù)； 步驟二:利用輸出數(shù)據(jù)確定衛(wèi)星姿態(tài)估計系統(tǒng)的狀態(tài)變量和量測量； 步驟三:在k時刻進行標(biāo)準(zhǔn)的容積卡爾曼率波時間更新和量測更新，得到一步狀態(tài)預(yù)測方差、一步量測預(yù)測方差及互協(xié)方差； 步驟四:利用多重次漸消因子對一步狀態(tài)預(yù)測方差進行校正； 步驟五:利用矯正后的一步狀態(tài)預(yù)測方差，重新進行容積卡爾曼濾波量測更新，求得k+Ι時刻的狀態(tài)估計和狀態(tài)估計方差； 步驟六:姿態(tài)估計非線性離散系統(tǒng)的結(jié)束時刻為M，若k+1 =M，則輸出k+1時刻的狀態(tài)估計的姿態(tài)四元數(shù)及陀螺漂移，完成姿態(tài)估計，若k+l〈M，令k = k+Ι則重復(fù)步驟三至步驟五。
2.根據(jù)權(quán)利要求1所述的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于:所述的狀態(tài)變量；=^^ β?}\ qk為姿態(tài)四元數(shù)，β,為陀螺漂移， 建立衛(wèi)星姿態(tài)估計系統(tǒng)的狀態(tài)方程:
3.根據(jù)權(quán)利要求2所述的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于:所述的得到一步狀態(tài)預(yù)測方差、一步量測預(yù)測方差及互協(xié)方差的方法為: 步驟3.1:時間更新，求得一步狀態(tài)預(yù)測方差； 由k時刻的狀態(tài)估計1_和狀態(tài)估計方差Pk|k，求取容積點為:
4.根據(jù)權(quán)利要求3所述的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于:所述的多重次漸消因子滿足以下方程:

5.根據(jù)權(quán)利要求4所述的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于:所述的求得k+Ι時刻的狀態(tài)估計和狀態(tài)估計方差的方法為: 步驟5.1:利用矯正后的一步狀態(tài)預(yù)測方差進行量測更新； 更新后的一步量測預(yù)測、更新后的一步量測預(yù)測方差及更新后的互協(xié)方差為:
6.根據(jù)權(quán)利要求5所述的一種用于衛(wèi)星姿態(tài)估計的范數(shù)約束強跟蹤容積卡爾曼濾波方法，其特征在于:所述的陀螺測量噪聲為σν = 0.35° /h，陀螺漂移的兩側(cè)噪聲為σ; =0.021/#?星敏感器測量噪聲為Os= 18"，初始陀螺漂移β = [I I l]To /h，初始狀態(tài)估計值為
【文檔編號】G01C21/24GK104019817SQ201410234807
【公開日】2014年9月3日 申請日期:2014年5月30日 優(yōu)先權(quán)日:2014年5月30日
【發(fā)明者】錢華明, 黃蔚, 沈忱 申請人:哈爾濱工程大學(xué)