基于高斯均值移动配准的动态偏差估计方法

摘要

一种基于高斯均值移动配准的动态偏差估计方法，包括如下步骤：步骤一，多平台系统中的传感器负责测量目标状态；步骤二，构造高斯均值移动方法的高斯核密度估计器；步骤三，利用扩展卡尔曼滤波器估计目标状态；步骤四，计算目标状态测量值的估计值；步骤五，计算传感器的动态偏差的估计值；步骤六，修正动态偏差的估计值；步骤七，修正动态偏差的估计值的收敛性判别。本发明方法仅使用一个目标的测量数据，能够快速、准确地估计传感器的动态偏差。该方法简单有效、易于实施，可广泛应用于机器人，智能交通，空中交通管制和航天、航空、航海等各领域。

说明书

技术领域

本发明涉及一种目标跟踪技术领域的方法，具体是一种基于高斯均值移动配准的动态偏差估计方法。

背景技术

在多传感器的目标跟踪系统中，信息融合技术可以提高对目标的探测、识别与跟踪能力。同时，多传感器的使用也带来一些问题，例如传感器的配准。未经配准的传感器组合使用可能导致系统性能比单一传感器的性能更差，使跟踪效果恶化，甚至会产生虚假目标。因此，在对多传感器的量测数据融合之前，需要对传感器进行配准。

传感器的偏差通常是固定的，或变化非常缓慢，固定偏差的配准可以分为离线配准和在线配准两种情况。离线配准方法主要有最小二乘法、广义最小二乘法和极大似然法等。在线配准方法中，传统的方法是扩维的卡尔曼滤波器方法，它通过将目标的状态和传感器的偏差扩维成一个状态矢量，再利用卡尔曼滤波器对扩维后的状态矢量进行估计，但该方法计算量很大。Nabaa和Bishop针对空间三维的分布式传感器网络，提出了扩展卡尔曼滤波器配准方法，该方法是将卡尔曼滤波器用于非线性系统中，利用非线性坐标转换机动模型，估计目标的状态和传感器的偏差。然而，传感器的偏差可能会由于技术维护、环境变化或其他原因而发生变化，它需要一种实时的估计方法。一方面，当传感器所在的环境突然变化时，传感器的偏差可能也会随之突然变化，随后又保持在一个恒定值上。Okello和Challa将传感器的配准与目标的航迹融合描述为一个贝叶斯估计问题，提出一个等效测量的配准方法。Li和Leung提出了unscented Kalman filter(“无味”卡尔曼滤波器)配准方法，该方法利用扩维状态与测量方程同时估计传感器的偏差和目标状态。另一方面，传感器的偏差也可能是缓慢变化且连续的，该变化是一个时变、动态的过程，等效测量的配准方法和无味卡尔曼滤波器配准方法均不能有效地估计此种情况的偏差。

均值移动方法是一种流行的非参数聚类方法，已经被用于图像分割、目标跟踪和图像融合领域。D.Comaniciu和P.Meer在《IEEE Transactions on PatternAnalysis and Machine Intelligence》(《模式分析与机器智能》)(pp.603-619，2002)发表的“Mean shift：a robust approach toward feature space analysis”(均值移动：一种属性空间分析的鲁棒方法)中给出了核函数为凸函数时均值移动方法收敛的充分条件，并给出了证明。

经对现有技术文献检索后发现，X.Lin和Y.Bar-shalom等人在《IEEETransactions on Aerospace and Electronic Systems》(《宇航与电子系统》)(pp.576-590，2004)发表了“Exact multisensor dynamic bias estimationwith local tracks”(基于局部航迹的多传感器动态偏差的“精确”估计方法)，该文中提出了一种可以用于解决动态偏差的“精确”估计方法，其本质是利用了最小均方根误差估计器。但是，该方法是基于多帧多目标的方法，随着目标数目的减少，该方法的估计精度会随之下降。

发明内容

本发明针对上述现有技术的问题，提供了一种基于高斯均值移动配准的动态偏差估计方法，使其基于单个目标的测量数据，并用高斯均值移动方法与扩展卡尔曼滤波器估计传感器的动态偏差，提高了传感器的动态偏差的估计精度。

本发明是通过以下技术方案实现的，本发明包括如下具体步骤：

步骤一，多平台系统中的传感器负责测量目标状态；

