改進(jìn)Sage-Husa算法在SINS/GPS組合導(dǎo)航中的應(yīng)用

摘 要：針對在組合導(dǎo)航系統(tǒng)中應(yīng)用Sage-Husa自適應(yīng)濾波算法存在的濾波精度低和可靠性差等問題，提出一種改進(jìn)的Sage-Husa自適應(yīng)濾波算法。首先在 Sage-Husa自適應(yīng)濾波算法基礎(chǔ)上，通過指數(shù)漸漸消記憶加權(quán)估計方法來估計噪聲統(tǒng)計特性，提高算法的自適應(yīng)能力，后結(jié)合強(qiáng)跟蹤濾波引入漸消因子在線修正預(yù)測均方誤差矩陣，使改進(jìn)后的算法具有應(yīng)對系統(tǒng)誤差干擾等不確定因素的能力。仿真結(jié)果表明，改進(jìn)后Sage-Husa算法具有更高的自適應(yīng)能力以及在出現(xiàn)模型誤差和粗差干擾時能夠有效抑制濾波發(fā)散，保持良好的濾波性能，應(yīng)用在組合導(dǎo)航系統(tǒng)中具有更強(qiáng)的穩(wěn)定性和定位精度。

關(guān)鍵詞：組合導(dǎo)航；Sage-Husa自適應(yīng)濾波；漸消因子；強(qiáng)跟蹤濾波；噪聲估計；模型誤差

DOI：10.15938/j.jhust.2024.03.005

中圖分類號： TN713；TN966

文獻(xiàn)標(biāo)志碼： A

文章編號： 1007-2683（2024）03-0037-08

Application of Improved Sage-Husa Algorithm

in SINS/GPS Integrated Navigation

WEN Sheng， LIU Caiyun， LUAN Tiantian， SUN Mingxiao

（School of Automation，Harbin University of Science and Technology，Harbin 150080，China）

Abstract：To address the issues of low filtering accuracy and poor reliability encountered when employing the Sage-Husa adaptive filtering algorithm in combined navigation systems， an improved Sage-Husa adaptive filtering algorithm is proposed. Firstly， on the basis of the Sage-Husa adaptive filtering algorithm， the statistical characteristics of noise are estimated by the weighted estimation method of exponential fading memory to improve the adaptive ability of the algorithm. Then， by the introduction of the fading factor in the strong tracking filtering， the prediction mean-square error matrix is corrected online， so that the improved algorithm has the ability to cope with the uncertainties such as systematic error interference. Simulation results demonstrate that the improved Sage-Husa algorithm exhibits greater adaptive ability. It effectively suppresses filter divergence and maintains excellent filtering performance， even when faced with model error and coarse interference. Furthermore， when applied in combined navigation systems， the algorithm showcases improved stability and positioning accuracy.

Keywords：integrated navigation；Sage-Husa adaptive filtering；attenuation factor；Strong tracking filtering；noise estimation；model error

0 引 言

隨著時代的發(fā)展，人們對于導(dǎo)航定位的需求也越來越高，單一的導(dǎo)航系統(tǒng)很難滿足人們的需求，故產(chǎn)生了組合導(dǎo)航技術(shù)。組合導(dǎo)航是將兩種或兩種以上導(dǎo)航系統(tǒng)以多種方式進(jìn)行組合，以提高導(dǎo)航定位精度和穩(wěn)定性等方面的性能。在各組合導(dǎo)航系統(tǒng)中，捷聯(lián)式慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system，SINS）和全球衛(wèi)星定位系統(tǒng)（global positioning system，GPS）的組合具有很強(qiáng)的互補(bǔ)優(yōu)勢，性能可靠，是一種較為理想的組合導(dǎo)航系統(tǒng)［1］。

SINS/GPS組合導(dǎo)航系統(tǒng)一般采用卡爾曼（Kalman）濾波進(jìn)行數(shù)據(jù)的融合處理。理論上，只有在隨機(jī)動態(tài)系統(tǒng)的動力學(xué)模型準(zhǔn)確已知，且外部噪聲統(tǒng)計特性為不相關(guān)的高斯白噪聲的條件下，標(biāo)準(zhǔn)卡爾曼濾波才能獲得最優(yōu)估計［2-3］。但實際上，準(zhǔn)確的系統(tǒng)模型很難獲取，噪聲也具有不確定性，此外在組合導(dǎo)航實際應(yīng)用中，還存在量測異常，動力學(xué)模型誤差等問題［4］。為了解決上述問題，國內(nèi)外學(xué)者提出了多種自適應(yīng)算法。

