0 引　言

目标跟踪作为一个多年来的研究热点，在导航、雷达、声呐、图像、无线传感器网络等领域都有广泛的应用^{[1 − 3]}。

被动声呐是目前用于水下目标跟踪的重要传感器之一。被动声呐不需要主动发声，不易被发现，且能够探测到远距离目标，利用被动声呐可以探测到几千米到几十千米以上的目标^[4]。系统仅利用目标方位信息，实时估计出目标的运动轨迹信息，包括目标的位置与速度等，这一过程被称为纯方位跟踪（Bearing-Only Tracking, BOT）^[5]。

纯方位跟踪系统是典型的非线性系统，纯方位跟踪的主要挑战是非线性观测偏差以及径向距离的可观测性差^[6]。目前常用的纯方位跟踪算法有拓展卡尔曼滤波（Extend Kalman Filter, EKF）^[7]、伪线性卡尔曼滤波（Pseudolinear Kalman Filter, PLKF）^[8]、无迹卡尔曼滤波 （Unscented Kalman Filter, UKF）^[9]等算法。EKF算法通过截断泰勒级数展开对观测方程实现线性化，计算量小，但在仅有非线性的方位信息时，容易发散，精度不稳定；UKF需要调整多个超参数，算法复杂度高，对机动环境的适应性差。PLKF是一种线性递归贝叶斯估计方法，该算法利用伪线性估计器（Pseudolinear Estimator, PLE），将伪线性方程代替非线性方程的，与其他应用于纯方位跟踪的算法相比，PLKF的复杂度低，同时又有较好的跟踪性能。然而，PLKF算法存在更严重的非线性观测偏差问题，其等效观测矩阵和观测噪声存在相关性。DOGANCAY等^[10]分析了PLKF的偏差，推导了一种偏差补偿的方法，提出了偏差补偿伪线性卡尔曼滤波（Bias Compensated PLKF, BC-PLKF），在观测噪声稳定的情况下，该算法提升了PLKF的跟踪精度。当环境噪声与算法设置的误差方差不匹配时，算法出现过度补偿的情况，导致补偿以后的误差增大。

因此，考虑对传统自适应方法进行改进，对传统PLKF算法的补偿方法进行自适应优化。目前常用的自适应方法是基于Sage-Husa的新息自适应算法，利用新息的方差推测观测误差的方差，从而使误差方差匹配^[11]。孙铭芳等^{[12 - 13]}将新息协方差加权引入滤波器增益矩阵计算，提出IEKF（Innovation-based adaptive EKF）算法，该算法改善了EKF在噪声不匹配时的性能，但在实际噪声较小时，该算法在迭代时可能导致矩阵负定，使滤波发散。周萌萌等^[14]在IEKF基础上引入了渐消因子，进一步修正观测误差协方差，降低了发散概率。由于传统PLKF算法的伪线性方程观测误差方差随着平台与目标的相对位置变化发生改变，常用的Sage-Husa自适应滤波方法无法直接应用于PLKF算法中^[15]。

针对传统BC-PLKF算法在观测噪声不匹配时存在的滤波发散问题，提出了一种基于残差的自适应伪线性卡尔曼滤波 （Selective-Angle-Measurement Residual-based Adaptive PLKF, SAM-RA-PLKF）算法。根据观测残差对原始观测噪声方差进行修正，迭代修正PLKF的等效观测噪声方差，解决了传统的Sage-Husa自适应滤波无法直接应用于PLKF算法的问题。将角度选择策略 （Selective-Angle-Measurement, SAM）应用于修正的输出状态中，在系统发生滤波补偿过度时，直接使用PLKF滤波结果作为目标的滤波输出，在不增加计算量的同时，抑制该算法的过度补偿。在被动声呐的纯方位水下目标跟踪仿真场景中，在基线算法BC-PLKF发散的情况下，所提算法相比于EKF算法和IEKF算法跟踪性能更好。

1 纯方位目标跟踪建模 1.1 问题描述

在二维环境下，机动平台沿着如图所示的平台轨迹进行区域巡逻，利用被动声呐对探测区域的目标方位检测，考虑单个机动平台对水下目标进行纯方位跟踪。图1为本实验的仿真场景图。

1.2 状态方程

