本發(fā)明涉及屬于航空航天系統(tǒng)處理中導(dǎo)航制導(dǎo)與控制的技術(shù)領(lǐng)域，具體涉及一種基于Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波方法，實(shí)現(xiàn)慣性導(dǎo)航系統(tǒng)非線性系統(tǒng)誤差模型狀態(tài)參數(shù)最優(yōu)濾波計(jì)算，可應(yīng)用于慣性導(dǎo)航系統(tǒng)。

背景技術(shù)：

傳統(tǒng)的隨機(jī)貝葉斯濾波方法一般要求已知過程噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性，或者假設(shè)其滿足一定的分布條件，而實(shí)際的非線性系統(tǒng)中系統(tǒng)狀態(tài)或者參數(shù)的統(tǒng)計(jì)特性往往是未知的，從而導(dǎo)致其概率分布假設(shè)很難得到滿足，尤其是在噪聲本身為非高斯、非白噪聲或者有偏噪聲的情形。另外，實(shí)際系統(tǒng)的非線性特征也會(huì)嚴(yán)重降低貝葉斯濾波方法的計(jì)算效能，因此，常規(guī)的隨機(jī)貝葉斯濾波算法應(yīng)用有很大的局限性。但是，集員濾波算法僅僅要求噪聲的有界性，不需要精確獲得噪聲的統(tǒng)計(jì)特性，這一點(diǎn)在實(shí)際系統(tǒng)中通常是能夠得到保證的，并且在集員濾波的計(jì)算框架下得到的狀態(tài)參數(shù)估計(jì)結(jié)果是一個(gè)可行解集合，而不是常規(guī)濾波計(jì)算獲得的單個(gè)估計(jì)點(diǎn)值。從控制角度來說，集員濾波方法提供了魯棒控制和最優(yōu)控制等理論所要求的狀態(tài)參數(shù)邊界，可更好地實(shí)現(xiàn)濾波方法與控制策略結(jié)合。

實(shí)際上，非線性系統(tǒng)狀態(tài)參數(shù)可行集合形狀一般無法準(zhǔn)確確定，甚至是非凸的，集員濾波方法常用的形狀描述方法有橢球、區(qū)間、超平形體以及全對(duì)稱多胞形等滿足一定規(guī)則的幾何集合來近似實(shí)際可行集，達(dá)到降低算法計(jì)算復(fù)雜度的目的。其中，橢球法具有仿射變換不變性，包絡(luò)矩陣協(xié)方差意義以及便于優(yōu)化計(jì)算等優(yōu)點(diǎn)獲得廣泛應(yīng)用。Schweppe和Bertsekas首先提出了可以利用外定界橢球集合來包含系統(tǒng)的真實(shí)狀態(tài)，但是沒有考慮橢球的最優(yōu)化問題。在此基礎(chǔ)上，F(xiàn)ogel和Huang給出了最優(yōu)化定界橢球算法，得到了最小體積和最小跡橢球集合；Maksarov、Kurzhanski和Chernousko等人進(jìn)一步發(fā)展了針對(duì)狀態(tài)和參數(shù)估計(jì)計(jì)算的橢球計(jì)算技術(shù)；并且Lin針對(duì)特定應(yīng)用情況提出了一種自適應(yīng)的邊界估計(jì)計(jì)算的橢球算法；Polyak推倒了用于具有模型不確定性系統(tǒng)的橢球算法，進(jìn)一步擴(kuò)展了橢球集員濾波算法的應(yīng)用領(lǐng)域。

但是，上述這些算法都是應(yīng)用于線性系統(tǒng)的，Scholte和Campell將橢球定界算法推廣到非線性系統(tǒng)提出了一種擴(kuò)展集員濾波算法，其主要思想是首先對(duì)非線性系統(tǒng)線性化處理，并采用區(qū)間分析技術(shù)估計(jì)線性化近似后的高階項(xiàng)誤差范圍，將其用橢球外包后與噪聲橢球集合實(shí)施直和計(jì)算組成虛擬噪聲橢球集合，然后對(duì)得到的線性化系統(tǒng)實(shí)施線性橢球集員濾波計(jì)算，最終得到非線性系統(tǒng)狀態(tài)參數(shù)的估計(jì)計(jì)算結(jié)果。