文［5］中，通過強(qiáng)迫新息序列保持正交，確定了漸消濾波，可以提高算法穩(wěn)定性，有效解決了模型誤差下濾波發(fā)散問題。文［6-7］提出的強(qiáng)跟蹤濾波器（Strong Tracking Filter，STKF），具有較強(qiáng)的關(guān)于模型不確定性的魯棒性以及較強(qiáng)的突變狀態(tài)跟蹤能力。在此基礎(chǔ)上，文［8-9］通過引入統(tǒng)計學(xué)檢驗，確立了漸消因子的引入時機(jī)，并設(shè)計了多重漸消因子，進(jìn)一步提高了強(qiáng)跟蹤濾波的估計精度。

相比上述算法，文［10］和文［11］中提出的Sage-Husa自適應(yīng)濾波算法（Sage-Husa Adaptive Kalman Filter，SHAKF）不僅可以提高濾波精度，還可以對系統(tǒng)噪聲進(jìn)行在線實時估計，引起了廣泛關(guān)注。文［12］將魯棒估計與Sage-Husa自適應(yīng)濾波結(jié)合，減小了觀測粗差影響，提高了濾波的穩(wěn)定性。文［13］引入統(tǒng)計學(xué)檢驗，對Sage-Husa自適應(yīng)濾波中的遺忘因子做出改進(jìn)，提升了自適應(yīng)濾波在量測信息異常狀況下的導(dǎo)航精度精度。文［14-15］引入濾波異常判定條件，通過減少Sage-Husa自適應(yīng)濾波噪聲統(tǒng)計特性的非必要估計次數(shù)，減小了計算量。

為了減小組合導(dǎo)航系統(tǒng)在模型誤差和粗差干擾情況下等不確定性因素情況下定位誤差，進(jìn)一步提高組合導(dǎo)航系統(tǒng)的定位精度以及穩(wěn)定性，本文提出一種改進(jìn)Sage-Husa抗差濾波算法，相比于Sage-Husa濾波，該濾波算法做出了以下改進(jìn)：①采用指數(shù)漸消記憶法估計量測噪聲，可以使濾波器在長時間濾波之后仍能保持一定的自適應(yīng)能力，降低量測粗差對濾波的影響。②結(jié)合強(qiáng)跟蹤濾波的思想，對狀態(tài)一步預(yù)測均方誤差矩陣引入漸消因子，使得算法具有應(yīng)對系統(tǒng)模型誤差和狀態(tài)擾動等系統(tǒng)不確定因素的能力。最后通過仿真對比實驗驗證了本文提出的算法的有效性。

1 SINS/GPS 組合導(dǎo)航系統(tǒng)建模

SINS/GPS組合系統(tǒng)最直接的組合方式是松組合，其原理是SINS與GPS獨立工作，利用GPS接收機(jī)對載體的定位和測速信息與SINS輸出的位置和速度信息進(jìn)行組合，然后用濾波器估計出慣導(dǎo)系統(tǒng)解算信息的誤差來校正慣導(dǎo)系統(tǒng)［16］，原理如圖1所示。

1.1 SINS誤差數(shù)學(xué)模型

采用東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系n系，載體坐標(biāo)系為b系，地心慣性坐標(biāo)系為i系，地球坐標(biāo)系為e系；建立SINS線性誤差方程［17］：

φ·=φ×ωnin+（δωnie+δωnen）－Cnbεb

δ·v=fn×φ+vn×（2δωnie+δωnen）－（2ωnie+ωnen）×δvn+

CnbΔb

δ·L=δvNRM+h－vN（RM+h）2δh

δ·λ=secLRN+hδvE+vEsecLtanLRN+hδL－vEsecL（RN+h）2δh

δ·h=δvU（1）

式中：φ為東、北、天3個方向姿態(tài)角誤差；δv為東、北、天3個方向速度誤差；fn為加速度計在導(dǎo)航系比力輸出；δL、δλ、δh分別為緯度誤差、經(jīng)度誤差、高度誤差；ωnin表示導(dǎo)航系相對于慣性系的旋轉(zhuǎn)角速度，包括兩部分：地球自轉(zhuǎn)引起的導(dǎo)航系旋轉(zhuǎn)角速度ωnie以及慣性系統(tǒng)在地球表面附近移動引起的導(dǎo)航系旋轉(zhuǎn)角速度ωnen，且有下式成立：