系统状态方程为：

$ {x_k} = F{x_{k - 1}} + {w_{k - 1}} 。$

(1)

式中：$ {x_k}{\text{ = }}{\left[ {{p_k},{v_k}} \right]^{\rm T}} $为$ k $时刻的目标状态向量；$ {p_k} = \left[ {{p_{x,k}},{p_{y,k}}} \right] $为目标$ k $时刻的位置向量；$ {v_k} = \left[ {{v_{x,k}},{v_{y,k}}} \right] $为目标$ k $时刻的速度向量；$ {\boldsymbol{F}} $为状态转移矩阵，$ F = \left[ {\begin{array}{*{20}{c}} {\text{1}}&{\text{0}}&T&0 \\ {\text{0}}&{\text{1}}&0&T \\ {\text{0}}&{\text{0}}&1&0 \\ {\text{0}}&{\text{0}}&0&1 \end{array}} \right] $；${w_k}$为过程噪声，假设其各项相互独立且服从正态分布，w_k～N(0，Q)，协方差阵$ Q $给出如下：

$ Q = \left[ {\begin{array}{*{20}{c}} {{{{q}}_x}\frac{{{T^3}}}{3}}&{\text{0}}&{{{{q}}_x}\frac{{{T^3}}}{2}}&0 \\ {\text{0}}&{{{{q}}_y}\frac{{{T^3}}}{3}}&0&{{{{q}}_y}\frac{{{T^3}}}{2}} \\ {{{{q}}_x}\frac{{{T^3}}}{2}}&{\text{0}}&{{{{q}}_x}T}&0 \\ {\text{0}}&{{{{q}}_y}\frac{{{T^3}}}{2}}&0&{{{{q}}_y}T} \end{array}} \right] 。$

(2)

式中：$T$为采样时间间隔；$ {{{q}}_x} $和$ {{{q}}_y} $分别为$x$和$y$方向上的过程噪声功率谱密度。

1.3 观测方程

系统观测方程为：

$ {\tilde \theta _k} = {\theta _k}{\text{ + }}{v_k}{\text{ = }}h({x_k},{r_k}) + {v_k}。$

(3)

式中：$ {\theta _k}{\text{ = }}h({x_k},{r_k}) = \arctan \dfrac{{{p_{x,k}} - {r_{x,k}}}}{{{p_{y,k}} - {r_{y,k}}}} $；$ {\theta _k} $为实际相对方位夹角，$ {\tilde \theta _k} $为观测平台与目标相对$X$轴正半轴的实际观测方位夹角；$ {r_k} = \left[ {{r_{x,k}},{r_{y,k}}} \right] $为平台在$k$时刻的位置向量；$ {v_k} $为方位观测噪声，$ v_k\sim\rm{\mathit{N}}(0,\sigma_k^2) $，表示观测方位的随机偏差。

2 算法描述 2.1 PLKF的传统偏差补偿分析

传统PLKF算法的流程如下，将观测式(3)进行如下形式变换：

$ \frac{{\sin ({\theta _k} - {v_k})}}{{\cos ({\theta _k} - {v_k})}} = \frac{{{p_{x,k}} - {r_{x,k}}}}{{{p_{y,k}} - {r_{y,k}}}} 。$

(4)

展开并进行近似处理以后，得到：

$ u_k^{\rm T}{r_k} = u_k^{\rm T}M{x_k} + {\eta _k}。$

(5)

式中：$ {u_k} = {\left[ {\begin{array}{*{20}{c}} {\sin {{\tilde \theta }_k}} \\ { - \cos {{\tilde \theta }_k}} \end{array}} \right]^{\rm T}} $，$ M = \left[ {\begin{array}{*{20}{c}} 1&0&0&0 \\ 0&1&0&0 \end{array}} \right] $，记等效观测值$ {z_k} = u_k^{\rm T}{r_k} $，等效观测矩阵$ {{\boldsymbol{H}}_k} = u_k^{\rm T}M $，则有：

$ {z_k} = {{\boldsymbol{H}}_k}{x_k} + {\eta _k}。$

(6)

其中，等效噪声

$ {\eta _k} = - \left\| {{d_k}} \right\|\sin {v_k} 。$

(7)