所述多平台系统，其中的每个传感器都是一个相对独立的系统，根据系统的测量方程独立测量监视区域内辐射源(目标)的距离、方位角等位置信息，多平台系统采用基于GPS(全球定位系统)的统一时间基准，使多平台系统内各传感器之间信号同步，一个扫描周期结束，各传感器通过战术数据链将数据传送到融合中心进行后续处理。

步骤二，构造高斯均值移动方法的高斯核密度估计器；

所述高斯均值移动方法，是指一个数据点d_t朝着给定窗口大小的数据点集合的均值移动，直到数据点d_t收敛到均值的邻域内的迭代过程，在满足收敛条件下，通过反复的迭代过程，高斯均值移动方法的结果是收敛的。

所述高斯均值移动方法，其收敛条件是高斯核函数的变量是相互独立的，即各变量的相关系数为零，且变量的协方差是对角阵，则高斯均值移动方法是收敛的。

所述高斯均值移动方法，是指在测量噪声服从高斯分布假设条件基础上的均值移动方法，具体为：根据极大似然密度函数构造一个多变量的高斯核密度估计器，对该估计器求梯度并对梯度进行数学变换，得到高斯均值移动方法的迭代公式，具体为：$y_{l+1}=\frac{\underset{j=1}{\overset{m}{\Sigma }}K(y_l,z\left(j\right),R)z\left(j\right)}{\underset{j=1}{\overset{m}{\Sigma }}K(y_l,z\left(j\right),R)},$l＝1，2，...    公式一

其中，K(y_l，z(j)，R)为高斯核函数，y_l为第1迭代后得到的均值，z(j)为第j次的观测值矢量，R为观测噪声的协方差矩阵。

所述高斯核密度估计器，是指由一组加权的高斯核函数构成的密度函数，高斯核函数是指在高斯噪声条件下的极大似然密度函数，极大似然密度函数是指在已经得到传感器含有偏差的测量信息的情况下，使偏差出现的概率最大的数值即作为偏差的估计值的随机变量的概率分布。

步骤三，利用扩展卡尔曼滤波器估计目标状态；

所述利用扩展卡尔曼滤波器估计目标状态，是指根据目标的状态方程、系统的测量方程以及传感器的动态偏差方程，把状态方程和测量方程中的非线性函数通过泰勒级数一阶展开，将其线性化，再用卡尔曼滤波器估计目标状态。

步骤四，根据估计得到的目标状态、系统的测量方程以及前一时刻的偏差估计值，计算得到目标状态测量值的估计值，其中，前一时刻的偏差估计值在第一次计算时取初始值。

步骤五，将目标状态的测量值以及测量值的估计值，代入到高斯均值移动方法的迭代公式中，计算传感器的动态偏差的估计值；

所述计算传感器的动态偏差的估计值，具体为：

${\hat{\beta }}_{l+1}^{*}=\frac{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2}{[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(1\right)\left]\right\}·\delta \left(j\right)}{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2}{[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(j\right)\left]\right\}}$公式二

其中，为第l次迭代后得到的动态偏差的估计值，$\delta \left(j\right)=z\left(j\right)-\hat{z}\left(j\right),$δ(j)为测量值的残差，z(j)为第j次的观测值，为步骤四中目标状态测量值的估计值，R^-1为观测噪声的协方差矩阵的逆矩阵，exp表示指数函数。

步骤六，修正动态偏差的估计值；

由步骤五计算得到的动态偏差的估计值，对舍入误差比较敏感，在求解过程中往往会出现无解的情况，所以需要修正动态偏差的估计值，消除舍入误差对运算结果的不利影响。

所述修正动态偏差的估计值，是指对步骤五中动态偏差的估计值的计算公式二中自然函数的指数进行归一化，具体为：

${\hat{\beta }}_{l+1}^{*}=\frac{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2{\left|\right|M\left|\right|}_1}{G}_j\}·\delta \left(j\right)}{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{{2\left|\right|M\left|\right|}_1}{G}_j\}}$公式三