ωnin=ωnie+ωnen（2）

式中：δωnie、δωnen分別為地球自轉(zhuǎn)角速度誤差和導(dǎo)航系轉(zhuǎn)動角速度誤差，表示如下：

δωnie=［0-ωiesinL·δLωiecosL·δL］T（3）

δωnen=－δvN（RM+h）+vNδh（RM+h）2

δvE（RN+h）－vEδh（RN+h）2

tanL·δvE（RN+h）+vEsec2L·δL（RN+h）－vEtanL·δh（RN+h）2（4）

式中：ωie為地球自轉(zhuǎn)角速度；RM，RN分別為子午圈與卯酉圈半徑。

式（1）中εb、Δ b為機(jī)體坐標(biāo)系下3個坐標(biāo)軸方向的陀螺儀漂移誤差及加速度漂移誤差，其中εb=［εx，εy，εz］為陀螺儀漂移誤差；Δ b=［Δx，Δy，Δz］為三軸加速度計漂移誤差；在實際應(yīng)用中陀螺漂移可以簡化為常值誤差和擾動白噪聲之和，常值誤差又有固定常值誤差和隨機(jī)常值誤差組成，固定常值誤差可以通過標(biāo)定技術(shù)進(jìn)行補(bǔ)償，而隨機(jī)常值誤差由于器件逐次啟動時的不確定性，通常由濾波器進(jìn)行狀態(tài)估計然后進(jìn)行補(bǔ)償，假設(shè)陀螺中的常值誤差已經(jīng)補(bǔ)償。則陀螺漂移誤差可以由隨機(jī)常值誤差εbc和擾動白噪聲wbg之和表示，同理加速度計誤差也可以作類似簡化，具體如下式所示：

εb=εbc+wbg，（ε·bc=0）

Δ b=Δ bc+wba，（Δ ·bc=0）（5）

1.2 SINS/GPS 組合導(dǎo)航系統(tǒng)濾波方程

組合導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合處理需要系統(tǒng)誤差狀態(tài)方程，因此根據(jù)式（1）～（5），建立松組合導(dǎo)航系統(tǒng)誤差的狀態(tài)方程：

X·=FX+GWb（6）

X=［φ，δvn，δp，εb，Δ b］T（7）

式中：φ=［φE，φN，φU］為三維姿態(tài)誤差；δvn=［δvE，δvN，δvU］是東向、北向、天向速度誤差；δp=［δL，δλ，δh］為緯度、經(jīng)度、高度誤差；F為誤差狀態(tài)轉(zhuǎn)移矩陣，可由式（1）推導(dǎo)得出：

F=FaaFavFap－Cnb03×3

FvaFvvFvp03×3Cnb

03×3FpvFpp03×303×3

06×15（8）

Faa=－（ωnin×）（9）

Fav=0－1（RM+h）0

1RN+h00

tanLRN+h00（10）

Fap=00vN（RM+h）2

－ωiesinL0－vE（RN+h）2

vEsec2LRN+h+ωiecosL0－vEtanL（RN+h）2（11）

Cnb=［C1C2C3］（12）

C1=cosαcosψ－sinαsinθ

－cosθsinψ

sinαcosψ+cosαsinθsinψ（13）

C2=cosαsinψ+sinαsinθcosα

cosθcosψ

sinαsinθ－cosαsinθcosψ（14）

C3=－sinαcosθ

sinθ

cosαcosθ（15）

Fva=［（Cnbfb）×］（16）

Fvv=（vn×）Fav－［（2ωnie+ωnen）×］（17）

Fvp=00vN（RM+h）2

－（2+ωiesinL）0－vE（RN+h）2

2ωiecosL+vEsec2LRN+h0－vEtanL（RN+h）2（18）

Fpv=01RM+h0

secLRN+h00

001（19）

Fpp=00－vE（RM+h）2

vEsecLtanLRN+h0－vEsecL（RN+h）2

000（20）

式中：（ωnin×）為ωnin的反對稱矩陣；Cnb為姿態(tài)變換矩陣；α、θ、ψ為機(jī)體滾轉(zhuǎn)角、俯仰角、航向角；vn為n系下東、北、天向速度向量；fb為載體系比力輸出。

