Page 8 - 《应用声学》2023年第3期

446 2023 年 5 月

将连续时间系统表示为离散形式，直角坐标系 其中，ε β (k) 表示测量噪声，满足零均值高斯分布，
下，k 时刻目标运动状态 X T (k) 和观测站运动状态 ε β (k) ∼ N(0, δ (k))。由于系统的观测方程非线性，
2
β
X W (k)分别表示为 因此对其进行泰勒级数展开取其一阶项来进行测
T 量方程的线性化。一阶泰勒级数展开：
X T (k) = [r xt (k), r yt (k), v xt (k), v yt (k)] , (8)

T
X W (k) = [r xw (k), r yw (k), v xw (k), v yw (k)] , (9) H k = ∂h(X k )

∂X k
X k =X k+1|k
其中，r xt (k) 和 r xw (k) 分别为 k 时刻目标和观测站 [ ∂h(X k ) ∂h(X k ) ∂h(X k ) ∂h(X k ) ]
= , (14)
沿 X 轴方向距离；r yt (k) 和 r yw (k) 为 k 时刻目标和 ∂r x,k ∂r y,k ∂v x,k ∂v y,k
观测站沿 Y 轴方向的距离；v xt (k) 和 v xw (k) 分别为 其中：
k 时刻目标和观测站沿 X 轴方向的速度；v yt (k) 和 ∂h(X k ) r y,k+ 1|k
v yw (k) 分别为 k 时刻目标和观测站沿 Y 轴方向的 = r 2 + r 2 ,
∂r x,k
x,k+1|k y,k+1|k
速度。 ∂h(X k ) r x,k+ 1|k
= − ,
则目标与观测站的相对运动状态为 X R (k) = r 2 + r 2
∂r y,k
x,k+1|k y,k+1|k
X T (k) − X W (k)： ∂h(X k ) ∂h(X k )
= 0, = 0.
T ∂v x,k ∂v y,k
X R (k) = [r x (k), r y (k), v x (k), v y (k)] , (10)
因此BO-EKF算法的迭代公式如下 [2] ：
其中，r x (k) 和 v x (k) 分别为 k 时刻目标相对观测站
沿 X 轴方向的距离和速度；r y (k) 和 v y (k) 分别为 k X k+1|k = AX k|k − U k+1|k , (15)
T
时刻目标相对观测站沿Y 轴方向的距离和速度。 P k+1|k = AP k|k A + Q, (16)
T
3.1 BO-EKF方法 P k+1|k H k+1
G k+1 = T 2 , (17)
对于匀速直线运动的目标，目标与观测站的相 H k+1 P k+1|k H k+1 + δ β,k+1
对运动状态方程为 X k+1|k+1 = X k+1|k + G k+1 (β k+1 − h(X k+1|k )),
(18)
X(k + 1) = AX(k) − U(k, k + 1) + w(k), (11)
P k+1|k+1 = (I − G k+1 H k+1 )P k+1|k , (19)
其中：
   
其中，X k+1|k 为k+1时刻状态向量的预测值，P k+1|k
r x (k) 1 0 T 0
    为k+1时刻状态向量的协方差矩阵的预测值，G k+1
   
 r y (k)   0 1 0 T  为k + 1时刻的卡尔曼增益，X k+1|k+1 为k + 1 时刻
X(k) =   , A =   ,
    状态向量的估计值，P k+1|k+1 为k + 1时刻状态向量
 v x (k)   0 0 1 0 
   
v y (k) 0 0 0 1 的协方差矩阵的估计值。
3.2 Cs-BO-EKF方法
 
r xw (k + 1) − r xw (k) − Tv xw (k)
对于匀速直线运动的目标，为校准参考声速，
 
 
 r yw (k + 1) − r yw (k) − Tv yw (k) 
U(k, k + 1) =   , 提高测向精度，将相速度 c s (k) 也作为估计状态量，
 
 v xw (k + 1) − v xw (k)  目标与观测站的相对运动状态方程为
 
v yw (k + 1) − v yw (k)
X(k + 1) = AX(k) − U(k, k + 1) + w(k), (20)
T 表示采样时间间隔，w(k) 表示过程噪声，一般假
其中：
设其满足零均值高斯分布，协方差矩阵为 Q。目标
   
方位的观测方程为 r x (k) 1 0 T 0 0
   
( )
r x (k)    
β(k) = arctan + ε β (k), (12)  r y (k)   0 1 0 T 0 




r y (k)    
X(k) =  v x (k)  , A =  0 0 1 0 0  ,
可以表示为    




 v y (k)   0 0 0 1 0 
   
β(k) = h(X k ) + ε β (k), (13) c s (k) 0 0 0 0 1