[發明專利]一種基于最小二乘和多重漸消因子的改進kalman濾波方法

有效

• 申請號: 201910035132.0
• 申請日: 2019-01-15
• 公開（公告）號: CN110061716B
• 公開（公告）日: 2021-05-04
• 發明（設計）人: 葉彥斐;陳剛;陳恒;黃家輝;童先洲
• 申請（專利權）人: 河海大學
• 主分類號: H03H17/02
• 分類號: H03H17/02
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 朱楨榮

1.一種基于最小二乘和多重漸消因子的改進kalman濾波方法，其特征在于，包括以下步驟：

步驟1、根據傳感器測量信息，獲取開始濾波前的時間序列p及其對應時刻的跟蹤目標的位置測量值y_p，其中p＝1，2，...，m，m為濾波初始時刻，對位置測量值進行最小二乘擬合，計算濾波初值；

步驟2、分析跟蹤目標運動特征及傳感器的測量精度，建立關于跟蹤目標的狀態方程和觀測方程；

步驟3、根據步驟2中得到的狀態方程、觀測方程各參數，及步驟1中得到的濾波初值進行改進的kalman濾波；改進的kalman濾波具體如下：計算狀態預測向量；通過新息協方差計算得到多重漸消因子，并利用輔助函數對多重漸消因子進行修正處理，然后利用修正后的多重漸消因子計算預測誤差協方差；計算濾波增益、狀態估計向量及估計誤差協方差；更新采樣時刻，獲得各采樣時刻跟蹤目標的狀態向量；

步驟1中計算濾波初值，具體如下：

步驟1.1、利用最小二乘法對步驟1中采集到的時間序列及其對應的跟蹤目標的位置測量值進行擬合，得到關于時間序列的二階擬合方程；

建立二階擬合方程f(t)和其對應的目標函數J：

f(t)＝β₀+β1₁t+β₂t² (1.1.1)

其中，β₀,β₁，β₂為擬合參數；

將時間序列p作為上述擬合方程的自變量t，代入擬合方程；在目標函數中，令變量y等于跟蹤目標的位置測量值y_p，代入目標函數；

對目標函數的各擬合參數求導，并令其值為0，得到矩陣方程：

計算得到二階擬合參數β₀，β₁，β₂；

將計算得到的擬合參數代入式(1.1.1)，得到符合跟蹤目標運動軌跡的二階擬合方程；

步驟1.2、利用步驟1.1中的擬合方程及濾波初始時刻p＝m，求取濾波初值；

將濾波初始時刻m作為自變量代入步驟1.1中得到的擬合方程，求解得到濾波初始時刻跟蹤目標的位置信息f(m)；

對步驟1.1得到的擬合方程進行一階求導，得到跟蹤目標運動的速度方程v(t)；

將濾波初始時刻作為自變量代入速度方程v(t)，得到濾波初始時刻跟蹤目標的速度信息v(m)；

步驟1.3、將步驟1.2中求得的m時刻跟蹤目標的位置信息f(m)和速度信息v(m)組合，得到濾波初始時刻的狀態估計向量

步驟2中建立關于跟蹤目標的狀態方程和觀測方程，表達式如下：

X(k)＝Φ_k|k-1X(k-1)+W(k) (2.1)

Z(k)＝H_kX(k)+V(k) (2.2)

其中，k為當前濾波時刻，k＝m+1，…，N，N為最終的濾波采樣時刻，X(k)為跟蹤系統在當前濾波時刻的狀態向量，Z(k)為跟蹤系統在當前濾波時刻的觀測向量，Φ_k|k-1為當前濾波時刻的狀態轉移矩陣，H_k為當前濾波時刻的觀測矩陣，W(k)和V(k)分別為跟蹤系統的過程噪聲和量測噪聲，且均為均值為0，當前濾波時刻協方差分別為Q_k和R_k的不相關白噪聲，Q_k為過程噪聲協方差，R_k為量測噪聲協方差；