G為噪聲分配矩陣：

G=-Cnb03×3

03×3Cnb

09×6（21）

W為系統(tǒng)噪聲矩陣，表示為

W=［wgx，wgy，wgz，wax，way，waz］T（22）

式中：wgx、wgy、wgz為陀螺儀角速度測量白噪聲；wax、way、waz為加速度計白噪聲。

松組合導(dǎo)航系統(tǒng)中，SINS解算出的三維速度、位置向量為vnSINS，pnSINS，GPS解算出的三維速度、位置向量為vnGNSS，pnGNSS，以速度和位置誤差為量測的量測方程為

Z=vnSINS-vnGNSS

pnSINS-pnGNSS=HX+V（23）

H=06×3I6×606×6，V=VvVp（24）

式中：H為觀測矩陣；V為量測噪聲矩陣；Vv、Vp分別為三維衛(wèi)星接收機(jī)速度量測白噪聲和位置量測白噪聲。

2 改進(jìn)的Sage-Husa濾波算法

由于自適應(yīng)卡爾曼濾波需要離散化的系統(tǒng)實現(xiàn)，因此將式（6）與式（23）組成的系統(tǒng)方程進(jìn)行離散化以獲得離散化系統(tǒng)模型：

Xk=Φk，k-1Xk-1+Γk-1Wk

Zk=HkXk+Vk（25）

其中：Xk為n維狀態(tài)向量；Zk為m維的量測向量；Φk，k－1為n階的狀態(tài)的一步轉(zhuǎn)移矩陣；Γk－1為n×l階的系統(tǒng)噪聲分配矩陣；Wk為l維系統(tǒng)噪聲向量；Hk為m×n階量測矩陣；Vk為m維量測噪聲向量。假定系統(tǒng)噪聲Wk和量測噪聲Vk為零均值高斯白噪聲，即滿足：

E（Wk）=0 ，E（Vk）=0（26）

E［WkWTj］=Qkδkj

E［VkVTj］=Rkδkj

E［WkVTj］=0（27）

Sage-Husa自適應(yīng)濾波可以根據(jù)量測信息實時估計并修正系統(tǒng)噪聲統(tǒng)計特性，從而減少量測值的誤差，提高濾波精度。Sage-Husa自適應(yīng)濾波算法如下［18］：

Xk，k－1=Φk，k－1Xk－1（28）

Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk-1Qk-1ΓTk-1（29）

vk=Zk-HkXk，k-1（30）

Rk=（1－1k）Rk-1+1k（vkvTk－HkPk，k-1HTk）（31）

Kk=PkHTk（HkPkHTk+Rk）-1（32）

Xk=Xk，k-1+Kkvk（33）

Pk=（I-KkHk）Pk，k-1（I－KkHk）T+KkRkKTk（34）

Qk=1k［KkvkvTkKTk+Pk-Φk，k-1PkΦTk，k－1］+（1－1k）Qk-1（35）

式中：vk為新息向量；Kk為濾波增益矩陣；Pk為一步預(yù)測均方誤差矩陣；Qk、Rk分別為系統(tǒng)噪聲矩陣和量測噪聲矩陣。

上述Sage-Husa自適應(yīng)算法不能在系統(tǒng)噪聲和量測噪聲均未知的情況下同時估計出二者參數(shù)，在實際應(yīng)用中由于系統(tǒng)結(jié)構(gòu)參數(shù)復(fù)雜且較為固定，而觀測量受外部影響較大，為了濾波穩(wěn)定性，本文在原算法基礎(chǔ)之上假定式（35）中系統(tǒng)噪聲方差陣Qk為常值［19］。

2.1 基于指數(shù)漸消記憶加權(quán)平均的噪聲估計

上述Sage-Husa自適應(yīng)濾波算法中，Rk由等加權(quán)時間平均估計得出，分析得知當(dāng)k→∞時，有Rk=Rk－1，即長時間濾波后失去自適應(yīng)能力，這種方法的優(yōu)點是，當(dāng)噪聲參數(shù)為固定已知常值時，在估計有效和收斂的前提下，隨時間增長，噪聲參數(shù)估計會越來越精確。但在實際應(yīng)用中，載體往往處于高動態(tài)變化中，實際噪聲參數(shù)具有弱平穩(wěn)性，因為環(huán)境因素還會出現(xiàn)異常量測等問題［20］，若仍使用等加權(quán)時間平均估計，會出現(xiàn)較大誤差。因此為了對新息始終保持適當(dāng)大小的自適應(yīng)能力以提高濾波精度，對式（31）中Rk的估計方法改為指數(shù)漸消記憶加權(quán)平均估計：

