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

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

的运算量也很接近。因此 η S-CKF 和 η S-UKF 的取值  1 0 ∆ 0 ∆ /2 0 
2
范围几乎是一样的。  2 


 0 1 0 ∆ 0 ∆ /2 
下面以 IS-CKF 算法为例，给出本文所提算法  
 0 0 1 0 ∆ 0 
的主要步骤，如表 1 所示。IS-UKF 与 IS-CKF 算法 Φ =   . (22)


 0 0 0 1 0 ∆ 
结构相同，只是对量测更新部分做了相应更改。  
 
 0 0 0 0 1 0 
表 1 IS-CKF 算法步骤  
Table 1 Improved simplified CKF algorithm 0 0 0 0 0 1
=
常 规 算 法 初 始 状 态 向 量 为 X 0
输入： 初始值 Y 0 、P 0 、Q、R 和转移函数矩阵 Φ、量测函数 T
[5, 3, 5, 3, 0, 0] ， 过 程 噪 声 方 差 为 Q =
h(·)；修正系数 ζ；采样时刻的总数 K
diag [1, 1, 0.1, 0.1, 0.01, 0.01]， 初 始 协 方 差 矩 阵
初始化：k = 0
为 P 0,0 = diag [1, 1, 0.1, 0.1, 0.01, 0.01]。 UKF 算
For k = 1 : K
时间更新：由式 (18) 和式 (20) 求出状态向量的一步预 法 中 的 参 数 为 α = 1， κ = 2， β = 0，
ˆ
ˆ
测值 Y k,k−1 及对应的协方差矩阵 P k,k−1 ； λ = 2， 以 下 同。 改 进 算 法 初 始 状 态 向 量 为
量测更新：同标准 CKF 的量测更新过程，求出状 T
Y 0 = [5, 3, 5, 3, 0, 0, 0] ， 过程噪声方差为 Q =
ˆ
ˆ
态向量的估计值 Y k 及对应的协方差矩阵 P k,k ；
diag [1, 1, 0.1, 0.1, 0.01, 0.01, 0.0001]， 初始协方差
ˆ
ˆ
更新 ˆε k ：根据 Y k 求出 ˆr k ，然后根据式 (9) 更新 Y k 中
的 ˆε k ； 矩阵为 P 0,0 = diag [1, 1, 0.1, 0.1, 0.01, 0.01, 0.0001]。
End For 给定真实的 ε k = 0.005，量测噪声方差为 R =
ˆ ˆ [ 2 −6 2 ]
输出： 目标的状态估计 Y k ，估计误差方差矩阵 P k,k diag 25 m , 5 × 10 rad 。修正系数为ζ = 0.8。
800
3 仿真实验与结果分析 ᜺฾ᓬག
700
 θ k
600 ፇౌ
首先使用仿真实验验证本文所提改进算法的
500
性能。使用MATLAB软件进行蒙特-卡洛仿真实验 y/m 400
来统计上述几种算法的平均运行时间和均方根误 300
差 (Root mean square error, RMSE)，并进行比较 200
ᄬಖ
分析。 100
0 नݽ
本实验中认为节点和目标均处于同一水平面
-100-50 0 50 100 150 200 250 300
上，且深度信息均已知，只考虑 x(东西) 和 y(南北) x/m
两个方向的坐标。如图 1 所示，假设只有一个观
图 1 目标轨迹
测节点 (坐标 [B x , B y ] = [−100 m, 700 m]) 对目标
Fig. 1 Real trajectory of the target
进行测距和测向。目标从原点处开始先做 CV 运
图 2 给出了某一次实验中 SCKF 和 IS-CKF 两
动，速度为 v x = 5 m/s，v y = 3 m/s；5 s 后加速
2
度为 a x = −0.1 m/s ，a y = 0.1 m/s 并持续到 种算法估计的轨迹。由于引入了偏差系数 ε k 对滤
2
2
55 s；55∼105 s 期间的加速度为 a x = 0.1 m/s ， 波结果进行修正，IS-CKF 的滤波精度得以提升。
2
a y = −0.1 m/s ；105 ∼ 110 s 期间做 CV 运动，速 SUKF 和 IS-UKF 的轨迹对比结果与图 2 类似，因
此未再给出。表2和表3给出了这几种算法进行500
度为上一阶段结束时 (105 s) 的目标速度。实验中
次蒙特 -卡洛实验后的均方根误差、平均运行时间
采用 CA 模型对目标的运动状态进行建模，本文
对比。由于简化前后的算法精度基本不变，表 3 只
所提改进的 UKF 和 CKF 算法 (IS-UKF、IS-CKF
给出了 IS-UKF、IS-CKF、SUKF、SCKF 四种算法
和 Improved UKF、Improved CKF) 的状态向量如
的精度对比结果。该场景下，相比常规算法，改进
式 (5) 所示，状态转移矩阵如式 (6) 所示，观测方程
算法的估计精度有一定的提升，稍高于常规算法，
为式 (8)；常规 UKF 和 CKF 算法 (SUKF、SCKF 和
虽然状态向量维数多了一维，但 IS-UKF和IS-CKF
标准UKF、标准CKF) 的状态向量如式(3)所示，状
算法的运行时间分别和相应的常规算法(标准UKF
态转移矩阵如式(22)所示，观测方程为式(4)。
和标准CKF)相当。