步驟3具體如下：

步驟3.1、利用步驟2中得到的狀態轉移矩陣計算狀態預測向量具體公式如下：

其中，k為當前濾波時刻，k-1為上一濾波時刻，為上一濾波時刻的狀態估計向量；

步驟3.2、根據步驟3.1得到的狀態預測向量及步驟2中得到的觀測矩陣和觀測向量計算新息向量Y(k)，并利用上一時刻的估計誤差協方差及標準kalman濾波框架下的預測誤差協方差計算公式求解標準kalman框架下的新息協方差理論值C′(k)，具體公式如下：

其中，P′(k|k-1)為標準kalman濾波框架下的預測誤差協方差，上標T為矩陣的轉置，P(k-1|k-1)為上一濾波時刻的估計誤差協方差，E[*]為期望值；

步驟3.3、利用步驟3.2中得到的新息向量及新息協方差理論值，計算實際新息協方差與新息協方差理論值的差值F(k)，具體公式如下：

F(k)＝Y(k)^TY(k)-γtrace[E[Y(k)Y(k)^T]] (3.3.1)

其中，γ為可調儲存系數，γ≥1，trace[*]為矩陣的跡；

利用上述差值F(k)計算如下輔助函數S(k)，為求取多重漸消因子做準備，函數具體如下：

其中，arctan為反三角函數中的反正切函數；

步驟3.4、根據步驟3.2中的新息協方差理論值及新息向量求取多重漸消因子矩陣；

設當前濾波時刻對應的多重漸消因子λ_k為對角陣，即λ_k＝diag{λ_1k,λ_2k,...,λ_nk}，其中λ_ik對應第i個數據通道在k時刻的漸消因子的值，且滿足λ_ik≥1，i＝1,2,…,n，n為預測誤差協方差陣的維數；

設修正后的新息協方差C(k)與新息協方差理論值C′(k)滿足如下關系：

C(k)＝α_kC′(k) (3.4.1)

其中，Y₀為m+1時刻的新息向量，I_n為n階的單位矩陣，α_k為對角陣因子，即α_k＝diag{α_1k,α_2k，...,α_nk}，且滿足α_ik≥1，α_ik為k時刻新息協方差理論值的第i個數據通道的參數，由步驟3.2中新息協方差和新息向量的計算方程及公式(3.4.1)、(3.4.2)計算對角陣因子，即

其中，c′_ijk為k時刻對應的新息協方差陣理論值C′(k)中第i行第j列元素的值，c_ijk為k時刻對應的修正計算后的新息協方差陣C(k)中第i行第j列元素的值；

在利用新息協方差計算多重漸消因子的過程中，認為對角陣因子與多重漸消因子矩陣等價，利用式(3.4.3)及步驟3.3中的輔助函數，計算當前濾波時刻的多重漸消因子；具體如下：

λ_k＝diag{max^S(k){1,c_1jk/c′_1jk},max^S(k){1,c_2jk/c′_2jk},...,max^S(k){1,c_njk/c′_njk}}；

其中，max^S(k){*}為最大值的S(k)次方；

步驟3.5、根據步驟3.4中計算得到的當前時刻的多重漸消因子λ_k及上一時刻的估計誤差協方差求取預測誤差協方差P(k|k-1)，具體如下：

步驟3.6、根據步驟3.1中的狀態預測向量和步驟3.5中的預測誤差協方差求取濾波增益、狀態估計向量及估計誤差協方差，公式如下：

P(k|k)＝(I_n-K(k)H_k)P(k|k-1)

其中，K(k)為濾波增益，為狀態估計向量，P(k|k)為估計誤差協方差；

步驟3.7、采樣時刻更新，進行步驟3.1～3.6，直至采樣時刻結束，以此得到更加可靠的各采樣時刻跟蹤目標的狀態向量。

2.根據權利要求1所述的一種基于最小二乘和多重漸消因子的改進kalman濾波方法，其特征在于，γ＝1為最嚴格收斂判據。