Rk=（1－βk）Rk-1+βk（vkvTk-HkPk，k-1HTK）（36）

βk=βk－1βk－1+b（37）

式中：βk為加權(quán)系數(shù)，β0=1，而0<b<1稱為遺忘因子，當(dāng)k充分大時有，β≈1－b。遺忘因子b取值越小，則對新息越敏感，對量測噪聲變化或量測異常的自適應(yīng)能力就越強(qiáng)，但b不能取值過小，否則噪聲估計結(jié)果會跳變劇烈，常取b=0.9～0.999［21］。

2.2 漸消因子

對上述Sage-Husa算法進(jìn)行分析。首先，新息序列在理論上為白噪聲，則由式（30）中新息表達(dá)式可以得出新息方差理論值E（vkvTk）為

E（vkvTk）=Hk（Φk，k-1Pk-1ΦTk，k-1+

Γk-1Qk-1ΓTk-1）HTk+Rk（38）

若采用同式（36）的指數(shù)漸消記憶加權(quán)方式，由實際新息時間序列vk估計如下方差陣：

Ck=（1－β）Ck－1+βvkvTk（39）

理論上，式（38）與式（39）應(yīng)當(dāng)一致，即近似有：

Ck≈Hk（Φk，k-1Pk-1ΦTk，k-1+Γk-1Qk-1ΓTk-1）HTk+Rk（40）

對于實際系統(tǒng)，由于建模不夠準(zhǔn)確或載體的不規(guī)則運(yùn)動，導(dǎo)致系統(tǒng)結(jié)構(gòu)參數(shù)Qk-1、Rk、Φk，k-1、Hk存在描述偏差，都會破壞式（40）的平衡性，影響濾波穩(wěn)定性，可以基于式（40）進(jìn)行檢驗和修改模型參數(shù)，以提高濾波穩(wěn)定性和精度。將式（40）移項，變?yōu)?/p>

Ck－HkΓk-1Qk-1ΓTk-1HTk－Rk≈

HkΦk，k-1Pk-1ΦTk，k-1HTk（41）

將強(qiáng)跟蹤濾波思想引入Sage-Husa濾波，實現(xiàn)形式為引入漸消因子λk修正預(yù)測均方差陣使式（41）成立，即：

Ck－HkΓk-1Qk-1ΓTk-1HTk－Rk=

HkΦk，k-1λkPk-1ΦTk，k-1HTk（42）

而后根據(jù)式（42）求得漸消因子λk：

λk=max（1，tr（Nk）tr（Mk））（43）

Nk=Ck-HkΓk-1Qk-1ΓTk-1HTk-Rk

Mk=HkPk，k-1HTk（44）

將λkPk-1替代式（29）中的Pk-1：

Pk，k-1=λkΦk，k-1Pk-1ΦTk，k-1+Γk-1Qk-1ΓTk-1（45）

式（45）中，由于λk≥1，濾波過程中若發(fā)生參數(shù)模型誤差較大時，可以修改Pk-1以調(diào)整濾波增益Kk加大量測對濾波估計的作用以保證濾波穩(wěn)定性。若濾波平穩(wěn)運(yùn)行時λk=1，為Sage-Husa自適應(yīng)濾波。

在Sage-Husa自適應(yīng)算法基礎(chǔ)之上將式（36）替換式（31），將式（45）替換式（29）得到改進(jìn)后的Sage-Husa濾波算法。

3 仿真結(jié)果與分析

為了驗證算法的有效性，本文使用MATLAB構(gòu)建仿真平臺，以無人機(jī)作為通航載體的代表，生成其運(yùn)動軌跡和實時狀態(tài)以及相關(guān)的慣性器件輸出的比力和角速度。其中無人機(jī)運(yùn)動軌跡中包含了加速、減速、爬升、下降、勻速、轉(zhuǎn)彎等多種運(yùn)動過程，可以有效驗證算法的性能，具體運(yùn)動軌跡如圖2所示。實驗使用東北天（ENU）坐標(biāo)系作為導(dǎo)航坐標(biāo)系，初始位置和速度均為零。

