[發明專利]一種基于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
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于KLD采樣的混合粒子聯邦濾波數據處理方法，其特征在于，具體包括以下步驟：

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

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

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

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

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

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

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

(1)采用無跡卡爾曼濾波(Untraced Kalman Filtering，UKF)算法對先驗粒子集進行重要性采樣得到預測粒子并計算相應權值當前采樣粒子數n＝n+1；

(2)將預測的粒子及其權值并入預測粒子集合和對應的權值集合中，即：

(3)如果采樣到的粒子落入空的子空間中，將非空子空間數m＝m+1，同時將該子空間標記為非空；

(4)計算KLD，如果滿足則退出循環，否則繼續Step2中的(1)～(4)的采樣操作直到滿足條件，其中，n為當前采樣粒子數，m為非空子空間數，ε表示KLD采樣誤差界限，z_1-δ是上分位數為1-δ的標準正態分布；

Step3：對Step2中的粒子權值進行歸一化：

其中，表示k時刻歸一化后第i個粒子的權值，表示k時刻第i個粒子的權值，N表示采樣粒子個數；

Step4：狀態估計：

其中，表示k時刻的狀態量，表示k時刻歸一化后第i個粒子的權值，表示k時刻第i個粒子的狀態量，N表示采樣粒子個數；

Step5：k＝k+1，轉到Step2，直到濾波結束；

步驟6：得到各子系統的局部最優估計值后，將其輸送給主濾波器，主濾波器將各個子系統的結果進行最優融合，得到整個系統的狀態估計值；

步驟7：根據待修正的姿態、位置、速度及濾波后系統誤差估計值對導航參數的姿態、速度、位置進行修正。