[發(fā)明專利]一種平滑約束無跡卡爾曼濾波方法及目標跟蹤方法

在審

• 申請?zhí)枺?/td>: 201911114914.X
• 申請日: 2019-11-14
• 公開（公告）號: CN111047627A
• 公開（公告）日: 2020-04-21
• 發(fā)明（設計）人: 張宏偉;張小虎;楊夏
• 申請（專利權）人: 中山大學
• 主分類號: G06T7/277
• 分類號: G06T7/277
• 代理公司: 佛山信智匯知識產權代理事務所(特殊普通合伙) 44629
• 代理人: 唐杏姣

1.一種平滑約束無跡卡爾曼濾波方法，其特征在于，包括如下步驟：

步驟1，根據無跡變換獲取當前目標觀測時刻目標狀態(tài)的原始先驗概率密度函數；

步驟2，通過數值期望計算原始先驗概率的均值和方差；

步驟3，引入噪聲約束信息，通過計算近似可行域的中心，以獲取修正先驗概率密度；

步驟4，通過后驗迭代優(yōu)化尋求滿足約束條件的高斯分布均值和方差，產生滿足約束條件的新的高斯西格瑪點；

步驟5，對高斯西格瑪點進行加權計算，完成濾波過程。

2.根據權利要求1所述的一種平滑約束無跡卡爾曼濾波方法，其特征在于，在步驟1中，根據無跡變換獲取當前目標觀測時刻目標狀態(tài)的原始先驗概率密度函數的步驟具體包括：

步驟1a，根據采樣規(guī)則，利用修正先驗概率的均值和方差產生滿足約束條件的2n_x+1個西格瑪點以及相應的權值w_i，具體如下式所示：其中，n_x是狀態(tài)變量的維數，λ是尺寸因子，λ決定周圍西格瑪點的范圍，可以為n_x+λ≠0的任意值，是(n_x+λ)P_x均方根的第i列，該均方根的求解可由Cholesky分解得到,w_i是第i個粒子的權值，并且

步驟1b，將西格瑪點代入狀態(tài)方程，利用公式以及加權計算預測均值和預測協(xié)方差P_x；

步驟1c,時間更新，利用公式以及加權計算量測均值預測協(xié)方差P_zz與測量的互協(xié)方差P_xz。

3.根據權利要求2所述的一種平滑約束無跡卡爾曼濾波方法，其特征在于，在步驟2中，通過數值期望計算原始先驗概率的均值和方差的公式為：以及

4.根據權利要求3所述的一種平滑約束無跡卡爾曼濾波方法，其特征在于，在步驟3中，引入噪聲約束信息，通過計算近似可行域的中心，以獲取修正先驗概率密度的步驟具體為：

步驟3a,在可行域內，設加性噪聲的概率密度函數具有有界的連通支持，即p(e_k)＝0,其中表示滿足約束條件的狀態(tài)的n維聯(lián)通區(qū)域可行區(qū)域；

步驟3b，根據最大后驗估計理論，定義可行域的中心點為其中上標c表示約束，K表示總觀測時刻；

步驟3c,設并計算的近似初始優(yōu)化值，采用非線性規(guī)劃方法近似求解全局最小值，其中和分別表示在處的雅克比和海森矩陣；

步驟3d，選擇待估計參量的L1范數為障礙函數，將目標函數可＝表達為求解一個無約束凸優(yōu)化序列的形式，具體如下：

步驟3e，設如果均值在可行域，計算均值并將其定為初值，即否則，取權值最大的數值作為初值，即

步驟3f,通過擬牛頓迭代方法遍歷搜索全局最優(yōu)解為其中α*是步長，dⁱ是搜索方向，若則令α*＝1；

步驟3g，由可行域的中心，計算其一階矩作為正態(tài)分布方差獲取修正的先驗概率密度為

5.根據權利要求4所述的一種平滑約束無跡卡爾曼濾波方法，其特征在于，在步驟4中，通過后驗迭代優(yōu)化尋求滿足約束條件的高斯分布均值和方差，產生滿足約束條件的新的高斯西格瑪點的步驟具體包括：

步驟4a，根據采樣規(guī)則，利用修正先驗概率的均值和方差產生滿足約束條件的2n_x+1個sigma點以及相應的權值具體如下式所示：

其中，n_x是狀態(tài)維數，λ是尺寸因子，可以為n_X+λ≠0的任意值，是(n_x+λ)P_x均方根的第i列，w_i是第i個粒子的權值，并且

步驟4b，將西格瑪點代入狀態(tài)方程，利用公式以及加權計算預測均值和預測協(xié)方差P_x；

步驟4c,加權計算量測均值通過公式以及預測協(xié)方差p_zz以及狀態(tài)與測量的互協(xié)方差p_xz，；

步驟4d,利用公式以及計算增益K_k+1，狀態(tài)均值和方差P_k+1。