仿真時間設(shè)置為2700s，GPS采樣頻率為1Hz，慣導(dǎo)采樣頻率為100Hz。傳感器具體參數(shù)設(shè)置詳見表1。

為了驗證本文算法的性能，分別設(shè)置兩種場景來模擬濾波過程中系統(tǒng)模型誤差和粗差干擾狀況，并用改進(jìn)后的Sage-Husa自適應(yīng)濾波算法和Sage-Husa算法分別進(jìn)行濾波估計，并計算兩種算法下的均方根誤差（root mean square error，RMSE）。

RMSE=1n∑ni=1（X～i－Xi）2（46）

式中：n表示信號序列；Xi表示導(dǎo)航信息真值，由軌跡模擬得到；X～i表示組合導(dǎo)航系統(tǒng)輸出的信息估計值。

場景1，在導(dǎo)航實踐中，由于載體運(yùn)動極不規(guī)則，運(yùn)動模型和精度相當(dāng)有限，這會使得導(dǎo)航數(shù)據(jù)處理過程中，常常不適當(dāng)?shù)倪^高估計模型信息的精度，因而導(dǎo)致濾波發(fā)散。因此為了模擬模型誤差場景，在t=1000及t=1500時刻處，將一步預(yù)測均方誤差矩陣縮小100倍來模擬系統(tǒng)模型誤差問題，分別用本文中的改進(jìn)后的算法和原Sage-Husa算法進(jìn)行濾波，導(dǎo)航解算誤差結(jié)果如圖3及表2所示。圖中RSHAKF代表本文提出的改進(jìn)后的Sage-Husa自適應(yīng)算法，SHAKF為原Sage-Husa算法。

對于上述圖中兩種算法在所模擬場景1下位置均方根誤差見表2。

由圖3及表2中可以看出，Sage-Husa算法在面對場景1設(shè)置的系統(tǒng)模型誤差問題時，濾波估計并不準(zhǔn)確，位置誤差曲線出現(xiàn)明顯跳變，緯度誤差和經(jīng)度誤差值高達(dá)10m，位置均方根誤差達(dá)到3～4m，無法滿足組合導(dǎo)航高精度要求?；诟倪M(jìn)后的算法得到的位置誤差曲線更為平穩(wěn)，在兩個干擾時刻沒有明顯變化，相比Sage-Husa算法有著較高的定位精度，場景1下位置均方根誤差僅為1～1.5m。這是因為改進(jìn)后的算法引進(jìn)了漸消因子，其實質(zhì)是在出現(xiàn)模型誤差時，通過設(shè)置模型誤差的協(xié)方差矩陣，進(jìn)而影響濾波增益，調(diào)整預(yù)測信息和量測信息的權(quán)重從而可以更好的應(yīng)對模型誤差，具有更強(qiáng)的穩(wěn)定性。

場景2，在t=1200及t=1700時將量測位置信息插入100m粗差，分別在同一仿真參數(shù)及仿真軌跡情況下的組合導(dǎo)航數(shù)據(jù)中，用原算法和本文提出的改進(jìn)的算法兩種濾波算法進(jìn)行濾波估計，兩種算法下的濾波得出的位置誤差如圖4所示。

上述兩種算法在場景2下的位置均方根誤差見表3。

由圖4可以看出，在場景2下，Sage-Husa算法得出的位置誤差曲線在兩個量測異常點出現(xiàn)較大波動，其中緯度誤差達(dá)到20m，且由表3可以看出Sage-Husa算法下得出的位置均方根誤差較大，緯度均方根誤差值甚至達(dá)到6.8m。相比之下，基于本文算法得出誤差曲線比較平穩(wěn)，且精度較高，在場景2下位置均方根誤差僅有1～1.7m。這是因為改進(jìn)后的Sage-Husa算法在保留噪聲估計特性的基礎(chǔ)之上做出了一定改進(jìn)，采用了指數(shù)漸消記憶法估計量測噪聲特性，具有一定的自適應(yīng)性，可以對噪聲方差矩陣進(jìn)行修正，從而減小異常信息的影響，提高濾波估計精度。

4 結(jié) 論

