[發明專利]一種基于KLD采樣的混合粒子聯邦濾波數據處理方法

有效

• 申請號: 202110408351.6
• 申請日: 2021-04-15
• 公開（公告）號: CN113124863B
• 公開（公告）日: 2022-10-25
• 發明（設計）人: 周翟和;鄒克臣;李永斌
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00;G01C21/20;G01S19/45;G01S19/47;G06F17/11;G06F17/15
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明提出的一種基于KLD采樣的混合粒子聯邦濾波數據處理方法，屬于數字濾波和多傳感器數據融合技術領域，主要用于解決多傳感器組合導航系統既包含線性子系統又包含非線性子系統時傳統聯邦卡爾曼濾波無法獲得有效精度的問題，該方法以聯邦濾波為框架，融合GPS、磁力計及氣壓高度計數據，將線性子系統采用卡爾曼濾波算法解算，非線性子系統采用改進的粒子濾波進行解算。本發明相較于傳統的聯邦卡爾曼濾波算法有更高的精度和可靠性，適用于數據融合等應用場合。

技術領域

本發明為了解決多傳感器組合導航系統既包含線性子系統又包含非線性子系統時傳統的聯邦卡爾曼濾波(Federated Kalman Filtering，FKF)無法獲得有效精度的問題，提出了在聯邦濾波算法的基礎上，線性子系統采用卡爾曼濾波(Kalman Filter，KF)算法解算，非線性子系統采用KLD(Kullback-Lerbler Distance)采樣的無跡粒子濾波(UnscentedParticle Filter，UPF)算法進行解算，屬于數據處理和數據融合技術領域，適用于導航數據融合等應用場合。

背景技術

導航技術從誕生發展至今，沒有任何一個導航系統能夠同時滿足精度要求和可靠性要求，為解決這一不足，誕生出了組合導航系統。組合導航本質是多傳感器系統，多傳感器系統進行組合導航時常見的方式有集中式濾波和聯邦式濾波兩種。集中式濾波在高維系統中存在很多不足，比如計算量大、容錯性差等，而聯邦式濾波應用于多傳感器系統具有較為突出的優勢。經典的聯邦卡爾曼濾波算法是1998年由Carlson提出的，由于該算法具備計算量小、可靠性高等優點，在數據處理領域得到了較為廣泛的應用。但是當系統中包含非線性環節時，聯邦卡爾曼濾波算法的性能會大大降低。

發明內容

在本發明中，提出一種將線性子系統采用卡爾曼濾波算法、非線性子系統采用KLD-UPF算法進行數據融合的方法，總稱為混合粒子聯邦濾波(Hybrid Particle FederalFilter，HPFF)算法，該算法相比較傳統的聯邦卡爾曼濾波算法具有更高的穩定性和可靠性。

本發明具體過程如下：

步驟1：根據三軸加速度計和三軸陀螺儀的數據，應用姿態更新、速度更新、位置更新算法分別對姿態角、速度和位置進行更新；

步驟2：根據捷聯慣性導航系統(Strap-down Inertial Navigation System，SINS)狀態變量和系統噪聲建立SINS系統狀態方程；根據全球定位系統(GlobalPositioning System，GPS)誤差狀態變量和系統噪聲建立GPS狀態方程；

步驟3：利用GPS偽距作為觀測量建立SINS/GPS子系統非線性量測方程，根據磁力計信息和SINS系統解算的姿態信息可以得到SINS/磁力計子系統量測方程，根據氣壓高度計信息和SINS系統解算的姿態信息可以得到SINS/氣壓高度計子系統量測方程；

步驟4：將系統信息按照信息分配因子進行分配，信息分配因子按照無重置系統β_m＝0，β_i＝1/3的原則來確定，其中，β_m和β_i分別為主濾波器和子濾波器信息分配系數；

步驟5：對線性子系統采用卡爾曼濾波、非線性系統采用KLD采樣的無跡粒子濾波算法進行時間更新和量測更新，得到各子系統的局部最優估計和協方差矩陣；其中，對非線性子系統SINS/GPS采用KLD采樣的UPF算法處理如下：

Step1：在k＝0時刻，對先驗概率密度p(X₀)進行采樣得到初始粒子群其中N表示采樣粒子個數，初始化每個粒子權值為1/N；

Step2：進行KLD采樣，設定k時刻當前采樣粒子數n＝0，非空子空間數m＝0，預測粒子集合對應權值集合進行以下循環：