其中，$G_j={[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(j\right)],$${\left|\right|M\left|\right|}_1=\underset{j=N-m+1}{\overset{N}{\Sigma }}\left|\delta \left(j\right)\right|=\left|\delta \left(1\right)\right|+···+\left|\delta \left(m\right)\right|,$||M||₁表示一阶范数，具体表示m个测量值的残差绝对值的和，为第l次迭代后得到的动态偏差的估计值，$\delta \left(j\right)=z\left(j\right)-\hat{z}\left(j\right),$δ(j)为测量值的残差，z(j)为第j次的观测值，为步骤四中目标状态测量值的估计值，R^-1为观测噪声的协方差矩阵的逆矩阵，exp表示指数函数。

步骤七，修正动态偏差的估计值的收敛性判别。

根据系统对偏差估计精度的要求，设定偏差收敛的门限值。

所述收敛性判别，是指对每次采样周期而言，修正动态偏差的估计值需重复多次，对前后两次修正动态偏差的估计值的矢量差求二阶范数，如果该范数大于设定的门限值，认为偏差没有收敛，返回步骤六继续修正动态偏差的估计值的计算，直到它满足收敛条件，即该范数小于或等于设定的门限值。具体是指：

$\left|\right|m\left({\hat{\beta }}_l^{*}\right)\left|\right|\le ϵ,ϵ>0$公式四

其中，$m\left({\hat{\beta }}_l^{*}\right)={\hat{\beta }}_{l+1}^{*}-{\hat{\beta }}_l^{*},$为第l次迭代后得到的动态偏差的估计值，ε为设定的门限值。

与现有技术相比，本发明具有以下有益效果：本发明将高斯均值移动方法与扩展卡尔曼滤波器方法结合，利用单个目标的测量数据，可以快速、有效地估计传感器的动态偏差，本发明在估计传感器的距离偏差和方位角偏差的均方根误差方面分别提高了67％和32％的估计精度。本发明提出高斯均值移动配准方法，解决了多传感器系统中动态偏差的在线估计问题。本发明方法的实施步骤较为简单，没有引证文献方法中大量的矩阵运算，并且本发明方法简单有效、易于实施，可广泛应用于机器人，智能交通，空中交通管制和航天、航空、航海等各领域。

附图说明

图1为本发明实施例中的工作流程图；

图2为本发明实施例中传感器与目标的初始位置分布图；

图3为本发明实施例中目标航迹比较图；

图4为本发明实施例中距离偏差估计的蒙特卡罗仿真结果图；

图5为本发明实施例中方位角偏差估计的蒙特卡罗仿真结果图；

图6为本发明实施例中距离偏差估计误差的均方根误差与克拉美罗下届的比较图；

图7为本发明实施例中方位角偏差估计误差的均方根误差与克拉美罗下届的比较图。

具体实施方式

下面结合附图对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图2所示，本实施例中的2个传感器的坐标分别为：(0，0)、(0，50)，单位：km，目标的初始状态为X₀＝[30000m，40m/s，30000m，40m/s]′，测量预测协方差矩阵为P(0|0)＝diag[(1000m)²，(50m/s)²，(1000m)²，(50m/s)²]。本实施例中传感器和目标(辐射源)都在同一平面XY内，每个传感器都可以测量目标的距离和方位角。2个传感器的动态偏差的初始值均为b₁＝b₂＝[50m，8mrad]′，距离和方位角的测量噪声的方差的标准差分别为σ_r＝2m和σ_θ＝0.5mrad，动态偏差方程的状态矩阵分别为F_b1(k)＝F_b2(k)＝0.99×I_2×2，噪声的协方差矩阵为Q_b1＝Q_b2＝diag[(2m)²，(0.3mrad)²]，采样周期T＝ls，采样次数N＝40，功率谱密度$\tilde{q}={6m}^2/s^3.$

本实施例包括如下步骤：

步骤一，系统中有2个传感器，每个传感器的位置固定，每个传感器都独立测量目标的距离和方位角，传感器含有偏差的测量方程为：

z(k)＝h(X(k))+β(k)+w(k)    (1)

其中，k为采样时刻，k＝1，...，N，N为采样次数，X(k)表示k时刻目标的真实状态；z(k)＝[z₁(k)′，...，z_n(k)′]′为n个传感器的观测值组成的矢量，“′”表示矩阵的转置；h(X(k))＝[h₁(X(k))′，...，h_n(X(k))′]′为目标状态的非线性测量函数的矢量；β(k)＝[β₁(k)′，...，β_n(k)′]′为传感器的动态偏差矢量，且假定动态偏差矢量与目标状态无关，本实施例中初始的动态偏差矢量β(0)＝[b₁′，b₂′]′；w(k)＝[w₁(k)′，...，w_n(k)′]′为零均值的白噪声，服从高斯分布，测量噪声的协方差矩阵为R＝diag[R₁，...，R_n]，其中$R_i=\mathrm{diag}[{\sigma }_{\mathrm{ir}}^2,{\sigma }_{\mathrm{i\theta }}^2]$表示第i个传感器的噪声的协方差矩阵，i＝1，...，n，σ_ir²，σ_iθ²分别为第i个传感器的距离和方位的噪声的协方差。

动态偏差矢量的动态方程为：

β(k+1)＝F_b(k)β(k)+v_b(k)    (2)

其中，F_b(k)＝diag[F_b1(k)，...，F_bn(k)]为状态转移矩阵，v_b(k)为零均值白噪声，动态偏差噪声的协方差矩阵Q_b(k)＝diag[Q_b1(k)，...，Q_bn(k)]。

步骤二，给定的观测值{z(k)；k＝1，...，N}，极大似然密度函数为：

$p=\left(z\left(k\right)\right|X\left(k\right),\beta \left(k\right))=\frac{1}{{\left(2\pi \right)}^n\sqrt{\left|R\right|}}\exp \{-\frac{1}{2}{(z\left(k\right)-\bar{z}\left(k\right))}^{\prime }{R}^{-1}(z\left(k\right)-\bar{z}\left(k\right))\}---\left(3\right)$

其中，$\bar{z}\left(k\right)={\left(\begin{array}{ccc}{\bar{z}}_1{\left(k\right)}^{\prime }& ···& \end{array},{\bar{z}}_n,{\left(k\right)}^{\prime }\right)}^{\prime }$${\bar{z}}_i\left(k\right)=h_i\left(X\left(k\right)\right)+{\beta }_i\left(k\right),$R^-1表示矩阵R的逆矩阵。

对于一个给定的由m个数据点组成的集合{z(j)；j＝1，...，m；m≤N}，必定属于一个d(d＞0)维的欧几里德范数空间的一个子集。

高斯核密度估计器是由一组加权的高斯核函数构成的密度函数，具体为：

$f_K\left(\xi \right)=\frac{1}{m}\underset{j=1}{\overset{m}{\Sigma }}K(\xi ,z\left(j\right),R)---\left(4\right)$

$K(\xi ,z\left(j\right),R)=\frac{1}{{\left(2\pi \right)}^n\sqrt{\left|R\right|}}\exp \{-\frac{1}{2}{(\xi -z\left(j\right))}^{\prime }{R}^{-1}(\xi -z\left(j\right))\}---\left(5\right)$

其中，ξ为自变量，f_K(ξ)为关于ξ的函数，K(ξ，z(j)，R)为高斯核函数。

高斯均值移动方法具体如下：

为了求出函数f_K(ξ)的极值点，先对函数f_K(ξ)求梯度：

$\frac{\partial f_K\left(\xi \right)}{\partial \xi }=\frac{1}{m}\underset{j=1}{\overset{m}{\Sigma }}K(\xi ,z\left(j\right),R)R^{-1}(z\left(j\right)-\xi )=f_K\left(\xi \right)R^{-1}m\left(\xi \right)---\left(6\right)$

其中，

$m\left(\xi \right)=\frac{R▿f_k\left(\xi \right)}{f_k\left(\xi \right)}=\frac{\underset{j=1}{\overset{m}{\Sigma }}K(\xi ,z\left(j\right),R)z\left(\xi \right)}{\underset{j=1}{\overset{m}{\Sigma }}K(\xi ,z\left(j\right),R)}---\left(7\right)$

m(ξ)为均值移动矢量，它表示由核函数K(ξ，z(j)，R)加权的数据点指向样本均值，这种反复移动的过程，被称作均值移动方法。式(7)表明均值移动矢量与归一化的密度梯度估计成正比，并且它总是指向梯度增加的方向，令y_l＝ξ，式(7)可以写为：

y_l+1＝y_l +m(y_l)    (8)

其中

$y_{l+1}=\frac{\underset{j=1}{\overset{m}{\Sigma }}K(y_l,z\left(j\right),R)z\left(j\right)}{\underset{j=1}{\overset{m}{\Sigma }}K(y_l,z\left(j\right),R)},l=\mathrm{1,2},···---\left(9\right)$

y_l+1是以y_l为中心点，由高斯核计算得到的加权的均值，y₁为核函数初始位置的中心点，通过均值移动的迭代计算，式(8)会收敛到密度估计器的梯度为零的一个固定点。

高斯均值移动方法的式(8)是在式(3)中的联合似然函数基于高斯噪声的假设条件得到的，高斯均值移动方法将被用于传感器的动态偏差配准。

步骤三，利用扩展卡尔曼滤波器估计目标状态；

目标的状态方程采用离散的连续白噪声加速模型，

X(k+1)＝ FX(k)+v(k)    (10)

其中，$X={[x,\stackrel{·}{x},y,\stackrel{·}{y}]}^{\prime },$F＝diag[F_x，F_y]，F_x＝F_y＝[1，T；0，1]，v(k)为零均值的白色噪声，噪声的协方差Q＝diag[Q_x，Q_y]，$Q_x=Q_y=[\frac{1}{3}{T}^3,\frac{1}{2}{T}^2;\frac{1}{2}{T}^2,T]\tilde{q}.$T为采样周期，为功率谱密度。

所述利用扩展卡尔曼滤波器估计目标状态，具体如下：

根据式(1)和式(10)，利用卡尔曼滤波器对泰勒级数一阶展开后的状态方程和观测方程进行滤波，得到目标状态的估计值：

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)[z(k+1)-H(k+1)\hat{X}(k+1|k)-\hat{\beta }\left(k\right)]---\left(11\right)$