本文提出一種改進(jìn)的Sage-Husa濾波算法，該算法在Sage-Husa自適應(yīng)濾波算法基礎(chǔ)之上，結(jié)合強(qiáng)跟蹤濾波引入了漸消因子，可以使得改進(jìn)后的算法具有應(yīng)對系統(tǒng)模型誤差和粗差干擾等問題的能力。由仿真結(jié)果可以看出，在SINS/GPS組合導(dǎo)航系統(tǒng)濾波過程中，面對模型誤差和粗差干擾等問題時，相比于原Sage-Husa算法，本文中提出的改進(jìn)后的Sage-Husa自適應(yīng)算法具有更高的精度和穩(wěn)定性。

參 考 文 獻(xiàn)：

［1］ 蔣晨.GNSS/INS組合導(dǎo)航濾波算法及可靠性分析［D］.徐州：中國礦業(yè)大學(xué)， 2018.

［2］ 孫淑光， 陳建達(dá).改進(jìn)調(diào)節(jié)因子的GPS/SINS組合導(dǎo)航自適應(yīng)濾波算法［J］.航天控制， 2022， 40（4）：53.

SUN Shuguang，CHEN Jianda GPS/SINS Integrated Navigation Adaptive Filtering Algorithm with Improved Adjustment Factor［J］. Aerospace Control， 2022， 40（4）：53.

［3］ 林孝工，焦玉召，聶君. 互相關(guān)噪聲下動力定位船艏向估計方法［J］.電機(jī)與控制學(xué)報，2018，22（3）：114.

LIN Xiaogong，JIAO Yuzhao，Nie Jun. A Method for Estimating the Bow Direction of Dynamic Positioning Ships under Cross correlation Noise. ［J］ Electric Machines and Control，2018，22（3）：114.

［4］ 劉菲， 王志， 戴曄瑩， 等.基于預(yù)測殘差的抗差自適應(yīng)濾波組合導(dǎo)航算法［J］.北京航空航天大學(xué)學(xué)報， 2023， 49（6）： 1301.

LIU Fei，WANG Zhi，DAI Huaying， et al. Robust Adaptive Filtering Integrated Navigation Algorithm Based on Predictive Residual［J］. Journal of Beijing University of Aeronautics and Astronautics ， 2023， 49（6）： 1301.

［5］ 夏啟軍， 孫優(yōu)賢， 周春暉.漸消卡爾曼濾波器的最佳自適應(yīng)算法及其應(yīng)用［J］.自動化學(xué)報， 1990， 16（3）：210.

XIA Qijun，SUN Youxian，ZHOU Chunhui. The Best Adaptive Algorithm and Its Application for Gradually Eliminating Kalman Filter. ［J］. Acta Automatica Sinica， 1990， 16（3）：210.

［6］ 徐樹生， 林孝工， 李新飛.強(qiáng)跟蹤自適應(yīng)平方根容積卡爾曼濾波算法［J］.電子學(xué)報， 2014， 42（12）：2394.

XU Shusheng，LIN Xiaogong，LI Xinfei. Strong Tracking Adaptive Square Root Volume Kalman Filter Algorithm［J］. Acta Electronica Sinica， 2014， 42（12）：2394.

［7］ 趙利強(qiáng)， 羅達(dá)燦， 王建林， 等.自適應(yīng)強(qiáng)跟蹤容積卡爾曼濾波算法［J］.北京化工大學(xué)學(xué)報（自然科學(xué)版）， 2013， 40（3）：98.

ZHAO Liqiang，LUO Dacan，WANG Jianlin et al. Adaptive Strong Tracking Volume Kalman Filter Algorithm［J］. Journal of Beijing University of Chemical Technology（Natural Science Edition）， 2013， 40（3）：98.

［8］ 郭士犖， 吳苗， 許江寧， 等.自適應(yīng)漸消卡爾曼濾波及其在SINS初始對準(zhǔn)中的應(yīng)用［J］.武漢大學(xué)學(xué)報（信息科學(xué)版）， 2018， 43（11）：1667.

GUO Shihun，WU Miao，XU Jiangning，et al. Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment［J］. Journal of Wuhan University （Information Science Edition）， 2018， 43（11）：1667.

［9］ 熊鑫， 黃國勇， 王曉東.基于多重漸消因子的強(qiáng)跟蹤SVDCKF組合導(dǎo)航算法［J］.重慶郵電大學(xué)學(xué)報（自然科學(xué)版）， 2021， 33（2）：330.

XIONG Xin，HUANG Guoyong，WANG Xiaodong. A Strong Tracking SVDCKF Integrated Navigation Algorithm Based on Multiple Fading Factors［J］. Journal of Chongqing University of Posts and Telecommunications （Natural Science Edition）， 2021， 33（2）：330.

