[發(fā)明專利]一種具有魯棒特性的協(xié)同導(dǎo)航濾波方法

有效

• 申請?zhí)枺?/td>: 201610907997.8
• 申請日: 2016-10-18
• 公開（公告）號: CN106568442B
• 公開（公告）日: 2020-01-10
• 發(fā)明（設(shè)計(jì)）人: 盧少然;范愛中;李晟
• 申請（專利權(quán)）人: 中冶華天南京電氣工程技術(shù)有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 11327 北京鴻元知識產(chǎn)權(quán)代理有限公司
• 代理人: 陳英俊;楊樺

1.一種具有魯棒特性的協(xié)同導(dǎo)航濾波方法，其特征在于，包括以下步驟：

(1)在協(xié)同導(dǎo)航從艇定位中，利用投影統(tǒng)計(jì)對從艇接收到的來自主艇的量測信息進(jìn)行異常判斷，包括以下分步驟：

1)獲得k個時刻的N維狀態(tài)向量的中位向量，表示為：

x_k表示第k個時刻的N維狀態(tài)向量，k＝1，…K，

x_nk代表x_k中第n個元素，

med代表取中位元素；

2)計(jì)算每個狀態(tài)向量關(guān)于中值偏差的單位向量，其中，偏差向量u_k：

u_k＝x_k-m，k＝1....K

由此得到狀態(tài)向量關(guān)于中值偏差的單位向量v_k為：

3)確定每個狀態(tài)向量x_k在各單位向量v_k上的投影：

第k個時刻的狀態(tài)向量的投影向量表示為z_k＝[z_1k，z_2k，...z_Kk]^T，

其中，H表示向量x轉(zhuǎn)置，

4)對每個z_k，通過中值和完全中值偏差來計(jì)算標(biāo)準(zhǔn)投影p_nk，

其中，c為調(diào)節(jié)參數(shù)，

MAD_k表示第k個時刻的值的完全中值偏差，

5)對所有k值重復(fù)上述2)、3)、4)計(jì)算過程，則第K個點(diǎn)的PS值為

得到PS值后，根據(jù)該量測點(diǎn)的量測值相對于量測點(diǎn)簇的偏離程度，賦予該量測點(diǎn)的量測值的權(quán)值ω_i；

(2)獲取量測信息的權(quán)值后，建立基于改進(jìn)Huber估計(jì)的協(xié)同導(dǎo)航魯棒濾波模型：

由協(xié)同導(dǎo)航的運(yùn)動方程和量測方程得到非線性模型：

其中，下標(biāo)k代表在時刻k時，x為狀態(tài)向量，y為量測值，u為系統(tǒng)輸入，v為系統(tǒng)噪聲，w為量測噪聲，v與w均服從零均值白噪聲，量測噪聲協(xié)方差陣為R，

量測方程近似寫作：因此協(xié)同導(dǎo)航非線性模型改寫為：

其中是協(xié)同導(dǎo)航從艇的位置預(yù)測值，表示實(shí)際值與預(yù)測值的差，

矩陣H_k定義為：

其中，是量測噪聲的平均值；

定義以下式子：

其中，R_k是k時刻的量測噪聲協(xié)方差陣；

P_k/k-1為一步預(yù)測誤差均方差；

則非線性模型可另寫為：

Z_k＝G_kx_k+ξ_k

定義殘差表示估計(jì)值和量測值之間的差，ζ＝Gx-Z，

定義函數(shù)則對角矩陣其中ω_i為量測值的權(quán)值，由步驟5)計(jì)算得到，

將對角矩陣Ψ分為兩部分：

Ψ_y與Ψ_x為重新構(gòu)造觀測模型后利用Huber估計(jì)求得的對角矩陣，將權(quán)值對角矩陣Ψ代入卡爾曼濾波中進(jìn)行濾波更新，實(shí)現(xiàn)濾波魯棒性。

2.根據(jù)權(quán)利要求1所述的具有魯棒特性的協(xié)同導(dǎo)航濾波方法，其特征在于，將權(quán)值對角矩陣Ψ代入卡爾曼濾波中進(jìn)行濾波更新的過程如下：

時間更新

P_k/k-1＝Φ_k，k-1P_k-1Φ^T_k，k-1+Q_k

量測更新

其中Φ_k，k-1為狀態(tài)過渡矩陣；

Q_k是k時刻的系統(tǒng)噪聲協(xié)方差陣；

Ψ_y與Ψ_x為重新構(gòu)造觀測模型后利用Huber估計(jì)求得的對角矩陣。