[發明專利]基于濾波器方差陣修正的多源信息非等間隔聯邦濾波方法

有效

• 申請號: 201310739063.4
• 申請日: 2013-12-27
• 公開（公告）號: CN103697894A
• 公開（公告）日: 2014-04-02
• 發明（設計）人: 方崢;熊智;華冰;邵慧;楊鏡;許建新;彭惠;吳旋;柏青青;趙慧;潘加亮
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 許方

1.基于濾波器方差陣修正的多源信息非等間隔聯邦濾波方法，其特征在于，包括以下步驟：

步驟一、選取東北天地理坐標系，慣性導航系統誤差狀態量定義為：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T?????（1）

其中，φ_E,φ_N,φ_U分別表示慣性導航系統誤差狀態量中的東向平臺誤差角狀態量、北向平臺誤差角狀態量和天向平臺誤差角狀態量；δv_E,δv_N,δv_U分別表示慣性導航系統誤差狀態量中的東向速度誤差狀態量、北向速度誤差狀態量和天向速度誤差狀態量；δL,δλ,δh分別表示航空機載慣性導航系統誤差狀態量中的緯度誤差狀態量、經度誤差狀態量和高度誤差狀態量；ε_bx,ε_by,ε_bz，ε_rx,ε_ry,ε_rz分別表示慣性導航系統誤差狀態量中的X軸、Y軸、Z軸方向陀螺常值漂移誤差狀態量和X軸、Y軸、Z軸方向陀螺一階馬爾可夫漂移誤差狀態量；▽_x,▽_y,▽_z分別表示慣性導航系統誤差狀態量中的X軸、Y軸和Z軸方向加速度計零偏，上標T為轉置；

步驟二、采用地理坐標系下位置、速度、姿態線性化觀測原理，根據子濾波系統的工作特性，建立地理坐標系下子濾波系統的量測方程，所述子濾波系統具體包括INS/CNS姿態量測系統、INS/SAR圖像匹配水平位置量測系統、INS/TER地形匹配水平位置量測系統：

X(t)·=F(t)X(t)+G(t)W(t)---(2)]]>

其中，X(t)為步驟一中

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T加入時間參數t的表達形式，為X(t)的微分形式，F(t)表示INS/CNS/SAR/TER組合導航系統狀態方程的一步狀態轉移矩陣；G(t)表示INS/CNS/SAR/TER組合導航系統狀態方程的系統白噪聲誤差矩陣；W(t)為慣性/衛星組合導航系統狀態方程的系統誤差白噪聲矢量；

步驟三、將各子濾波系統量測方程中的子濾波系統誤差狀態量進行KF濾波，計算子濾波器的濾波估計均方差陣；增加修正方差陣ΔP_i，調整最終信息融合時的冗余子濾波系統信息的權重分配，具體如下：

在離散系統的卡爾曼濾波誤差分析中，

x~i(k)P=defxi(k)r-x^i(k)---(3)]]>

Pi(k)P=defE[x~i(k)Px~i(k)PT]---(4)]]>

其中，分別為第i個子濾波器的第k時刻狀態真實量和濾波估計量，為理想狀態值與濾波估計值的差值，為子系統理想估計均方差陣，E[.]為取均值運算，T為轉置運算；

在子濾波器的濾波估計均方差陣的基礎上增加修正方差陣ΔP_i后，得到經過修正的系統濾波器輸出的估計均方差陣P_i(k)：

P_i(k)＝P_i(k)+ΔP_i????????（5）

最終得到改進后的主濾波器信息融合算法為：

Pi(k)=Pi(k)+ΔPiPf(k)-1=Σi=1nPi(k/k)-1x^f(k)=Pf(k)(Σi=1nPi(k/k)-1x^i(k/k))---(6)]]>

其中，P_f(k)分別為主濾波器濾波輸出的濾波估計量和誤差方差陣；為主濾波器濾波輸出的誤差方差陣P_f(k)的逆，為第i個子濾波器k時刻的估計均方差陣P_i(k)的逆，為第i個子濾波器k時刻濾波估計量；

步驟四、采用無重置聯邦濾波器非等間隔修正算法，將一個濾波周期內的卡爾曼濾波分為兩個信息更新過程：時間更新和量測更新；

各子濾波器在信息輸出時，若未到量測時間則輸出時間更新信息，若到量測時間時則輸出量測更新信息；

步驟五、采用聯邦濾波器對步驟三中子濾波系統得出的濾波結果進行數據融合，輸出全局最優估計值。