［10］周勇， 張玉峰， 張超， 等.基于Sage-Husa的線性自適應(yīng)平方根卡爾曼濾波算法［J］.西北工業(yè)大學(xué)學(xué)報， 2013， 31（1）：89.

ZHOU Yong，ZHANG Yufeng，ZHANG Chao，et al. Linear Adaptive Square Root Kalman Filter Algorithm Based on Sage Husa［J］. Journal of Northwestern Polytechnical University， 2013， 31（1）：89.

［11］WANG Xingyu， WANG Anna， WANG Dazhi， et al. A Modified Sage-Husa Adaptive Kalman Filter for State Estimation of Electric Vehicle Servo Control System［J］. Energy Reports， 2022， 8（5）：20.

［12］Junting Wang， Tian he Xu， Zhen jie Wang.Adaptive Robust Unscented Kalman Filter for AUV Acoustic Navigation［J］. Sensors， 2020， 20（1）： 60.

［13］曾慶化， 趙天鈺， 趙賓， 等.基于指數(shù)漸消遺忘因子的組合導(dǎo)航自適應(yīng)濾波算法［J］.中國慣性技術(shù)學(xué)報， 2021， 29（3）：307.

ZENG Qingyu，ZHAO Tianyu，ZHAO Bin，et al. Adaptive filtering algorithm for integrated navigation based on exponential fading forgetting factor［J］. Journal of Chinese Inertial Technology， 2021， 29（3）：307.

［14］徐曉蘇， 潘永飛， 鄒海軍.基于自適應(yīng)濾波的SINS/DVL組合導(dǎo)航系統(tǒng)［J］.華中科技大學(xué)學(xué)報（自然科學(xué)版）， 2015， 43（3）：95.

XU Xiaosu，PAN Yongfei，ZOU Haijun. SINS/DVL Integrated Navigation System Based on Adaptive Filtering［J］. Journal of Huazhong University of Science and Technology （Natural Science Edition）， 2015， 43（3）：95.

［15］孫淑光， 溫啟新.改進(jìn)Sage-Husa算法在飛機(jī)組合導(dǎo)航中的應(yīng)用［J］.全球定位系統(tǒng)， 2021， 46（3）：54.

SUN Shuguang，WEN Qi Xin. Application of Improved Sage Husa Algorithm in Aircraft Integrated Navigation［J］. Global Position System， 2021， 46（3）：54.

［16］趙強(qiáng)，范思遠(yuǎn)，唐政林.基于迭代擴(kuò)展卡爾曼濾波的車輛運(yùn)動狀態(tài)估計［J］.森林工程，2021，37（1）：66.

ZHAO Qiang，F(xiàn)AN Siyuan，TANG Zhenglin. Vehicle Mtion Sate Etimation Bsed on Ierative Etended Kalman Fltering［J］. Forest Engineering， 2021，37（1）：66.

［17］嚴(yán)恭敏， 翁浚.捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社， 2019.

［18］肖業(yè)偉， 謝小剛.強(qiáng)跟蹤抗差自適應(yīng)濾波算法及其在無人機(jī)導(dǎo)航定位中的應(yīng)用［J］.測繪通報， 2021（4）：64.

XIAO Yewei，XIE Xiaogang. Strong Tacking Rbust Adaptive Filtering Algorithm and its Application in Unmanned Aerial Vehicle Navigation and Positioning［J］. Bulletin of Surveying and Mapping， 2021（4）：64.

［19］LUO Zeyuan， Fu Zanhao， Xu Qiwei.An Adaptive Multi-Dimensional Vehicle Driving State Observer Based on Modified Sage-Husa UKF Algorithm.［J］.Sensors （Basel， Switzerland）， 2020， 20（23）： 6889.

［20］高亢.里程計輔助的車載捷聯(lián)慣導(dǎo)系統(tǒng)動基座初始對準(zhǔn)方法研究［D］ .哈爾濱：哈爾濱工業(yè)大學(xué)， 2019.

［21］張欣.多旋翼無人機(jī)的姿態(tài)與導(dǎo)航信息融合算法研究［D］.長春：中國科學(xué)院研究生院（長春光學(xué)精密機(jī)械與物理研究所）， 2015.

（編輯：溫澤宇）