但是，基于Taylor級(jí)數(shù)線性化處理得到的擴(kuò)展集員濾波算法存在著很大的缺陷，首先當(dāng)系統(tǒng)非線性比較強(qiáng)時(shí)，圍繞系統(tǒng)狀態(tài)參數(shù)預(yù)測(cè)估計(jì)或者狀態(tài)參數(shù)預(yù)估值的一階Taylor級(jí)數(shù)展開式往往存在著很大的截?cái)嗾`差，使得該算法存在著數(shù)值計(jì)算穩(wěn)定性變差、計(jì)算復(fù)雜，甚至出現(xiàn)濾波算法發(fā)散的現(xiàn)象。再者一階Taylor級(jí)數(shù)擴(kuò)展需要計(jì)算Jacobi矩陣，二階Taylor級(jí)數(shù)擴(kuò)展需要計(jì)算復(fù)雜的Hessian矩陣，計(jì)算量巨大，對(duì)處理器要求很高，難以滿足導(dǎo)航系統(tǒng)快速初始對(duì)準(zhǔn)的要求。

技術(shù)實(shí)現(xiàn)要素：

為了解決現(xiàn)有技術(shù)在非線性捷聯(lián)摜導(dǎo)系統(tǒng)(Strapdown inertial Navigation System，SINS)初始對(duì)準(zhǔn)狀態(tài)參數(shù)計(jì)算過程中，基于Taylor級(jí)數(shù)線性近似的擴(kuò)展橢球集員濾波方法的計(jì)算復(fù)雜、效率低下、計(jì)算精度不能滿足系統(tǒng)要求的技術(shù)問題，本發(fā)明提出一種基于Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波方法，充分利用了Chebyshev正交多項(xiàng)式的特性，有效地減小了計(jì)算量，提高了系統(tǒng)狀態(tài)參數(shù)估計(jì)的計(jì)算效率，且能夠有效地改善擴(kuò)展集員濾波方法的計(jì)算精度。

本發(fā)明的技術(shù)方案是：一種基于Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波方法，其步驟如下：

步驟一：建立組合導(dǎo)航系統(tǒng)非線性誤差的狀態(tài)方程和觀測(cè)方程；

步驟二：根據(jù)第k-1步迭代計(jì)算獲得系統(tǒng)狀態(tài)變量的均值和方差，確定第k-1步組合導(dǎo)航系統(tǒng)狀態(tài)參數(shù)向量的狀態(tài)分量的不確定區(qū)間，其中k＝1,2,···；

步驟三：基于Chebyshev多項(xiàng)式插值表達(dá)式對(duì)組合導(dǎo)航系統(tǒng)非線性誤差的狀態(tài)方程和觀測(cè)方程實(shí)施Chebyshev多項(xiàng)式插值逼近計(jì)算處理，確定Lagrange余子的取值區(qū)間；

步驟四：計(jì)算Chebyshev插值逼近的線性化誤差邊界，利用橢球?qū)⒕€性化誤差外包得到非線性誤差的狀態(tài)方程和觀測(cè)方程的線性化誤差的外包橢球；

步驟五：計(jì)算虛擬過程的誤差橢球，包括Chebyshev多項(xiàng)式逼近的不確定性誤差和過程噪聲相加的兩個(gè)橢球直和運(yùn)算；

步驟六：利用線性化橢球集員濾波算法的預(yù)測(cè)步驟計(jì)算預(yù)測(cè)狀態(tài)橢球邊界，包括線性化預(yù)測(cè)橢球和虛擬過程噪聲橢球的直和計(jì)算；

步驟七：利用線性橢球集員濾波算法的更新步驟更新狀態(tài)橢球邊界，包括預(yù)測(cè)狀態(tài)橢球和觀測(cè)向量集合的交集計(jì)算；

步驟八：利用線性橢球集員濾波算法的狀態(tài)估計(jì)計(jì)算步驟完成系統(tǒng)狀態(tài)變量k時(shí)刻的估計(jì)計(jì)算和估計(jì)方差矩陣計(jì)算，從而完成組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)參數(shù)的估計(jì)計(jì)算任務(wù)。

所述組合導(dǎo)航系統(tǒng)非線性誤差的狀態(tài)方程和觀測(cè)方程為：

其中，x_k∈Rⁿ表示k時(shí)刻的狀態(tài)變量，z_k∈R^m表示k時(shí)刻的觀測(cè)向量，f(·)和h(·)是已知的非線性二階可導(dǎo)函數(shù)，w_k∈Rⁿ表示過程噪聲，v_k∈R^m表示觀測(cè)噪聲，且|w_i,k|≤1,i＝1,2,…,n，|v_j,k|≤1,j＝1,2,…,m，記w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k表示第k步的系統(tǒng)狀態(tài)噪聲包絡(luò)矩陣，R_k表示第k步的系統(tǒng)觀測(cè)噪聲包絡(luò)矩陣；n表示系統(tǒng)狀態(tài)變量的維數(shù)，m表示觀測(cè)變量的維數(shù)；

系統(tǒng)初始狀態(tài)x₀∈X₀，X₀為系統(tǒng)狀態(tài)的先驗(yàn)知識(shí)確定的有界集合，對(duì)于給定的測(cè)量序列向量那么第k步橢球集員濾波算法的狀態(tài)可行集合為X_k；定義橢球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示橢球集合的中心，P為滿足正定性的橢球包絡(luò)矩陣；系統(tǒng)初始狀態(tài)x₀估計(jì)的橢球集合為那么第k-1步估計(jì)得到的系統(tǒng)狀態(tài)橢球集合為其中，P₀表示初始系統(tǒng)狀態(tài)變量的橢球包絡(luò)矩陣，P_k-1表示狀態(tài)變量第k-1步的橢球包絡(luò)矩陣；

所述第k-1步組合導(dǎo)航系統(tǒng)狀態(tài)參數(shù)向量的狀態(tài)分量的不確定區(qū)間為：

其中，i＝1,2,…,n，表示橢球包絡(luò)矩陣P_k的第(i,i)元素，表示第k-1步的狀態(tài)變量的估計(jì)值，l是一個(gè)正實(shí)數(shù)，且l≥3。

所述確定Lagrange余子的取值區(qū)間的方法是：利用Chebyshev多項(xiàng)式逼近分別表達(dá)系統(tǒng)非線性的狀態(tài)方程和觀測(cè)方程，利用在逼近過程中產(chǎn)生的逼近誤差獲得Lagrange余子的最大區(qū)間：

根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)非線性誤差的狀態(tài)方程x_k＝f(x_k-1)+w_k-1，利用Chebyshev插值多項(xiàng)式獲得線性化逼近生成的Lagrange余子的極小化區(qū)間，以第k-1步狀態(tài)變量的估計(jì)點(diǎn)作為Chebyshev插值多項(xiàng)式逼近系統(tǒng)狀態(tài)方程的n階Chebyshev插值表達(dá)式獲得第k步狀態(tài)變量的預(yù)測(cè)點(diǎn)

其中，表示第i項(xiàng)的Chebyshev多項(xiàng)式，A_i表示Chebyshev多項(xiàng)式的系數(shù)，表示Chebyshev多項(xiàng)式逼近的插值余項(xiàng)。

當(dāng)系統(tǒng)狀態(tài)變量區(qū)間時(shí)，插值余項(xiàng)為Chebyshev插值多項(xiàng)式高階項(xiàng)，其表達(dá)式為：

根據(jù)Chebyshev插值多項(xiàng)式的性質(zhì)，當(dāng)插值節(jié)點(diǎn)取Chebyshev多項(xiàng)式的零點(diǎn)值時(shí)，插值余項(xiàng)獲得極小值：

若第k-1步系統(tǒng)狀態(tài)變量的取值區(qū)間為獲得極小化的插值余項(xiàng)為：

基于捷聯(lián)慣性組合導(dǎo)航系統(tǒng)非線性誤差的觀測(cè)方程z_k＝h(x_k)+v_k，利用Chebyshev插值多項(xiàng)式獲得插值逼近計(jì)算生成的Lagrange余子的極小化區(qū)間，以第k步狀態(tài)變量的預(yù)測(cè)點(diǎn)作為Chebyshev插值多項(xiàng)式逼近獲得觀測(cè)方程的插值逼近計(jì)算表達(dá)式：

其中，B_i是非線性觀測(cè)方程的Chebyshev多項(xiàng)式系數(shù)，表示基于系統(tǒng)狀態(tài)變量一步預(yù)測(cè)值的Chebyshev多項(xiàng)式，為極小化插值余項(xiàng)算子，且：

所述利用橢球?qū)⒕€性化誤差外包得到非線性誤差的狀態(tài)方程和觀測(cè)方程的線性化誤差的外包橢球的方法是：

利用Chebyshev插值多項(xiàng)式逼近操作獲得插值余項(xiàng)算子作為Lagrange余子，計(jì)算逼近誤差邊界，用橢球形狀將狀態(tài)方程的Chebyshev多項(xiàng)式逼近誤差外包：

獲得狀態(tài)方程逼近誤差的外包橢球?yàn)槠渲?，表示Chebyshev多項(xiàng)式逼近的系統(tǒng)過程模型狀態(tài)方程的不確定性噪聲方差矩陣，表示系統(tǒng)Chebyshev多項(xiàng)式逼近中的不確定性噪聲方差矩陣的對(duì)角元素；

用橢球形狀將觀測(cè)方程的Chebyshev多項(xiàng)式逼近誤差外包：

獲得觀測(cè)方程的線性化誤差的外包橢球?yàn)槠渲校硎綜hebyshev多項(xiàng)式逼近的觀測(cè)方程不確定性噪聲的方差矩陣，表示Chebyshev多項(xiàng)式逼近中造成的觀測(cè)方程不確定性噪聲方差矩陣的對(duì)角元素。

所述計(jì)算虛擬過程的誤差橢球的方法是：Chebyshev多項(xiàng)式插值逼近造成的不確定性誤差橢球和過程噪聲相加的兩個(gè)橢球直和運(yùn)算；通過逼近不確定性誤差和過程噪聲的直和計(jì)算獲得虛擬噪聲誤差橢球；

對(duì)非線性過程的狀態(tài)方程x_k＝f(x_k-1)+w_k-1計(jì)算虛擬過程的狀態(tài)噪聲誤差橢球?yàn)?/p>

其中，Q_k-1表示第k-1步的系統(tǒng)狀態(tài)噪聲包絡(luò)矩陣，是由橢球形狀的系統(tǒng)Chebyshev多項(xiàng)式插值逼近計(jì)算的不確定性誤差和過程噪聲相加獲得的，表示第k-1步的系統(tǒng)噪聲誤差橢球方差矩陣直和，且：

其中，為過程噪聲方差直和計(jì)算的尺度因子，且

對(duì)于非線過程的性觀測(cè)方程z_k＝h(x_k)+v_k計(jì)算虛擬觀測(cè)噪聲誤差橢球

其中，表示獲得的虛擬觀測(cè)噪聲方差矩陣直和，且：

其中，是觀測(cè)噪聲方差矩陣直和計(jì)算的尺度因子參數(shù)，

所述利用線性化橢球集員濾波算法的預(yù)測(cè)步驟計(jì)算預(yù)測(cè)狀態(tài)橢球邊界的方法是：利用第k-1步的系統(tǒng)狀態(tài)變量估計(jì)值和Chebyshev多項(xiàng)式逼近法展開計(jì)算第k步的狀態(tài)預(yù)測(cè)值，獲得狀態(tài)變量線性化預(yù)測(cè)值及其外包預(yù)測(cè)橢球，開展線性化預(yù)測(cè)橢球和虛擬過程噪聲橢球的直和運(yùn)算，獲得系統(tǒng)狀態(tài)變量的預(yù)測(cè)橢球邊界；

在第k-1步獲得系統(tǒng)狀態(tài)變量估計(jì)值利用Chebyshev多項(xiàng)式逼近計(jì)算第k步的系統(tǒng)狀態(tài)變量預(yù)測(cè)值，根據(jù)系統(tǒng)均值計(jì)算公式可有：

上式中設(shè)根據(jù)Chebyshev多項(xiàng)式性質(zhì)可以進(jìn)一步整理E_j項(xiàng)為：

其中，P_x,k-1表示第k-1步系統(tǒng)狀態(tài)變量的橢球包絡(luò)矩陣，Π(·)表示系統(tǒng)狀態(tài)變量的概率分布特征，利用E₀＝1，直至E_n項(xiàng)表達(dá)式獲得線性方程為：

其中，R是一個(gè)(n-1)×(n-1)的矩陣，且其元素滿足

獲得第k-1步的預(yù)測(cè)值表達(dá)式：

其中，是直到n的系統(tǒng)狀態(tài)向量的非中心矩構(gòu)造的向量，且：

表示Chebyshev多項(xiàng)式的系數(shù)構(gòu)造的向量，且：

A_n＝[A₀，A₁，…，A_n]^T，

Π_n是由Chebyshev多項(xiàng)式系數(shù)構(gòu)造的(n+1)×(n+1)的矩陣，且：

П_n＝[α_0,n,α_1,n,…,α_n,n]^T，

組成第i項(xiàng)Chebyshev多項(xiàng)式的所有的直到n次單項(xiàng)式的系數(shù)：

且有遞推表達(dá)公式為：

其起始項(xiàng)α_0,n＝[1,0，…，0]^T和α_1,n＝[0，1,0，…，0]^T，經(jīng)由前面兩項(xiàng)系數(shù)向量可以遞推出直到n的所有系數(shù)向量；對(duì)于系統(tǒng)狀態(tài)變量的方差計(jì)算，可以經(jīng)由方差矩陣的計(jì)算公式獲得：

對(duì)上式中的均值進(jìn)行化簡(jiǎn)得：從而獲得系統(tǒng)狀態(tài)變量的預(yù)測(cè)狀態(tài)橢球

其中，A_k,n表示整理獲得的Chebyshev多項(xiàng)式系數(shù)向量，表示系統(tǒng)狀態(tài)變量在第k-1步的估計(jì)誤差非中心矩向量，⊙表示Kronecker乘積，P_2n是一個(gè)(n+1)²×(2n+1)的矩陣，其表達(dá)式為：β_k-1為尺度因子參數(shù)。

所述利用線性橢球集員濾波算法的更新步驟更新狀態(tài)橢球邊界的方法是：利用系統(tǒng)觀測(cè)向量序列開展預(yù)測(cè)狀態(tài)橢球和觀測(cè)向量集合的交集計(jì)算；

將預(yù)測(cè)狀態(tài)橢球和觀測(cè)向量集合做直和交集計(jì)算，其中觀測(cè)集合為：

采用Chebyshev多項(xiàng)式Kalman濾波算法計(jì)算系統(tǒng)狀態(tài)變量的觀測(cè)更新計(jì)算過程，觀測(cè)向量的一步預(yù)測(cè)計(jì)算為

其中，由系統(tǒng)狀態(tài)變量的直到n的所有的非中心矩組成，是觀測(cè)方程的Chebyshev多項(xiàng)式的系數(shù)組成的向量；

相應(yīng)的觀測(cè)方程的觀測(cè)向量一步預(yù)測(cè)方差可計(jì)算為：

那么系統(tǒng)狀態(tài)變量和觀測(cè)向量的協(xié)方差可計(jì)算為

從而可以獲得

其中，表示系統(tǒng)觀測(cè)變量在第k-1步的預(yù)測(cè)誤差非中心矩向量，表示直到2n階次的系統(tǒng)觀測(cè)變量在第k-1步的預(yù)測(cè)誤差非中心矩向量，是一個(gè)(n+1)×(n+2)的矩陣，其表達(dá)式為

其中，表示系統(tǒng)狀態(tài)變量的取值區(qū)間范圍，若那么

10、根據(jù)權(quán)利要求8所述的基于Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波方法，其特征在于，所述利用線性橢球集員濾波算法的狀態(tài)估計(jì)計(jì)算步驟完成系統(tǒng)狀態(tài)變量k時(shí)刻的估計(jì)計(jì)算和估計(jì)方差矩陣計(jì)算的方法是：

其中，δ_k為算法健康度因子，其表達(dá)式為：表示k時(shí)刻系統(tǒng)狀態(tài)變量估計(jì)誤差包絡(luò)矩陣計(jì)算的中間算子，且：

其中，z_k表示觀測(cè)向量，Π_k,n是在第k步預(yù)測(cè)計(jì)算中由Chebyshev多項(xiàng)式系數(shù)構(gòu)造的(n+1)×(n+1)的矩陣，W_k表示第k步的系統(tǒng)觀測(cè)向量的一步預(yù)測(cè)誤差矩陣，K_k表示濾波算法的增益矩陣，ρ_k為預(yù)測(cè)誤差包絡(luò)矩陣的調(diào)節(jié)尺度因子參數(shù)。

本發(fā)明采用Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波算法對(duì)大方位失準(zhǔn)角初始對(duì)準(zhǔn)非線性誤差模型的狀態(tài)變量參數(shù)的估計(jì)計(jì)算，利用Chebyshev多項(xiàng)式具有極小化的逼近誤差的優(yōu)點(diǎn)，方位失準(zhǔn)角收斂速度很快，估計(jì)誤差獲得快速收斂，并且數(shù)值計(jì)算的穩(wěn)定性較好，沒有出現(xiàn)估計(jì)計(jì)算數(shù)據(jù)發(fā)散現(xiàn)象，計(jì)算效能較好。本發(fā)明采用Chebyshev多項(xiàng)式插值實(shí)施線性化逼近操作，有效避免Taylor級(jí)數(shù)展開式的一階Jacobian矩陣和二階Hessian矩陣的復(fù)雜計(jì)算，降低了算法的計(jì)算復(fù)雜度；相比于Taylor級(jí)數(shù)擴(kuò)展的傳統(tǒng)非線性集員濾波方法，本發(fā)明計(jì)算精度較高，并且有效保證了擴(kuò)展橢球集員濾波算法的計(jì)算穩(wěn)定性。

附圖說明

為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1是本發(fā)明Chebyshev多項(xiàng)式逼近非線性系統(tǒng)函數(shù)的矩信息計(jì)算的流程圖。

圖2是本發(fā)明的流程圖。

圖3是本發(fā)明的艦船載體機(jī)動(dòng)運(yùn)行軌跡圖。

圖4是導(dǎo)航系統(tǒng)姿態(tài)失準(zhǔn)角狀態(tài)估計(jì)的誤差數(shù)據(jù)曲線圖(Chebyshev多項(xiàng)式法)。

圖5是導(dǎo)航系統(tǒng)速度狀態(tài)估計(jì)的誤差數(shù)據(jù)曲線圖(Chebyshev多項(xiàng)式法)。

圖6是導(dǎo)航系統(tǒng)姿態(tài)失準(zhǔn)角狀態(tài)估計(jì)的誤差數(shù)據(jù)曲線圖(Taylor級(jí)數(shù)擴(kuò)展法)。

圖7是導(dǎo)航系統(tǒng)速度狀態(tài)估計(jì)的誤差數(shù)據(jù)曲線圖(Taylor級(jí)數(shù)擴(kuò)展法)。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有付出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

如圖1和2所示，一種基于Chebyshev多項(xiàng)式插值逼近的擴(kuò)展橢球集員濾波方法，首先建立非線性捷聯(lián)慣性導(dǎo)航系統(tǒng)方程和觀測(cè)方程；計(jì)算第k-1步的系統(tǒng)狀態(tài)參數(shù)分量的不確定區(qū)間；基于Chebyshev多項(xiàng)式插值逼近對(duì)接連慣性導(dǎo)航系統(tǒng)的非線性方程和觀測(cè)方程實(shí)施Chebyshev多項(xiàng)式逼近計(jì)算，確定Lagrange余子的取值區(qū)間；接著計(jì)算Chebyshev多項(xiàng)式插值逼近計(jì)算的誤差邊界，獲取非線性系統(tǒng)方程和觀測(cè)方程的Chebyshev多項(xiàng)式插值逼近計(jì)算誤差的外包橢球，從而開展虛擬過程噪聲誤差橢球和虛擬觀測(cè)噪聲橢球計(jì)算；利用線性橢球集員濾波算法的預(yù)測(cè)步驟計(jì)算預(yù)測(cè)狀態(tài)變量的橢球邊界；利用線性橢球集員濾波算法的更新步驟更新計(jì)算狀態(tài)橢球邊界；再利用線性橢球集員濾波算法的狀態(tài)估計(jì)步驟計(jì)算第k步的狀態(tài)參數(shù)變量的估計(jì)值和估計(jì)方差矩陣，從而完成捷聯(lián)慣性導(dǎo)航系統(tǒng)狀態(tài)參數(shù)變量的估計(jì)計(jì)算任務(wù)。本發(fā)明利用Chebeshev多項(xiàng)式逼近計(jì)算實(shí)現(xiàn)非線性的擴(kuò)展橢球集員濾波方法，有效減小了基于Taylor級(jí)數(shù)的擴(kuò)展表達(dá)式計(jì)算的復(fù)雜性和計(jì)算量。

假設(shè)非線性捷聯(lián)慣性導(dǎo)航系統(tǒng)方程為f(x)，其可以被切比雪夫(Chebyshev)多項(xiàng)式和來任意逼近，如

其中，T(x)＝{T_n(x)，n＝0,…,∞}表示包含變量x的切比雪夫多項(xiàng)式的各項(xiàng)，{A_i,i＝0,1,2,…}是切比雪夫多項(xiàng)式系數(shù)，其計(jì)算表達(dá)式為：

且切比雪夫多項(xiàng)式具有正交性，切比雪夫多項(xiàng)式是采用n次的切比雪夫多項(xiàng)式加權(quán)代數(shù)式來逼近任意的非線性函數(shù)，這些加權(quán)多項(xiàng)式滿足正交特性，其正交性可表達(dá)為：

相鄰的三個(gè)切比雪夫多項(xiàng)式具有遞推關(guān)系，可以表達(dá)為：

并且切比雪夫多項(xiàng)式T_n(x)具有奇偶性，滿足：

T_n(-x)＝(-1)ⁿT_n(x)， (5)。

切比雪夫多項(xiàng)式滿足T(x)∈[-1,1]取值區(qū)間，并且在此區(qū)間中T(x)具有n個(gè)不同的實(shí)數(shù)零點(diǎn)，可由這些零點(diǎn)k＝1,2,···,n實(shí)施切比雪夫多項(xiàng)式插值逼近計(jì)算操作。

根據(jù)Chebyshev多項(xiàng)式的奇偶性質(zhì)及取值特性，Chebyshev多項(xiàng)式還可以寫為：

其中，α_n,i表示n次Chebyshev多項(xiàng)式的第i階單項(xiàng)式的系數(shù)，α_n,n-2i也是同樣的意義，表示n次Chebyshev多項(xiàng)式的第n-2i階單項(xiàng)式的系數(shù)，式中[n/2]表示取整數(shù)，從而也可以獲得兩項(xiàng)Chebyshev多項(xiàng)式的乘積表達(dá)式為

同時(shí)，Chebyshev多項(xiàng)式的函數(shù)在區(qū)間[-1,1]上交替出現(xiàn)n+1個(gè)極值點(diǎn)組，其最大值為1，最小值為-1。Chebyshev多項(xiàng)式的最高次項(xiàng)系數(shù)為2^n-1，n＝1,2,···。從而Chebyshev多項(xiàng)式存在著與零偏差最小的特性，并且其偏差為依此特性可以在Chebyshev多項(xiàng)式逼近非線性函數(shù)過程中獲得多項(xiàng)式插值余項(xiàng)的極小值，這有助于有效改善擴(kuò)展集員濾波算法的計(jì)算精度；且其計(jì)算相對(duì)簡(jiǎn)單，計(jì)算效率可以得到有效的提高，能夠滿足SINS導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的快速計(jì)算要求，完成對(duì)艦船SINS導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)大方位失準(zhǔn)角模型狀態(tài)參數(shù)的估計(jì)計(jì)算任務(wù)。

在實(shí)際應(yīng)用過程中，一般取有限項(xiàng)Chebyshev多項(xiàng)式插值逼近非線性系統(tǒng)函數(shù)，式(1)可以表達(dá)為有限項(xiàng)的Chebyshev多項(xiàng)式逼近為：

其中，R_n+1(x)表示非線性系統(tǒng)函數(shù)的Chebyshev多項(xiàng)式插值逼近計(jì)算的余項(xiàng)；其Chebyshev多項(xiàng)式系數(shù)表達(dá)式(2)可寫為：

且滿足收斂性質(zhì)為了后面便于利用Chebyshev多項(xiàng)式展開系統(tǒng)狀態(tài)變量的矩信息的插值逼近計(jì)算，在n+1個(gè)插值點(diǎn)上我們對(duì)Chebyshev多項(xiàng)式的正交性表達(dá)式(5)整理為：

另外應(yīng)該說明的是為了計(jì)算方便性，Chebyshev多項(xiàng)式插值逼近表達(dá)式中Chebyshev多項(xiàng)式系數(shù)在插值點(diǎn)上的計(jì)算表達(dá)式為：

其中，δ_0,n表示Kroneckerδ函數(shù)，滿足

還要說明的是一般情況下系統(tǒng)狀態(tài)變量的取值區(qū)間不在[-1,1]區(qū)間，這時(shí)需要做系統(tǒng)狀態(tài)變量的變量替換，若系統(tǒng)狀態(tài)變量取值區(qū)間為[a,b]，一般可采用變量替換表達(dá)式：

那么，相應(yīng)的系統(tǒng)變量x'∈[-1,1]，則系統(tǒng)替換變量的均值和方差分別可變換為：

其中，E[x]表示原變量x的均值，P^x表示原變量x的方差。系統(tǒng)狀態(tài)變量經(jīng)由變量替換后參與迭代計(jì)算，獲得替換變量的矩信息計(jì)算結(jié)果后，替換變量再轉(zhuǎn)換到原系統(tǒng)變量，其變換公式為

那么相應(yīng)的插值節(jié)點(diǎn)變換為

由此我們可以開展Chebyshev多項(xiàng)式插值逼近非線性函數(shù)的計(jì)算工作。

下面詳細(xì)闡述一下本發(fā)明的具體內(nèi)容。

步驟一：建立組合導(dǎo)航系統(tǒng)非線性誤差狀態(tài)方程和觀測(cè)方程。

設(shè)計(jì)一種捷聯(lián)慣性導(dǎo)航非線性系統(tǒng)狀態(tài)方程和觀測(cè)方程，如后面的應(yīng)用實(shí)例所述，其可以總結(jié)為

其中，x_k∈Rⁿ表示系統(tǒng)第k步的狀態(tài)向量和z_k∈R^m表示系統(tǒng)第k步的觀測(cè)向量，這里說明系統(tǒng)狀態(tài)變量x的下標(biāo)k表示迭代的第k步，本發(fā)明申請(qǐng)書的下標(biāo)表示都一致。f(·)為系統(tǒng)狀態(tài)方程的函數(shù)，h(·)為系統(tǒng)觀測(cè)方程的函數(shù)，f(·)和h(·)是已知的系統(tǒng)非線性可導(dǎo)函數(shù)。w_k∈Rⁿ表示第k步的過程噪聲，v_k∈R^m表示第k步的觀測(cè)噪聲，其隨時(shí)間變化，且滿足未知但有界(UBB)的假設(shè)條件。記w_k∈(0,Q_k)和v_k∈(0,R_k)，Q_k表示第k步的系統(tǒng)狀態(tài)噪聲包絡(luò)矩陣，R_k表示第k步的系統(tǒng)觀測(cè)噪聲包絡(luò)矩陣為第k步的系統(tǒng)狀態(tài)噪聲誤差矩陣，Q_k表示第k步的系統(tǒng)狀態(tài)噪聲包絡(luò)矩陣，R_k表示第k步的系統(tǒng)觀測(cè)噪聲包絡(luò)矩陣；n表示系統(tǒng)狀態(tài)變量的維數(shù)，m表示觀測(cè)變量的維數(shù)；且過程噪聲滿足|w_k,i|≤1,i＝1,2,…,n，觀測(cè)噪聲滿足|v_k,j|≤1,j＝1,2,…,m；且n表示系統(tǒng)狀態(tài)變量的維數(shù)，m表示觀測(cè)變量的維數(shù)。捷聯(lián)導(dǎo)航系統(tǒng)狀態(tài)變量的初始狀態(tài)x₀屬于一個(gè)已知的有界集合X₀，即x₀∈X₀，該集合可以由系統(tǒng)狀態(tài)的先驗(yàn)知識(shí)確定，對(duì)于給定的觀測(cè)向量序列第k步的橢球集員濾波估計(jì)計(jì)算的狀態(tài)可行集合為{X_k}。狀態(tài)可行集合{X_k}由所有可能的狀態(tài)點(diǎn)組成，這些狀態(tài)點(diǎn)與所有可獲取的信息，包括系統(tǒng)模型、噪聲假設(shè)和初始狀態(tài)集合相一致。

定義橢球集合E(a,P)＝{x∈Rⁿ|(x-a)^TP^-1(x-a)≤1}，其中，a表示橢球集合的中心，P為滿足正定性的橢球包絡(luò)矩陣。定義初始系統(tǒng)狀態(tài)估計(jì)橢球集合為假設(shè)經(jīng)由k-1的濾波估計(jì)計(jì)算，第k-1步獲得的系統(tǒng)狀態(tài)向量的可行橢球集合為則k時(shí)刻非線性橢球集員濾波算法的迭代計(jì)算過程由步驟二至步驟八組成。

步驟二：據(jù)第k-1步迭代計(jì)算獲得系統(tǒng)狀態(tài)變量的均值和方差，確定第k-1步組合導(dǎo)航系統(tǒng)狀態(tài)參數(shù)向量的狀態(tài)分量的不確定區(qū)間。

根據(jù)第k-1步的系統(tǒng)狀態(tài)向量的估計(jì)值和估計(jì)方差矩陣來確定當(dāng)前時(shí)刻系統(tǒng)狀態(tài)變量的不確定區(qū)間，第k-1步系統(tǒng)狀態(tài)參數(shù)向量的狀態(tài)分量的不確定區(qū)間為：其中i＝1,2,…,n，表示第k-1步橢球包絡(luò)矩陣P_k-1的(i，i)元素，表示第k-1步的狀態(tài)變量的估計(jì)值，l是一個(gè)正實(shí)數(shù)，它設(shè)置意義是保證第k-1步的系統(tǒng)狀態(tài)參數(shù)估計(jì)值有99.7％的概率落在設(shè)定的狀態(tài)變量取值區(qū)間之內(nèi)，其一般取值為l≥3。

步驟三：基于Chebyshev多項(xiàng)式插值逼近法對(duì)組合導(dǎo)航系統(tǒng)非線性誤差的狀態(tài)方程和觀測(cè)方程實(shí)施Chebyshev多項(xiàng)式逼近處理，確定Lagrange余子的取值區(qū)間。

以當(dāng)前第k-1步的系統(tǒng)狀態(tài)變量估計(jì)值實(shí)施Chebyshev插值多項(xiàng)式擴(kuò)展，取Chebyshev多項(xiàng)式插值誤差，或者稱之為插值余項(xiàng)，Lagrange余子項(xiàng)作為非線性系統(tǒng)方程狀態(tài)變量的不確定區(qū)間。利用Chebyshev多項(xiàng)式逼近表達(dá)系統(tǒng)非線性狀態(tài)方程，在逼近過程中產(chǎn)生逼近誤差，確定利用Chebyshev多項(xiàng)式插值逼近獲得的Lagrange余子的最大區(qū)間，以第k-1步狀態(tài)估計(jì)點(diǎn)做Chebyshev插值多項(xiàng)式獲得系統(tǒng)過程函數(shù)的逼近表達(dá)式。

根據(jù)捷聯(lián)慣性導(dǎo)航系統(tǒng)的非線性誤差狀態(tài)方程x_k＝f(x_k-1)+w_k-1，根據(jù)Chebyshev多項(xiàng)式的插值余項(xiàng)極小化性質(zhì)，利用Chebyshev插值多項(xiàng)式獲得線性化逼近生成的Lagrange余子的極小化區(qū)間，以第k-1步狀態(tài)變量的估計(jì)點(diǎn)作為Chebyshev插值多項(xiàng)式逼近系統(tǒng)狀態(tài)方程的n階Chebyshev插值表達(dá)式獲得第k步狀態(tài)變量的預(yù)測(cè)點(diǎn)

其中，表示第i項(xiàng)的Chebyshev多項(xiàng)式，A_i表示Chebyshev多項(xiàng)式系數(shù)，表示Chebyshev多項(xiàng)式逼近余項(xiàng)。。上式表示，當(dāng)系統(tǒng)狀態(tài)變量x_k-1∈[-1,1]區(qū)間時(shí)，Chebyshev插值多項(xiàng)式的前n項(xiàng)，其余高階項(xiàng)統(tǒng)一定義為插值余項(xiàng)其表達(dá)式為：

根據(jù)Chebyshev插值多項(xiàng)式的性質(zhì)，當(dāng)插值節(jié)點(diǎn)取Chebyshev多項(xiàng)式的零點(diǎn)值時(shí)，插值余項(xiàng)獲得極小值，也就是：

若第k-1步系統(tǒng)狀態(tài)變量的取值區(qū)間為x_k-1∈[a_k-1,b_k-1]，經(jīng)由式(12)變換可以獲得極小化的插值逼近誤差余項(xiàng)為：

Chebyshev插值多項(xiàng)式的這一性質(zhì)對(duì)于提高和改善插值多項(xiàng)式逼近非線性系統(tǒng)函數(shù)的計(jì)算精度具有重要意義。

同樣地，基于捷聯(lián)慣性組合導(dǎo)航系統(tǒng)非線性誤差的觀測(cè)方程z_k＝h(x_k)+v_k，根據(jù)Chebyshev插值多項(xiàng)式性質(zhì)，利用Chebyshev插值多項(xiàng)式獲得插值逼近計(jì)算生成的Lagrange余子的極小化區(qū)間，以第k步狀態(tài)變量的預(yù)測(cè)點(diǎn)作為Chebyshev插值多項(xiàng)式逼近獲得觀測(cè)方程的插值逼近計(jì)算表達(dá)式：

其中，B_i是非線性觀測(cè)方程的Chebyshev多項(xiàng)式系數(shù)，表示基于系統(tǒng)狀態(tài)變量一步預(yù)測(cè)值的Chebyshev多項(xiàng)式，為極小化插值余項(xiàng)算子，且：

步驟四：計(jì)算Chebyshev插值逼近的線性化誤差邊界，利用橢球?qū)⒕€性化誤差外包得到非線性誤差的狀態(tài)方程和觀測(cè)方程的線性化誤差的外包橢球。

利用Chebyshev插值多項(xiàng)式逼近操作獲得插值余項(xiàng)算子作為Lagrange余子，計(jì)算逼近誤差邊界，用橢球形狀將狀態(tài)方程的Chebyshev多項(xiàng)式逼近誤差外包：

獲得狀態(tài)方程逼近誤差的外包橢球?yàn)槠渲?，表示Chebyshev多項(xiàng)式逼近的系統(tǒng)過程模型狀態(tài)方程的不確定性噪聲方差矩陣，表示系統(tǒng)Chebyshev多項(xiàng)式逼近中的不確定性噪聲方差矩陣的對(duì)角元素。

用橢球形狀將觀測(cè)方程的Chebyshev多項(xiàng)式逼近誤差外包：

獲得觀測(cè)方程的線性化誤差的外包橢球?yàn)槠渲校硎綜hebyshev多項(xiàng)式逼近的觀測(cè)方程不確定性噪聲的方差矩陣，表示Chebyshev多項(xiàng)式逼近中造成的觀測(cè)方程不確定性噪聲方差矩陣的對(duì)角元素。

步驟五：計(jì)算虛擬過程誤差橢球，包括虛擬過程狀態(tài)噪聲誤差橢球和虛擬觀測(cè)噪聲橢球。

涉及到Chebyshev多項(xiàng)式插值逼近造成的不確定性誤差橢球和過程噪聲相加的兩個(gè)橢球直和運(yùn)算；通過逼近不確定性誤差和過程噪聲的直和計(jì)算獲得虛擬噪聲誤差橢球。

計(jì)算第k-1步的虛擬過程的狀態(tài)噪聲誤差橢球?yàn)榍?/p>

其中，表示第k-1步的系統(tǒng)噪聲誤差橢球方差矩陣直和，Q_k-1表示第k-1步計(jì)算的系統(tǒng)狀態(tài)噪聲包絡(luò)矩陣，是由橢球形狀的系統(tǒng)Chebyshev多項(xiàng)式插值逼近計(jì)算的不確定性誤差和過程噪聲相加獲得的，涉及到兩個(gè)橢球的直和計(jì)算：

其中，為過程噪聲方差直和計(jì)算的尺度因子，且對(duì)于非線性觀測(cè)方程z_k＝h(x_k)+v_k做上述計(jì)算步驟，計(jì)算虛擬觀測(cè)噪聲誤差橢球

虛擬觀測(cè)噪聲是由橢球的線性化誤差和過程噪聲相加獲得的，其中涉及到兩個(gè)橢球的直和計(jì)算：

其中，是觀測(cè)噪聲方差矩陣直和計(jì)算的尺度因子參數(shù)，從而獲得了得到系統(tǒng)觀測(cè)噪聲的虛擬噪聲橢球其中，表示獲得的虛擬觀測(cè)噪聲方差矩陣直和。

步驟六：利用線性化橢球集員濾波算法的預(yù)測(cè)步驟計(jì)算預(yù)測(cè)狀態(tài)橢球邊界。

涉及到線性化預(yù)測(cè)橢球和虛擬過程噪聲橢球的直和計(jì)算過程。利用第k-1步的系統(tǒng)狀態(tài)變量估計(jì)值和Chebyshev多項(xiàng)式逼近法展開計(jì)算第k步的狀態(tài)預(yù)測(cè)值，獲得狀態(tài)變量線性化預(yù)測(cè)值及其外包預(yù)測(cè)橢球，開展線性化預(yù)測(cè)橢球和虛擬過程噪聲橢球的直和運(yùn)算，獲得系統(tǒng)狀態(tài)變量的預(yù)測(cè)橢球邊界。

線性化預(yù)測(cè)橢球和虛擬過程噪聲直和計(jì)算過程。在第k-1步獲得系統(tǒng)狀態(tài)變量估計(jì)值利用Chebyshev多項(xiàng)式逼近計(jì)算第k步的系統(tǒng)狀態(tài)變量預(yù)測(cè)值，根據(jù)系統(tǒng)均值計(jì)算公式可有：

定義根據(jù)Chebyshev多項(xiàng)式性質(zhì)可以進(jìn)一步整理E_j項(xiàng)為：

其中，P_x,k-1表示第k-1步系統(tǒng)狀態(tài)變量的橢球包絡(luò)矩陣，Π(·)表示系統(tǒng)狀態(tài)變量的概率分布特征。從而可以利用E₀＝1，直至E_n項(xiàng)表達(dá)式獲得一個(gè)線性方程為：

其中，R是一個(gè)(n-1)×(n-1)的矩陣，其元素滿足矩陣R是一個(gè)下三角的稀疏矩陣。由此開展系統(tǒng)預(yù)測(cè)均值的計(jì)算任務(wù)。

那么，利用前面的公式可以很容易地獲得第k-1步的預(yù)測(cè)值表達(dá)式：

其中，是直到n的系統(tǒng)狀態(tài)向量的非中心矩構(gòu)造的向量，定義為：

表示Chebyshev多項(xiàng)式的系數(shù)構(gòu)造的向量，其定義為：

A_n＝[A₀，A₁，…，A_n]^T， (32)

Π_n是由Chebyshev多項(xiàng)式系數(shù)構(gòu)造的(n+1)×(n+1)的矩陣，其定義為：

П_n＝[α_0,n,α_1,n,…,α_n,n]^T，

且有組成第i項(xiàng)Chebyshev多項(xiàng)式的所有的直到n次單項(xiàng)式的系數(shù)，

它還有遞推表達(dá)公式為：

其起始項(xiàng)α_0,n＝[1,0，…，0]^T和α_1,n＝[0，1,0，…，0]^T，經(jīng)由前面兩項(xiàng)系數(shù)向量可以遞推出直到n的所有系數(shù)向量。

對(duì)于系統(tǒng)狀態(tài)變量的方差計(jì)算，可以經(jīng)由方差矩陣的計(jì)算公式獲得：

對(duì)于式(35)中的第二項(xiàng)是系統(tǒng)虛擬噪聲的方差矩陣，下面討論其第一項(xiàng)的計(jì)算方法。

其中可計(jì)算為：

其中，符號(hào)⊙表示Kronecker乘積，P_2n表示一個(gè)(n+1)²×(2n+1)的矩陣，其表達(dá)式為：由的所有乘積項(xiàng)組成的，A_k,n表示Chebyshev多項(xiàng)式系數(shù)構(gòu)造的向量，表示系統(tǒng)狀態(tài)變量在第k-1步的估計(jì)誤差非中心矩向量。同時(shí)引入尺度因子參數(shù)β_k-1，從而可以獲得虛擬過程噪聲預(yù)測(cè)直和計(jì)算方差矩陣為：

從而獲得系統(tǒng)狀態(tài)變量的預(yù)測(cè)狀態(tài)橢球

步驟七：利用線性橢球集員濾波算法的更新步驟更新狀態(tài)橢球邊界。

涉及到預(yù)測(cè)狀態(tài)橢球和觀測(cè)向量集合的交集計(jì)算，利用系統(tǒng)觀測(cè)向量序列開展預(yù)測(cè)狀態(tài)橢球和觀測(cè)向量帶的交集計(jì)算。

將預(yù)測(cè)狀態(tài)橢球和觀測(cè)集合S_y做直和交集計(jì)算，其中觀測(cè)集合S_y為：

下面首先考慮采用Chebyshev多項(xiàng)式Kalman濾波算法計(jì)算系統(tǒng)狀態(tài)變量的觀測(cè)更新計(jì)算過程，觀測(cè)向量的一步預(yù)測(cè)計(jì)算為：

其中，由系統(tǒng)狀態(tài)變量的直到n的所有的非中心矩組成，是觀測(cè)方程的Chebyshev多項(xiàng)式的系數(shù)組成的向量。相應(yīng)的觀測(cè)方程的觀測(cè)向量一步預(yù)測(cè)方差可計(jì)算為：

其中，表示系統(tǒng)狀態(tài)變量的第k步預(yù)測(cè)的直到2n的所有的非中心矩組成向量，那么系統(tǒng)狀態(tài)變量和觀測(cè)向量的協(xié)方差可計(jì)算為：

從而可以獲得

其中，表示系統(tǒng)狀態(tài)變量的第k步預(yù)測(cè)的直到n的所有的非中心矩組成向量，是一個(gè)(n+1)×(n+2)的矩陣，其表達(dá)式為：

其中，表示系統(tǒng)狀態(tài)變量的取值區(qū)間范圍，若那么

其中，z_k表示觀測(cè)向量，Π_k,n是在第k步預(yù)測(cè)計(jì)算中的由Chebyshev多項(xiàng)式系數(shù)構(gòu)造的(n+1)×(n+1)的矩陣，W_k表示第k步的系統(tǒng)觀測(cè)向量的一步預(yù)測(cè)誤差矩陣，K_k表示濾波算法的增益矩陣，ρ_k為預(yù)測(cè)誤差包絡(luò)矩陣的調(diào)節(jié)尺度因子參數(shù)。

步驟八：利用線性橢球集員濾波算法的狀態(tài)估計(jì)步驟完成系統(tǒng)狀態(tài)變量k時(shí)刻的估計(jì)計(jì)算和估計(jì)方差矩陣計(jì)算，從而完成SINS組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)參數(shù)的估計(jì)計(jì)算任務(wù)。

其中，δ_k為算法健康度因子，其表達(dá)式為：

表示k時(shí)刻系統(tǒng)狀態(tài)變量估計(jì)誤差包絡(luò)矩陣計(jì)算的中間算子。

本發(fā)明的優(yōu)勢(shì)在于采用Chebyshev多項(xiàng)式插值實(shí)施線性化逼近操作，有效避免Taylor級(jí)數(shù)展開式的一階Jacobian矩陣和二階Hessian矩陣的復(fù)雜計(jì)算，降低了算法的計(jì)算復(fù)雜度；相比于Taylor級(jí)數(shù)擴(kuò)展的傳統(tǒng)非線性集員濾波算法，本發(fā)明的計(jì)算精度比較高，并且有效保證了擴(kuò)展橢球集員濾波算法的計(jì)算穩(wěn)定性。

在本發(fā)明中，引入了三個(gè)尺度因子參數(shù)β_k-1和ρ_k，其數(shù)值確定方法如下：

尺度因子參數(shù)和β_k-1涉及到兩個(gè)橢球直和運(yùn)算的外包橢球最優(yōu)化問題，這里選取外包橢球的最小化計(jì)算方法，該方法求解形式簡(jiǎn)單，相比較于最小化外包橢球體積的優(yōu)化準(zhǔn)則，該方法性能魯棒性更強(qiáng)。即有從而可以采用式子獲得最優(yōu)的尺度因子參數(shù)和β_k-1，P₁和P₂表示泛指的任意兩個(gè)方差矩陣。

尺度因子參數(shù)需要E(0,Q_k-1)和的直和計(jì)算，那么其計(jì)算準(zhǔn)則式為其最優(yōu)計(jì)算式為

對(duì)于尺度因子參數(shù)β_k-1，需要兩個(gè)橢球和的直和計(jì)算，考慮觀測(cè)向量更新條件下的方差矩陣計(jì)算式為：

從而，可以得到尺度因子參數(shù)β_k-1的計(jì)算公式為

在迭代計(jì)算過程中，觀測(cè)集合S_y形式一般都比較復(fù)雜，從而導(dǎo)致系統(tǒng)狀態(tài)向量方差矩陣P_k的計(jì)算復(fù)雜性，無論采用最小化橢球體積法還是最小化橢球跡準(zhǔn)則，都使尺度因子參數(shù)ρ_k的優(yōu)化計(jì)算很困難，甚至無法獲得解析解，若采用數(shù)值計(jì)算方法的話計(jì)算復(fù)雜度很高。在本發(fā)明中采用最小化性能指標(biāo)δ_k上界形式來計(jì)算

這樣可以獲得尺度因子參數(shù)ρ_k的一種次優(yōu)計(jì)算式

其中，p_m是矩陣的最大奇異值，c_m是矩陣的最大奇異值。

具體實(shí)施算例：采用本發(fā)明開展對(duì)艦船SINS導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)大方位失準(zhǔn)角模型狀態(tài)參數(shù)的估計(jì)計(jì)算任務(wù)。該算例應(yīng)用的大方位失準(zhǔn)角初始對(duì)準(zhǔn)模型可以參閱專著《非線性系統(tǒng)建模與濾波方法》一書，在這里僅作為本發(fā)明的驗(yàn)證實(shí)例，系統(tǒng)模型狀態(tài)變量由三向失準(zhǔn)角和兩向速度變量組成隨機(jī)系統(tǒng)狀態(tài)向量，其方程為：

其中，系統(tǒng)狀態(tài)變量為：噪聲向量為并且系統(tǒng)觀測(cè)向量設(shè)為東向和北向速度，獲得線性觀測(cè)函數(shù)，其觀測(cè)矩陣為H＝[0_2×3 I_2×2]。

其中，SINS系統(tǒng)中導(dǎo)航解算更新采用如下式子(56)和(57)：

其中，表示由系統(tǒng)角速度向量組成的斜對(duì)稱矩陣，gⁿ＝[0 0 -g]^T，表示系統(tǒng)IMU組件中加速度計(jì)的比力輸出量，表示導(dǎo)航坐標(biāo)系下的東向速度和北向速度組成的速度向量。仿真參數(shù)設(shè)置為：初始姿態(tài)誤差角中滾轉(zhuǎn)角和俯仰角分別設(shè)為3°和6°，方位失準(zhǔn)角數(shù)值較大，設(shè)為27°；艦船初始東向航速為5m/s和北向航速為10m/s；陀螺儀漂移設(shè)為0.03°/h，隨機(jī)漂移設(shè)為0.005°/h；加速度計(jì)零點(diǎn)偏差設(shè)為0.002gm/s²，隨機(jī)噪聲設(shè)為0.0005g m/s²。假設(shè)SINS系統(tǒng)中陀螺儀常值漂移ε向量和加速度計(jì)零偏誤差向量分別符合一階Markov模型噪聲，其外定界橢球?yàn)棣?0,Q_ε)和Q_ε表示系統(tǒng)IMU組件的陀螺儀隨機(jī)漂移的橢球包絡(luò)矩陣，Q_Δ表示系統(tǒng)IMU組件的加速度計(jì)隨機(jī)漂移的橢球包絡(luò)矩陣。導(dǎo)航速度觀測(cè)方程中的速度誤差噪聲滿足外定界橢球Ε(0,R)。在動(dòng)基座條件下進(jìn)行SINS系統(tǒng)初始對(duì)準(zhǔn)狀態(tài)參數(shù)估計(jì)計(jì)算仿真，艦船載體在海面上做機(jī)動(dòng)轉(zhuǎn)彎運(yùn)行，其運(yùn)行軌跡如圖3所示，顯示出了艦船初始所在位置坐標(biāo)。

利用本發(fā)明的方法獲得系統(tǒng)狀態(tài)變量的估計(jì)計(jì)算結(jié)果，其中導(dǎo)航系統(tǒng)大方位失準(zhǔn)角姿態(tài)失準(zhǔn)角狀態(tài)估計(jì)數(shù)據(jù)曲線、導(dǎo)航系統(tǒng)大方位失準(zhǔn)角誤差模型系統(tǒng)速度狀態(tài)估計(jì)數(shù)據(jù)曲線、導(dǎo)航系統(tǒng)大方位失準(zhǔn)角誤差模型的陀螺儀參數(shù)估計(jì)誤差數(shù)據(jù)曲線和本發(fā)明的導(dǎo)航系統(tǒng)大方位失準(zhǔn)角誤差模型的加速度計(jì)參數(shù)估計(jì)誤差數(shù)據(jù)曲線分別如圖4、圖5、圖6和圖7所示。其中，圖4顯示的是慣導(dǎo)系統(tǒng)三個(gè)姿態(tài)失準(zhǔn)角的估計(jì)誤差數(shù)據(jù)，很明顯可以看到，本發(fā)明對(duì)大方位失準(zhǔn)角初始對(duì)準(zhǔn)非線性誤差模型的狀態(tài)變量參數(shù)的估計(jì)計(jì)算其估計(jì)誤差獲得快速收斂，并且數(shù)值計(jì)算的穩(wěn)定性較好，沒有出現(xiàn)估計(jì)計(jì)算數(shù)據(jù)發(fā)散現(xiàn)象，這證明了本發(fā)明算法的優(yōu)越計(jì)算效能，原因在于Chebyshev多項(xiàng)式逼近非線性函數(shù)過程中，相比于Taylor級(jí)數(shù)擴(kuò)展來說，Chebyshev多項(xiàng)式具有極小化的逼近誤差，從方位失準(zhǔn)角估計(jì)數(shù)據(jù)也可以看出，方位失準(zhǔn)角收斂速度很快，基本上可以和其他兩向的姿態(tài)失準(zhǔn)角同時(shí)收斂的，這充分體現(xiàn)了本發(fā)明的一個(gè)計(jì)算優(yōu)勢(shì)。同時(shí)，從圖5的兩向速度誤差估計(jì)數(shù)據(jù)曲線也可以看出本發(fā)明算法的計(jì)算效能。從圖6和圖7的系統(tǒng)狀態(tài)變量估計(jì)數(shù)據(jù)可知，本發(fā)明的數(shù)值計(jì)算效能優(yōu)于Taylor級(jí)數(shù)擴(kuò)展法。

本發(fā)明說明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員所公知的現(xiàn)有技術(shù)。應(yīng)當(dāng)理解的是，對(duì)本領(lǐng)域普通技術(shù)人員來說，可以根據(jù)上述說明加以改進(jìn)或者變換，而所有這些改進(jìn)和變換都應(yīng)屬于本發(fā)明所附權(quán)利要求的保護(hù)范圍。