其中，

$H(k+1)=\frac{\partial h\left(X\right)}{\partial X}{|}_{X=\hat{X}(k+1|k)}---\left(12\right)$

P(k+1|k)＝F(k)P(k|k)F(k)′+Q(k)    (13)

S(k+1)＝R(k+1)+H(k+1)P(k+1|k)H(k+1)′    (14)

W(k+1)＝P(k+1|k)H(k+1)′S(k+1)^-1    (15)

P(k+1|k+1)＝P(k+1|k)-W(k+1)S(k+1)W(k+1)′    (16)

步骤四，根据估计得到的目标状态、系统的测量方程以及前一时刻的偏差估计值，计算得到目标状态测量值的估计值，其中，前一时刻的偏差估计值在第一次计算时取初始值，具体如下：

$\hat{z}\left(k\right)=h\left(\hat{X}\left(k\right)\right)+\hat{\beta }\left(k\right),k=1,···,N---\left(17\right)$

其中，为目标状态测量值的估计值，为步骤三中的目标状态的估计值，为前一时刻的偏差估计值。

步骤五，计算传感器的动态偏差的估计值：由步骤二得到的高斯均值移动方法，对于j＝1，...，m，m≤N，将目标状态的测量值和测量值的估计值代入式(9)，可以得到：

