[發明專利]一種主被動觀測特征交織的融合濾波方法

有效

• 申請號: 202011048293.2
• 申請日: 2020-09-29
• 公開（公告）號: CN112198504B
• 公開（公告）日: 2022-04-08
• 發明（設計）人: 陸翔;周恒亮;徐慧;田田;張寧;匡華星
• 申請（專利權）人: 中國船舶重工集團公司第七二四研究所
• 主分類號: G01S13/86
• 分類號: G01S13/86;G06K9/62;G06F17/18
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種主被動觀測特征交織的融合濾波方法，其特征在于：

步驟一：按照下述子步驟實現主動傳感器目標運動特征的統計特性估計：

子步驟1：按照下述公式將主動傳感器的觀測結果由觀測域內極坐標系下用距離ρ、方位θ、俯仰坐標表示，轉換為轉換域內笛卡爾坐標系下用x、y、z坐標表示：

其中：

子步驟2：針對第n次觀測，按照下述公式，計算主動傳感器目標狀態估計、主動傳感器目標狀態估計協方差矩陣：

P(n|n-1)＝A(n-1)P(n-1|n-1)A^H(n-1)+Q(n)；

P(n|n)＝[I-K(n)C(n)]P(n|n-1)；

其中：

為預先設定的觀測擾動量：

K(n)＝P(n|n-1)C^H(n)[C(n)P(n|n-1)C^H(n)]^-1；

子步驟3：觀測域內主動傳感器目標狀態估計y(k)及觀測域內主動傳感器觀測結果協方差計算方法如下述公式所示：

y(k)＝[g₁,g₂,g₃,g₄,g₅]′；

其中：

其中表示x、y、z的導數：

將主動傳感器目標狀態估計記為：

將主動傳感器目標狀態估計協方差矩陣記為：

步驟二：按照下述子步驟實現被動傳感器目標運動特征統計特性估計：

子步驟1：將被動傳感器觀測結果表示為其中，X^k_本艦為地心－地固坐標系下表示的第n次觀測的本艦位置，ζ_目標記為：

子步驟2：按照下述公式，計算被動傳感器目標狀態估計、被動傳感器目標狀態估計協方差矩陣：

1)計算E_i：

2)計算E_i,b：

其中：

W_k-1＝N(0,Q(T₁))

u_n-1＝X_n-F(T₁)X_o^n-1；

T₁為本次觀測與上次觀測的時間差；將笛卡爾坐標系下的坐標轉換為修正球坐標系下的坐標；將修正球坐標系下的坐標轉換為笛卡爾坐標系下的坐標；

3)計算w_i：

其中：k為任意值；

4)計算η_n|n-1：

5)計算P_n|n-1：

6)計算量測預測協方差S_n：

H＝[0,0,0,1,1,0]；

S_n＝HP_n|n-1H'+R；

7)計算K_n：

K_n＝P_n|n-1H'S_k^-1；

8)計算預處理后被動傳感器觀測結果η_n|n：

η_n|n＝η_n|n-1+k_n(z_n-Hη_n|n-1)；

9)計算被動傳感器觀測結果的協方差矩陣P_n|n：

P_n|n＝P_n|n-1-K_nS_nK_n'；

將被動傳感器目標狀態估計記為：

將被動傳感器目標狀態估計協方差矩陣記為：

步驟三：按照如下子步驟執行主動觀測量的野值判斷：

子步驟1：計算主動觀測新息V_a(n+1)：

其中：Z_a(n+1)為主動傳感器第n+1次觀測結果，H_a(n+1)為主動傳感器觀測矩陣，為主動觀測狀態向量的一步預測結果；

子步驟2：計算主動觀測新息誤差統計特性F_a(n)：

R_a(n+1)為主動觀測協方差矩陣：

為主動傳感器觀測目標方位角度的測量方差；主動傳感器觀測目標俯仰角度的測量方差；為主動傳感器觀測目標距離的測量方差；

子步驟3：按照如下方法判斷主動觀測是否為野值點：

若滿足：

則判斷為正常值；

若滿足：

則判斷為野值；

設定C＝5；

步驟四：按照下述子步驟實現基于被動角度濾波的野值剔除：

子步驟1：記錄新息；

按照如下公式，計算被動傳感器觀測新息V_p(n+1)：

其中，H(n+1)＝[0,0,0,1,1,0]為被動傳感器觀測矩陣；Z(n+1)為n+1時刻被動傳感器觀測目標的被動觀測向量；

子步驟2：計算被動觀測新息的誤差統計特性F_p(n+1)：

F_p(n+1)＝H(n+1)P(n+1|n)H′(n+1)+R_p(n+1)；

其中，R_p(n+1)為被動觀測協方差矩陣：

為方位角度測量方差；為俯仰角度測量方差；

子步驟3：按照如下公式執行被動觀測結果的野值判斷：

若滿足：

則判斷為正常值；

若滿足：

則判斷為野值；

設定C＝5；

步驟五：按照如下方法執行多傳感器目標觀測特征交織的高精度融合濾波：

將主動傳感器目標狀態估計記為被動傳感器目標狀態估計記為主動傳感器目標狀態估計協方差記為被動傳感器目標狀態估計協方差記為則有高精度融合濾波結果