式中：$ {d_k} = {p_k} - {r_k} $，表示平台位置指向目标位置的向量。等效噪声方差为：

$ {R_k} = E\left\{ {\eta _k^2} \right\} = {\left\| {{d_k}} \right\|^2}E\left\{ {{{\sin }^2}{v_k}} \right\}。$

(8)

由于$ {v_k} $为零均值高斯噪声：

$ E\left\{ {{{\sin }^2}{v_k}} \right\} = \frac{{\text{1}}}{{\text{2}}}(1 - {e^{ - 2\sigma _k^2}})。$

(9)

当方位噪声较小，$ \dfrac{{\text{1}}}{{\text{2}}}(1 - {e^{ - 2\sigma _k^2}}) \approx \sigma _k^2 $，即此时有等效观测噪声方差：

$ {R_k} \approx {\left\| {{d_k}} \right\|^2}\sigma _k^2 。$

(10)

由此更新观测方程，得到PLKF算法的跟踪模型如下：

$ \left\{ \begin{array}{*{20}{l}} {x_k} = F{x_{k - 1}} + {w_{k - 1}} ，\\ {z_k} = {{\boldsymbol{H}}_k}{x_k} + {\eta _k}。\\ \end{array} \right. $

(11)

由以上对PLKF跟踪模型的推导结果可知，PLKF的观测矩阵${\boldsymbol{H}}$与观测噪声相关，这种相关性会增大算法的滤波误差。就此，如下对PLKF的偏差进行分析^[16]：

$ {\hat x_{k/k}} - {x_k} = {A_k} + {B_k} + {C_k} 。$

(12)

式中：

$ \begin{gathered} {A_k} = {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}，\\ \times P_{k/k - 1}^{ - 1}F({{\hat x}_{k - 1/k - 1}} - {x_{k - 1}})，\\ \end{gathered} $

(13)

$ {B_k} = - {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^TR_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}P_{k/k - 1}^{ - 1}{w_{k - 1}} ，$

(14)

$ {C_k} = - {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}{\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{\eta _k}。$

(15)

$k$时刻的偏差可以表示为：

$ {\alpha _k} = E\{ {\hat x_{k/k}} - {x_k}\} = E(A) + E(B) + E(C)。$

(16)

式中：$ {A_k} $为上一时刻${\alpha _{k - 1}}$的偏差传播，为系统本身误差，无法消除；$ {B_k} $表示${{\boldsymbol{H}}_k}$与上一时刻系统噪声${w_{k - 1}}$的相关性，基本可以忽略；$ {C_k} $表示${{\boldsymbol{H}}_k}$与观测噪声${\eta _k}$的相关性，由PLKF推导可知不能忽略，且为主要误差产生项。因此，对${C_k}$引发的偏差进行补偿可以减小PLKF的误差。${C_k}$可以近似计算：

$ \begin{gathered} {{\hat C}_k} = {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}R_k^{ - 1}E({\boldsymbol{H}}_k^{\rm T}{\eta _k}|{x_k}) = \\ - {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}R_k^{ - 1}\sigma _k^2{M^{\rm T}}(M{{\hat x}_{k/k}} - {r_k})。\\ \end{gathered} $

(17)

已知${P_{k/k}} = {(P_{k/k - 1}^{ - 1} + {\boldsymbol{H}}_k^{\rm T}R_k^{ - 1}{{\boldsymbol{H}}_k})^{ - 1}}$，得到补偿的状态估计：

$ {\begin{gathered} \hat x_{k/k}^{BCKF} = {{\hat x}_{k/k}} - {{\hat C}_k} = {{\hat x}_{k/k}} + {P_{k/k}}R_k^{ - 1}\sigma _k^2{M^{\rm T}}(M{{\hat x}_{k/k}} - {r_k}) 。\\ \end{gathered} }$

(18)

式中：$ \hat x_{k/k}^{BCKF} $为传统偏差补偿算法的状态向量。

2.2 改进残差自适应补偿PLKF算法

对PLKF的补偿涉及到观测噪声方差，在观测噪声方差未知时，设置的系统方差与实际系统方差的偏差大，PLKF的补偿算法效果降低甚至会产生负作用。为解决该问题需要对PLKF的等效观测误差方差进行自适应调整。

传统的卡尔曼滤波可以利用新息或残差进行自适应优化。依次对2种自适应方法进行分析并针对残差自适应方法做出改进应用在PLKF的自适应偏差补偿中。

$k$时刻的新息$ {d_k} $定义为滤波器实际观测值$ {z_k} $与预测观测值之差，即：

$ {d_k} = {z_k} - {\hat z_{k/k - 1}} = {{\boldsymbol{H}}_k}{\tilde x_{k/k - 1}} + {v_k} 。$

(19)

式中：$ {\tilde x_{k/k - 1}}{\text{ = }}{x_k} - {x_{k/k - 1}} $为一步预测误差。

新息方差：

$ {C_{{d_k}}} = E({d_k}d_k^{\rm T}) = {{\boldsymbol{H}}_k}{P_{k/k - 1}}{\boldsymbol{H}}_k^{\rm T} + {R_k} 。$

(20)

根据开窗估计可以得到新息的实时估计协方差为：

$ \hat{C}_{d_k}=\left\{\begin{array}{*{20}{l}}\dfrac{k-1}{k}\hat{C}_{d_{k-1}}+\dfrac{1}{k}d_kd_k^{\rm{T}}，k\leqslant L，\\ \dfrac{1}{L}\displaystyle\sum\limits_{i=k-L+1}^kd_id_i^{\rm{T}}，k > L。\end{array}\right. $

(21)

式中：$L$为采样时间宽度。

结合式(20)和式(21)基于观测新息对观测噪声方差进行估计：

$ {\hat R_k} = {\hat C_{{d_k}}} - {{\boldsymbol{H}}_k}{P_{k/k - 1}}{\boldsymbol{H}}_k^{\rm T}。$

(22)

显然，当采样宽度L较小时（即算法刚开始运行时），$ {\hat C_{{d_k}}} $由于采样波动会出现较小值，估计噪声方差可能会失去正定性，噪声方差失常，目标跟踪状态发生大幅度波动，最终导致滤波发生发散。而使用基于残差的自适应方法能够改善该问题。

$k$时刻的残差$ {r_k} $定义为滤波器实际观测值$ {z_k} $与估计观测值$ {\hat z_{k/k}} $之差，即：

$ {r_k} = {z_k} - {\hat z_{k/k}} = {R_k}C_{{d_k}}^{ - 1}{d_k}。$

(23)

残差方差为：

$ {C_{{r_k}}} = E({r_k}r_k^{\rm T}){\text{ = }}{R_k}C_{{d_k}}^{ - 1}{R_k}。$

(24)

根据开窗估计，残差的实时估计方差为：

$ \hat{C}_{r_k}=\left\{\begin{array}{*{20}{l}}\dfrac{k-1}{k}\hat{C}_{r_{k-1}}+\dfrac{1}{k}r_kr_k^{\rm{T}}，k\leqslant L，\\ \dfrac{1}{L}\displaystyle\sum\limits_{i=k-L+1}^kr_ir_i^{\rm{T}}，k > L。\end{array}\right. $

(25)

基于残差的观测误差方差估计^[15]为：

$ {\hat R_k} = {\hat C_{{r_k}}} + {{\boldsymbol{H}}_k}{P_k}{\boldsymbol{H}}_k^{\rm T}。$

(26)

由于 $ {R_k} $与径向距离向量$ {d_k} $和方位观测方差相关，目标距离随时间的变化较快，所以难以通过传统开窗估计方法直接得到某一时刻的PLKF统计残差$ {\hat C_{{r_k}}} $。

下面根据EKF和PLKF的观测矩阵关系，在基于残差的EKF更新基础上，推导提出针对PLKF算法的偏差更新方法。

在PLKF算法中：

$ {\hat R_k} = {\hat C_{{r_k}}} + {{\boldsymbol{H}}_k}{P_k}{\boldsymbol{H}}_k^{\rm T}。$

(27)

在EKF算法中：

$ \hat{\sigma}_k^2=\hat{C}_{r_{\theta,k}}+\boldsymbol{H}_{\theta,k}P_k\boldsymbol{H}_{\theta,k}^{\rm{T}}。$

(28)

式中：$ {\hat R_k} $为PLKF等效观测方程(11)的观测误差协方差，$ {{\boldsymbol{H}}_k} $为PLKF的估计观测矩阵，$ {\hat C_{{r_k}}} $为PLKF等效观测方程的残差；$ {{\boldsymbol{H}}_{\theta ,k}} $为EKF的估计观测矩阵。$ {\hat C_{{r_{\theta ,k}}}} $为探测方位的观测值的残差，计算方法如式(25)。

已知有：

$ {{\boldsymbol{H}}_k} = u_k^{\rm T}M = \left[ {\sin \theta , - \cos \theta ,0,0} \right] ，$

(29)

$ {{\boldsymbol{H}}_{\theta ,k}} = \left[ {\frac{{ - ({p_{y,k}} - {r_{y,k}})}}{{{{\left\| {{d_k}} \right\|}^2}}},\frac{{({p_{x,k}} - {r_{x,k}})}}{{{{\left\| {{d_k}} \right\|}^2}}},0,0} \right]。$

(30)

根据式(29)～式(30)的推导有：

$ \boldsymbol{H}_{\theta,k}=- \left\| d_k \right\| \boldsymbol{H}_k。$

(31)

根据式(27)、式(28)、式(31)得到，改进PLKF的观测误差方差更新公式如下：

$ \hat \sigma _k^2 = {\hat C_{{r_{\theta ,k}}}} + {{\boldsymbol{H}}_k}{P_k}{\boldsymbol{H}}_k^{\rm T}/({\left\| {{d_k}} \right\|^2}) 。$

(32)

式中：$ {\hat \sigma _k} $为在下一次滤波之前的残差估计观测方位方差。

在迭代过程中，使用采样时间之前的观测信息残差估计方位方差对下一时刻的伪线性观测方差进行修正，所以可以得到下一时刻的PLKF等效观测噪声方差$ {\hat R_{k + 1}} $为：

$ \begin{gathered} {{\hat R}_{k + 1}} \approx {\left\| {{d_{k + 1}}} \right\|^2}\hat \sigma _{k + 1}^2 \approx {\left\| {{d_{k + 1}}} \right\|^2}\hat \sigma _k^2 = \\ {\left\| {{d_{k + 1}}} \right\|^2}{{\hat C}_{{r_{\theta ,k}}}} + ({\left\| {{d_{k + 1}}} \right\|^2}/{\left\| {{d_k}} \right\|^2}){{\boldsymbol{H}}_k}{P_k}{\boldsymbol{H}}_k^{\rm T} 。\\ \end{gathered} $

(33)

得到新的目标状态补偿式如下：

${ \begin{gathered} \hat x_{k/k}^{RA} = {{\hat x}_{k/k}} - {{\hat C}_k} = {{\hat x}_{k/k}} + {P_{k/k}}\hat R_k^{ - 1}\hat \sigma _{k - 1}^2{M^{\rm T}}(M{{\hat x}_{k/k}} - {r_k})。\end{gathered} }$

(34)

式中：$ \hat x_{k/k}^{RA} $为基于残差自适应的补偿状态向量；$ \hat \sigma _{k - 1}^2 $为上一时刻对方位方差的估计；$ \hat R_k^{} $为当前时刻的等效噪声方差估计。

2.3 SAM准则优化修正

由于噪声波动的影响，在跟踪初始进行观测方差估计以及中途目标存在一定机动时，RA-PLKF存在过度补偿的风险，导致补偿以后的精度低于传统PLKF。在RA-PLKF的基础上，引入SAM机制，使用的SAM准则如下：

$ \hat{x}_{k/k}^{SAM-RA}=\left\{\begin{array}{*{20}{l}}\hat{x}_{k/k}，(\hat{\theta}_k^{RA}-\tilde{\theta}_k)^2 > \alpha_k^2，& \\ \hat{x}_{k/k}^{RA}，\mathrm{else}。& \end{array}\right. $

(35)

式中：$ \hat \theta _k^{RA} $为自适应补偿PLKF算法估计的方位角度，$ \hat \theta _k^{RA} = h(\hat x_{k/k}^{RA},{r_k}) $；$ \alpha _k^2 $为SAM-RA-PLKF的偏差阈值。

即在$k$时刻时，若$ {(\hat \theta _k^{RA} - {\tilde \theta _k})^2} > \alpha _k^2 $，($ \alpha _k^2 $取2-4倍^[16]的$ \hat \sigma _{k - 1}^2 $)，即此时RA-PLKF存在补偿过度，当前时刻算法不进行偏差补偿。

2.4 SAM-RA-PLKF算法

根据以上推导的残差补偿方法和SAM准则优化修正策略，得到SAM-RA-PLKF算法，算法流程如下：

步骤1　初始化。

设置目标初始状态$ {x_0} $，协方差$ {P_0} $，观测方位方差$ \sigma _0^2 $。

步骤2　状态预测。

$ \hat{x}_{k/k-1}=Fx_{k-1}。$

(36)

步骤3　协方差计算。

$ \hat{P}_{k/k-1}=FP_{k-1}F^{\rm{T}}+W_{k-1}。$

(37)

步骤4　自适应噪声方差估计，根据式(33)估计伪线性噪声方差$ {R_k} $。

步骤5　伪线性增益计算。

$ K_k=\hat{P}_{k/k-1}\boldsymbol{H}_k^{\rm{T}}(\boldsymbol{H}_k\hat{P}_{k/k-1}\boldsymbol{H}_k^{\rm{T}}+R_k)^{-1}。$

(38)

步骤6　目标状态更新。

$ x_{k/k}=\hat{x}_{k/k-1}\text{ + }K_k(z_k-h(\hat{x}_{k/k-1}))。$

(39)

步骤7　协方差更新。

$ P_{k/k}=(I-K_k\boldsymbol{H}_k)\hat{P}_{k/k-1}。$

(40)

步骤8　更新方位方差，根据式(32)，得到观测方位方差估计$ \hat \sigma _k^2 $。

步骤9　残差补偿滤波，将$ {x_{k/k}} $带入式(34)得到残差补偿的滤波结果$ \hat x_{k/k}^{RA} $。

步骤10　SAM准则状态估计，将$ \hat x_{k/k}^{RA} $带入式(35)，得到SAM补偿后的自适应滤波结果$ \hat x_{k/k}^{SAM - RA - PLKF} $。

步骤11　输出$ {\hat x_{k/k}}{\text{ = }}\hat x_{k/k}^{SAM - RA - PLKF} $，然后时间更新$ k = k + 1 $，回到步骤2继续进行滤波跟踪。

3 仿真实验与分析 3.1 实验设置

在图1所示的实验场景中，初始参数设置见表1。

观测平台的巡逻运动周期为T=600 s，采样间隔1 Hz。运动轨迹为：1～200 s在初始点保持匀速，201～300 s采用100 s的匀加速运动掉头由拐角1经过拐点2到达拐点3，401～600 s保持匀速，见表2。

设定采样间隔T_s=1 s，系统的过程噪声功率谱密度${q_x} = {q_y}{\text{ = 2}}{\text{.5}} \times {\text{e}^{-3}}$，观测方位噪声标准差$ \sigma_{\text{0}}=\text{1 mrad} $。

采用均方误差和时间平均均方根误差作为算法的性能评价指标，定义如下。

均方误差：

$ RMSE = \sqrt {\frac{1}{N}\sum\limits_{n = 1}^N {{{\left\| {\hat x_k^n - x_k^n} \right\|}^2}} } 。$

(41)

式中：$ N $为蒙特卡罗仿真的次数；$ \hat x_k^n $为第$n$次实验第$k$时刻的滤波输出状态向量；$ x_k^n $为第$n$次实验第$k$时刻的真实状态向量。

时间平均均方根误差：

$ RTAMS = \sqrt {\frac{1}{{M{{ \times }}N}}\sum\limits_{{{m}} = 1}^M {\sum\limits_{n = 1}^N {{{\left\| {\hat x_k^n - x_k^n} \right\|}^2}} } }。$

(42)

式中：$ M $为采样的总点数即T/T_s。RTAMS可以用来定量衡量算法的综合性能。

3.2 残差自适应对伪线性滤波的效果

在上述实验环境下，算法观测噪声方差（R₀、R₁、R₂、R₃、R₄）（分别为真实观测噪声方差$ \hat \sigma _0^2 $的1、2、4、6、8倍），实验分析对比传统BCKF算法和SAM-RA-PLKF对初始观测方差差异的敏感程度和对应算法性能，对该观测系统进行10次蒙特卡罗实验，得到MSE曲线如图2所示。

可知，当算法设定的观测噪声与真实的方差不匹配时，传统偏差补偿BC-PLKF跟踪效果随着设置观测方差与真实方差的偏差增大，跟踪效果越来越差。在设置噪声方差大于2倍的真实观测噪声方差时，跟踪误差达到了千米级，滤波发散，此时滤波器失去跟踪效果。如图3所示，在600 s内，SAM-RA-PLKF算法受到初始设置方差偏差的影响很小，且均方误差基本维持在300 m以内，相比于传统BC-PLKF滤波器性能稳定，且均方误差更小。该实验验证了所提的残差自适应修正对BC-PLKF算法的改进有效，在噪声方差波动的场景下，该算法相比传统的偏差补偿伪线性方法的稳定性和性能均得到提升。

3.3 综合算法性能对比

设置采样时间为600 s，初始观测噪声为R₅($ \hat \sigma _0^2 $的10倍)。在上述实验环境下进行400次的蒙特卡罗实验，分为10个不同方向目标跟踪，每个目标在相同目标轨迹下进行50次跟踪。跟踪目标轨迹如图4所示。

如图5所示，在位置方向上，在所有跟踪时段内，SAM-RA-PLKF的跟踪精度稍优于IEKF，优于EKF，而BC-PLKF已经发散；如图6所示，在300～400 s的跟踪阶段，所提算法的跟踪误差明显小于其他传统的算法，基本维持在200 m的误差左右。如图7所示，在速度方向上，在所有跟踪时段内，SAM-RA-PLKF跟踪效果最好，且相比IEKF精度提升明显，优于EKF，而BC-PLKF已经发散；如图8所示，在300～400 s的跟踪阶段，所提算法在速度方向上的跟踪误差也明显小于其他传统的算法，基本维持在1 m/s以内。整体上，从RMSE曲线上可以看出，无论是在位置还是速度方向上，在针对单平台的自适应噪声方差跟踪场景中，SAM-RA-PLKF算法性能最优，相比于传统的IEKF算法有所提升，更优于传统的EKF，而所提算法的基线算法BC-PLKF已经发生了滤波发散，验证了所提算法的有效性。

表3为该实验下记录的各个算法的时间平均均方根误差RTAMS和单个跟踪周期的算法运行时间。

从位置方向的RTAMS来看，跟踪精度上依次排序为：SAM-RA-PLKF>IEKF>EKF>BC-PLKF，定量分析所提算法的性能：SAM-RA-PLKF比IEKF减少了44.87%，比EKF减少了64.88%。从速度方向上，跟踪精度上依次排序为：SAM-RA-PLKF>IEKF>EKF>BC-PLKF，SAM-RA-PLKF比IEKF减少了17.30%，比EKF减少了30.99%。虽然新提出的算法在单次运行时间最久，但是该算法在时间数量级上维持在毫秒级别以内，不影响算法的实时性。

综合以上定性和定量分析，在本实验的巡逻防御场景中，从整体精度来看，SAM-RA-PLKF算法性能最好，比IEKF性能要好，IEKF比EKF要好，而所提算法的基线算法BC-PLKF算法已经发散无法跟踪目标。SAM-RA-PLKF算法在不影响实时性的同时，在综合性能上优于常用的纯方位跟踪算法EKF和IEKF，BC-PLKF。

4 结　语

在被动声呐纯方位跟踪场景中，针对传统的伪线性卡尔曼滤波算法在观测噪声不匹配时存在的滤波发散问题，本文基于传统的偏差补偿方法做出改进，将残差补偿机制加入了补偿算法流程中，提出了残差改进的自适应补偿方法，并使用SAM准则对其性能稳定性进行进一步优化，提出SAM-RA-PLKF算法。仿真表明，在观测噪声方差不匹配的环境下，本文所提算法在性能上优于常用的纯方位跟踪方法EKF、IEKF，改善了基线算法BC-PLKF的适应性，取得了更低的均方误差RMSE和时间均方误差RTAMS。所提残差自适应的伪线性卡尔曼滤波算法可以为海上环境的机动平台目标跟踪提供参考意义。