${\hat{\beta }}_{l+1}^{*}=\frac{\underset{j=N-m+1}{\overset{N}{\Sigma }}\left\{K\right[{\hat{\beta }}_l^{*},\delta \left(j\right),R\left]\right\}\delta \left(j\right)}{\underset{j=N-m+1}{\overset{N}{\Sigma }}\left\{K\right[{\hat{\beta }}_l^{*},\delta \left(j\right),R\left]\right\}},l=\mathrm{1,2},···---\left(18\right)$

其中，式(18)中的上标“*”是为了区分与β_i，表示高斯均值移动方法中的第1次估计，β_i则代表式(1)中第i个传感器的偏差，表示在点处计算得到的加权均值，在迭代方法的最后，会收敛到密度分布δ(j)的极值点，记作

$\delta \left(j\right)=z\left(j\right)-\hat{z}\left(j\right)---\left(19\right)$

z(j)＝h(X(j))+β(j)+ω(j)    (20)

式(19)中的由式(17)计算得到。

由式(17)-(20)，可以得到动态偏差的估计值：

${\hat{\beta }}_{l+1}^{*}=\frac{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2}{[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(1\right)\left]\right\}·\delta \left(j\right)}{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2}{[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(j\right)\left]\right\}}---\left(21\right)$

步骤六，直接根据步骤五计算得到的动态偏差的估计值，对舍入误差比较敏感，在求解过程中往往会出现无解的情况。因此，需要对自然函数的指数进行归一化的数学运算，修正动态偏差的估计值，消除舍入误差对运算结果的不利影响。

所述修正动态偏差的估计值，具体是指：

