Page 95 - 《应用声学》2020年第1期

第 39 卷 第 1 期 石桂欣等： 水下目标跟踪的改进非线性滤波快速算法 91

常规方法是得到式 (4) 中的观测信息 Z k 后，直接使 IUKF(Improved Unscented Kalman filter) 算法和
用UKF或CKF算法进行跟踪。 ICKF(Improved Cubature Kalman filter) 算法 (使
但是，常规方法未考虑距离信息往往是有偏的 用Y k 为状态向量)。
情况，这会引起额外的跟踪误差。为了提升目标跟 2.2 改 进 算 法 在 UKF 框 架 下 的 简 化 及 复 杂
踪精度，定义ε k 为偏差系数，同时，将ε k 也看作状态 度分析
变量之一，构建一个新的状态向量，记为Y k ，
实际场景中，经常出现目标运动模型为线性方
T 程、观测模型为非线性方程的情况。对于非机动目
Y k = [x k , y k , ˙x k , ˙y k , ¨x k , ¨y k , ε k ] . (5)
标，跟踪系统常采用的目标运动模型有匀速运动模
记∆为采样间隔，则状态转移矩阵为
型 (Constant velocity, CV)、匀加速运动模型 (Con-
 2 
1 0 ∆ 0 ∆ /2 0 0
stant acceleration, CA)等；对于机动目标，常用的有
 
2
 0 1 0 ∆ 0 ∆ /2 0  Singer 模型、当前统计模型、半马尔可夫模型等 [21] 。
 
 
0 0 1 0 ∆ 0 0
  这些运动模型均可以写成线性方程的形式。针对
 
 
Φ =  0 0 0 1 0 ∆ 0  . 这些场景，本文进一步对改进算法进行简化分析和
 
  推导。考虑将式(1)重写为式 (7)所示的线性运动模
0 0 0 0 1 0 0
 
 
  型，状态转移矩阵Φ是一个n x (n x 是状态向量的维
0 0 0 0 0 1 0
 
 
数)维的方阵，具体取值由运动模型决定。根据式 (7)
0 0 0 0 0 0 1
表示的状态方程，可以将UKF算法中的一步预测做
(6) (i)
相应的化简。记 χ , i = 0, 1, 2, · · · , 2n x 为滤
k−1,k−1
状态转移方程为 波算法的采样点，可得
Y k = ΦY k−1 + w k−1 , (7) χ (i) = Φχ (i) , i = 0, 1, 2, · · · , 2n x , (10)
k,k−1 k−1,k−1
其中，w k−1 是噪声向量。观测信息服从以下模型： 易得


T  (0) ˆ
Z k = [(1 + ε k ) · r k , θ k ] + v k , χ k,k−1 = ΦY k−1,k−1 , i = 0,


 

 (i) (i)
ˆ
 χ = ΦS k−1,k−1 v + ΦY k−1,k−1 , (11)
ε k = H k · Y k , k,k−1

 
√ 
2 2 (8)  i=1, 2, · · · , 2n x ,
r
 k = (x k − B x ) + (y k − B y ) ,


 √
 (i)
 ( ) 其中，v = n x + λ [1] , i = 1, 2, · · · , 2n x ，[1] i 表
 i
 y k − B y
θ k = arctan , 是n x × n x 的单位

x k − B x 示矩阵[I n x − I N x ]的第i 列，I n x
其中，H k = [0, 0, 0, 0, 0, 0, 1]，r k 和 θ k 定义同式 (4)， 矩阵，参数λ 是一个缩放比例参数；S k−1,k−1 是由上
ˆ
ε k 是一个接近于 0 的正或负的未知数，可能是常 一时刻的状态协方差矩阵 P k−1,k−1 进行 Cholesky
数，也可能随时间变化。使用 UKF 或者 CKF 算 分解得到的。状态向量的一步预测结果为
ˆ
ˆ
法进行滤波后，即可得到当前时刻状态的估计值 Y k,k−1 = ΦY k−1,k−1 . (12)
[ ] T
ˆ ˆ ˆ ˆ ˆ 。根据上文分析易
Y k = ˆx k , ˆy k , ˙x k , ˙y k , ¨x k , ¨y k , ˆε k i
记ω , i = 0, 1, 2, · · · , 2n x 是第i个Sigma点的
m
得 ˆε k ≈ ˆε k−1 ，ˆε k 收敛速度很慢。为了使算法更快收
i
权值，ω , i = 0, 1, 2, · · · , 2n x 是第 i 个协方差矩阵
c
敛，每次滤波操作后，对 ˆε k 的值进行如下修正：
的权值，具体取值详见文献[4]。于是可得
( )

Z k (1) [ ] [ ] T
ˆε k = ζˆε k + (1 − ζ) − 1 , 0 (i) ˆ (i) ˆ

ˆ r k (9) ω c χ k,k−1 − Y k,k−1 χ k,k−1 − Y k,k−1 = 0,
√
 (13)
 2 2
ˆ r k = (ˆx k − B x ) + (ˆy k − B y ) ,
[ (i) ] [ (i) ] T
ˆ
ˆ
式 (9) 中，ζ 为修正系数，Z k (1) 为观测向量中的 ω i c χ k,k−1 − Y k,k−1 χ k,k−1 − Y k,k−1
ˆ
第一项 (距离信息)，ˆr k 为利用 Y k 估计出的距离信 i (i) ( (i) ) T
= ω ΦS k−1,k−1 v ΦS k−1,k−1 v ,
c
息。为区别于标准的 UKF 和 CKF 算法 (使用 X k
为状态向量)，将本节改进提出的算法分别记为 i = 1, 2, · · · , 2n x . (14)