${\hat{\beta }}_{l+1}^{*}=\frac{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{2{\left|\right|M\left|\right|}_1}{G}_j\}·\delta \left(j\right)}{\underset{j=N-m+1}{\overset{N}{\Sigma }}\exp \{-\frac{1}{{2\left|\right|M\left|\right|}_1}{G}_j\}}---\left(22\right)$

其中，

$G_j={[{\hat{\beta }}_l^{*}-\delta \left(j\right)]}^{\prime }{R}^{-1}[{\hat{\beta }}_l^{*}-\delta \left(j\right)]---\left(23\right)$

${\left|\right|M\left|\right|}_1=\underset{j=N-m+1}{\overset{N}{\Sigma }}\left|\delta \left(j\right)\right|=\left|\delta \left(1\right)\right|+···+\left|\delta \left(m\right)\right|---\left(24\right)$

根据式(8)，偏差估计的迭代方程可以表示为：

${\hat{\beta }}_{l+1}^{*}={\hat{\beta }}_l^{*}+m\left({\hat{\beta }}_l^{*}\right)---\left(25\right)$

步骤七，修正动态偏差的估计值的收敛性判别：对每次采样周期而言，修正动态偏差的估计值需重复多次，对前后两次修正动态偏差的估计值的矢量差求二阶范数，如果该范数大于设定的门限值，认为偏差没有收敛，返回步骤六继续修正动态偏差的估计值的计算，直到它满足收敛条件，即该范数小于或等于设定的门限值。具体是指：

$\left|\right|m\left({\hat{\beta }}_l^{*}\right)\left|\right|\le ϵ,ϵ>0---\left(26\right)$

如果式(26)不成立，返回步骤六重新对估计偏差，直到式(26)成立。最后，将收敛的偏差估计赋值给k时刻的估计偏差即

$\hat{\beta }\left(k\right)={\hat{\beta }}_{\mathrm{conv}}^{*}---\left(27\right)$

图1为本发明实施例中估计传感器的动态偏差的流程图。首先，初始化动态偏差$\hat{\beta }\left(0\right)=0;$由采集的测量数据z(k)与目标的状态方程，根据式(11)和式(17)分别估计目标状态和测量值的估计值在得到一组测量值{z(j)j＝1，...，m}后，根据修正后的均值移动方法公式(22)估计并根据式(26)判断估计偏差是否收敛，如果没有收敛，返回式(22)，继续根据均值移动方法估计偏差；如果估计偏差满足收敛条件，执行式(27)，并进入下一时刻(k+1时刻)的循环。

图2为本实施例中传感器与目标的初始位置分布图。本实施例中使用2个传感器，在图中用小正方形“□”表示。本实施例中高斯均值移动方法只采用一个目标航迹的测量数据，目标用小菱形“◇”表示；本实施例中引证文献采用的是“精确”估计方法，该方法是基于多目标的方法，需要多个目标航迹的测量数据，在图中的用加号“+”表示该方法中的目标。

图3为本实施例中目标航迹比较图。用星号“*”表示的曲线是本实施例方法的仿真曲线，与目标真实航迹吻合，用虚线“--”表示的曲线是引证文献所用的“精确”估计方法的仿真曲线。从图3可以看出，本实施例方法消除了配准偏差的不利影响。

图4和图5分别为本实施例中距离偏差估计的蒙特卡罗仿真比较图与方位角偏差估计的蒙特卡罗仿真比较图。图4和图5中的传感器b₁和传感器b₂的估计偏差与真实偏差基本一致，仿真结果表明本实施例方法可以快速、准确地估计出传感器的动态偏差。

图6为本实施例中距离偏差的均方根误差与克拉美罗下届的比较图。仿真结果显示，本方法可以快速收敛到克拉美罗下届附近，并且在第20个采样周期附近就开始趋于平稳，而引证文献中的方法则在第35个采样周期附近才趋于平稳。与引证文献中的方法相比，本方法的均方根误差由10.59米减小到3.49米，精度提高了67％。

图7为本实施例中方位角偏差的均方根误差与克拉美罗下届的比较图。仿真结果显示，本方法可以快速收敛到克拉美罗下届附近，在第10个采样周期附近本方法的均方根误差就低于引证文献中的方法的均方根误差，并且均方根误差由3.62毫弧减小到2.46毫弧，精度提高了32％。