CN107633256A - 一种多源测距下联合目标定位与传感器配准方法 - Google Patents

Abstract

本发明公开了一种多源测距下联合目标定位与传感器配准方法，通过对多源测距传感器的量测模型进行分析，利用传感器的量测噪声为高斯白噪声的特性来构造似然函数，利用极大似然法对似然函数进行分析、推导并求解，得到目标融合位置和测距传感器的系统偏差估计的解析解。本方法将交互式多模型跟踪算法与极大似然算法相结合，在上一次传感器系统偏差估计值的基础上，使用交互式多模型算法计算得到目标的初始位置状态信息，代入系统偏差的估计公式，得到系统偏差估计值。使用本次计算的系统偏差估计继续代入下一次迭代，通过设置迭代次数或者是一个较小的阈值，得到收敛的系统偏差估计值，最后代入收敛的系统偏差估计计算得到精确的目标位置融合值。

Description

一种多源测距下联合目标定位与传感器配准方法

技术领域

本发明属于导航与定位技术领域，涉及一种多源测距下联合目标定位与传感器配准方法。

背景技术

多传感器空间配准主要是为了消除传感器的系统偏差，从而可以提高对目标的定位精度。目前，针对空间配准的方法主要包括序贯处理方法和批处理方法，序贯处理方法主要有基于滤波技术的在线估计、联合在线估计算法以及解耦在线估计算法，其计算量较小。批处理配准方法主要包括最小二乘法、广义最小二乘法、极大似然法和精确最大似然法等。该类算法需要对一段时间内的数据进行集中处理，因此计算量相对较大。

基于滤波技术的在线估计、联合在线估计算法以及解耦在线估计算法，其主要是将系统偏差当做待估的状态量进行滤波估计求解，通过构造等效量测方程和系统状态方程，进行递推的滤波计算，实时得到系统偏差的估计值。而批处理方法，如最小二乘法、广义最小二乘法和精确最大似然法等，通过某一时间段内的数据进行集中处理，求取本段时间内的最优估计值，计算量随着数据的增多而增大，且其主要是针对同类传感器空间配准。

在多传感器只存在某一量测信息的条件下，如量测信息只有测距信息，上述方法往往无法进行配准或是估计效果不理想，这是因为，在只有测距信息下，无法构造等效量测方程，从而无法进行滤波或是批处理计算偏差，或是信息量较少，在进行滤波估计时导致估计效果较差。

发明内容

为了克服现有技术中存在的问题，本发明的目的是提供一种基于极大似然的多源测距下联合目标定位与传感器配准方法，在多传感器只存在测距信息的环境下，能对传感器系统偏差进行估计求解，得到目标的位置信息。该配准方法相对于其他方法，具有精度更高、适用场景更广等优点。

为了达到上述目的，提供了一种多源测距下联合目标定位与传感器配准方法，包括以下步骤：

步骤1，初始化测距传感器系统偏差估计值β⁰为零， 表示初始化时第n个测距传感器的系统偏差估计值；n取值为自然数；由于无法得到传感器系统偏差值的范围，可将其初始值设置为零，进行迭代计算。

步骤2，计算在上次迭代得到的系统偏差估计值β^l-1(l为已经进行的迭代次数)下，计算各传感器量测时刻k的目标初始位置X⁰(k)， 表示第n个测距传感器量测时刻k的目标初始位置；在测量信息为径向距离、方位角的环境下，目标的初始位置可以通过非线性的量测方程反推求解，而在多源测距环境下，能利用到的信息只有距离信息，无法通过量测方程反推得到目标的初始位置信息。本方法中将交互式多模型算法(IMM)引入来求解目标的初始位置X⁰(k)。具体操作是，在上一次迭代得到的系统偏差估计值β^l-1下，使用多个运动模型拟合目标运动状态，利用所有传感器的测距信息进行并行滤波，从而得到共同目标的位置状态。在知道共同目标位置的条件下，就可以求取在各传感器距离量测下的

步骤3，求取本次迭代的系统偏差估计β^l：利用步骤2得到的目标初始位置计算得到其本次迭代的系统偏差估计值；

步骤4，判断系统偏差估计值是否收敛，即|β^l-β^l-1|≤ε，ε为一较小的正实数，一般取0.01。如果收敛，各个测距传感器的系统偏差估计为β＝[β₁β₂…β_n]^T，执行步骤5，β_n表示第n个测距传感器的系统偏差估计值；如果没有收敛，返回步骤2；

步骤5，求取目标位置：利用收敛的系统偏差估计β，计算得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

本发明步骤2包括如下步骤：

步骤2-1，本发明使用了在多源测距模型下的极大似然算法原理。考虑存在n个测距传感器对共同的一个目标进行量测的场景，各传感器的位置已知，测距传感器的量测模型如下：

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

其中，k＝1,2,…,N，N表示各测距传感器时间采样总数，且量测时间同步，第k时刻测距传感器的量测为z(k)，第k时刻第n个测距传感器的量测信息为径向距离r_n(k)，z(k)＝[z₁(k)z₂(k)…z_n(k)]^T＝[r₁(k)r₂(k)…r_n(k)]^T，h(·)是已知的非线性量测方程，x(k)为第k时刻测距传感器获取的目标真实的位置状态，β为各测距传感器的距离系统偏差，β＝[β₁β₂,…,β_n]，β_n表示第n个测距传感器的距离系统偏差，w(k)为第k时刻测距传感器零均值的高斯噪声，其方差矩阵为n表示第n个测距传感器，各测距传感器的量测噪声均独立；

步骤2-2，通过各测距传感器的量测信息Z＝[z(k)；k＝1,…,N]来求解传感器的测距系统偏差，对量测进行误差消除，从而得到对目标位置的正确估计。从式(1)可知，目标的位置状态x(k)也是未知的，因此需要最大化似然函数p(Z|X,β)来联合估计系统偏差β和目标位置X＝{x(k)；k＝1,…,N}：

根据各测距传感器之间的量测噪声相互独立的假设，并且为了表示方便，省去时间标号k，可得：

式中，K₁表示简化后一常量，与后续的计算无关，z_n表示第n个测距传感器的量测，x(k)表示k时刻目标位置，在假设已知第i个测距传感器的距离系统偏差β_i的条件下，把第i个测距传感器的量测z_i投影到目标状态空间：

x_i表示在第i个测距传感器的量测下目标的状态空间位置；

步骤2-3，在多源测距场景下，只有单一的距离信息是无法将量测z_i投影到目标状态空间的，因为其量测方程是ζⁱ为传感器i的位置，其解不唯一。与为测距传感器i在笛卡尔坐标下的位置(与为传感器的位置坐标)，x₁(k)与x₂(k)为目标在笛卡尔坐标下的位置(x₁(k)与x₂(k)为目标的位置坐标)。此时，关键是求取目标的位置状态，反推目标相对传感器的方位角，再把距离量测z_i投影到目标状态空间。为了求取在所有传感器量测下的目标位置状态，本发明使用了交互式多模型(IMM)目标跟踪算法，通过对各传感器的量测进行并行滤波，使用多模型对目标运动状态进行拟合，可以得到存在系统偏差下的目标位置状态。

由式(4)可知x_i是高斯随机变量通过非线性系统得到的随机变量，可以通过一阶泰勒线性化近似展开得到其概率密度函数近似表达式，其协方差的逆为

其中，

H_i其为h_i(·)在x处的雅可比矩阵，此时似然函数即公式(3)在目标状态空间下近似为：

K₂表示化简过程中的常量，与后续计算无关；

使用矩阵运算公式：

y^TAy-2y^TB+B^TA^-1B＝(y-A^-1B)A(y-A^-1B) (8)

其中，B为矩阵，A为对称矩阵，式(7)可重写为

式中，

步骤2-4，当式(9)中的第一项为零时，即时似然函数存在最大值，式(10)是目标在k时刻状态的最大似然估计，也是此时刻下不同测距传感器量测的融合结果，式(9)中包含了系统偏差的未知参数β。

式(9)中的第二项可重写为：

式中 表示k时刻第n个传感器对目标位置的估计，且：

block-diag表示为括号内各方块矩阵组成的块对角矩阵，{·}_ij表示n×n矩阵中的子矩阵，其中i,j＝1,2,…,n。

步骤2-5，将代入式(9)可得：

式中，K＝1/|2πΣ(k)|^1/2是归一化常量。X⁰(k)表示测距传感器在量测时刻k的目标初始位置。

步骤3包括如下步骤：

步骤3-1，对式(4)进行线性化，假设第i个测距传感器的系统偏差初始值为β_0i，该传感器对目标的状态估计初值为x_0i，可得

式中，上标-L表示矩阵H_i的左逆，对于多传感器系统，有

表示k时刻第n个传感器的量测矩阵的左逆，β_n表示第n个传感器的系偏差；

步骤3-2，式(15)可写为更简洁的方程：

式中，

式中为各传感器对目标的初始状态估计，如上面所提及的，本发明使用了交互式多模型(IMM)目标跟踪算法，通过对各传感器的量测进行并行滤波，使用多模型对目标运动状态进行拟合，可以得到存在系统偏差下的目标位置状态，然后通过反推目标相对各传感器的角度，用量测距离信息得到X₀(k)。β₀＝[β₀₁ β₀₂ …β_0n]^T为各测距传感器的初始系统偏差估计。

步骤3-3，在n个传感器全部采样时刻的量测下，系统偏差的估计为

表示目标在笛卡尔坐标下的位置估计，通过式(8)，(13)，(16)和(19)，可得

步骤3-4，通过矩阵运算公式(9)，式(20)可重写为

式中

式中，C是一与β无关的常量，是归一化常量，均与β的估计无关，为β的最大似然估计。当迭代计算时，为本次迭代的估计值β^l。

步骤5包括：求取目标各量测时刻下的位置其中x_i为式(16)中的各测距传感器对共同目标的估计值，其是利用收敛的系统偏差估计β，代入目标位置估计等式(16)，得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

有益效果：本发明针对多源测距传感器组网定位系统，通过量测方程构造目标状态的似然函数，并使其最大化，求解出各测距传感器的系统偏差值，并最终得到目标的融合位置，本发明所提出的方法能够有效解决多源测距场景下的目标定位和传感器配准问题，并且可以推广到多源测角以及混合测量信息的场景下。仿真结果表明，在传感器只存在测距信息的环境下，本发明提出的方法具有快速的收敛性，有效的解决了传感器系统偏差估计和目标定位问题。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述或其他方面的优点将会变得更加清楚。

图1是基于最大似然的多源测距下联合目标定位与传感器配准流程图。

图2是目标运动航迹图。

图3是真实航迹、配准前IMM滤波航迹和配准融合后航迹对比图。

图4是配准前后目标位置均方根误差曲线图。

图5是各传感器系统偏差估计值曲线图。

图6是目标相对传感器1径向距离真值、传感器1量测值和配准后量测值图。

图7是目标相对传感器2径向距离真值、传感器2量测值和配准后量测值图。

图8是目标相对传感器3径向距离真值、传感器3量测值和配准后量测值图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

本方法的详细流程如图1所示。

步骤1，初始化测距传感器系统偏差估计值β⁰为零， 表示初始化时第n个测距传感器的系统偏差估计值；n取值为自然数；由于无法得到传感器系统偏差值的范围，可将其初始值设置为零，进行迭代计算。

步骤2，计算在上次迭代得到的系统偏差估计值β^l-1(l为已经进行的迭代次数)下，计算各传感器量测时刻k的目标初始位置X⁰(k)， 表示第n个测距传感器量测时刻k的目标初始位置；在测量信息为径向距离、方位角的环境下，目标的初始位置可以通过非线性的量测方程反推求解，而在多源测距环境下，能利用到的信息只有距离信息，无法通过量测方程反推得到目标的初始位置信息。本方法中将交互式多模型算法(IMM)引入来求解目标的初始位置X⁰(k)。具体操作是，在上一次迭代得到的系统偏差估计值β^l-1下，使用多个运动模型拟合目标运动状态，利用所有传感器的测距信息进行并行滤波，从而得到共同目标的位置状态。在知道共同目标位置的条件下，就可以求取在各传感器距离量测下的

步骤3，求取本次迭代的系统偏差估计β^l：利用步骤2得到的目标初始位置计算得到其本次迭代的系统偏差估计值；

步骤4，判断系统偏差估计值是否收敛，即|β^l-β^l-1|≤ε，ε为一较小的正实数，一般取0.01。如果收敛，各个测距传感器的系统偏差估计为β＝[β₁ β₂ … β_n]^T，执行步骤5，β_n表示第n个测距传感器的系统偏差估计值；如果没有收敛，返回步骤2；

步骤5，求取目标位置：利用收敛的系统偏差估计β，计算得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

本发明步骤2包括如下步骤：

步骤2-1，本发明使用了在多源测距模型下的极大似然算法原理。考虑存在n个测距传感器对共同的一个目标进行量测的场景，各传感器的位置已知，测距传感器的量测模型如下：

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

其中，k＝1,2,…,N，N表示各测距传感器时间采样总数，且量测时间同步，第k时刻测距传感器的量测为z(k)，第k时刻第n个测距传感器的量测信息为径向距离r_n(k)，z(k)＝[z₁(k) z₂(k) … z_n(k)]^T＝[r₁(k) r₂(k) … r_n(k)]^T，h(·)是已知的非线性量测方程，x(k)为第k时刻测距传感器获取的目标真实的位置状态，β为各测距传感器的距离系统偏差，β＝[β₁β₂,…,β_n]，β_n表示第n个测距传感器的距离系统偏差，w(k)为第k时刻测距传感器零均值的高斯噪声，其方差矩阵为n表示第n个测距传感器，各测距传感器的量测噪声均独立；

步骤2-2，通过各测距传感器的量测信息Z＝[z(k)；k＝1,…,N]来求解传感器的测距系统偏差，对量测进行误差消除，从而得到对目标位置的正确估计。从式(1)可知，目标的位置状态x(k)也是未知的，因此需要最大化似然函数p(Z|X,β)来联合估计系统偏差β和目标位置X＝{x(k)；k＝1,…,N}：

根据各测距传感器之间的量测噪声相互独立的假设，并且为了表示方便，省去时间标号k，可得：

式中，K₁表示简化后一常量，与后续的计算无关，z_n表示第n个测距传感器的量测，x(k)表示k时刻目标位置，在假设已知第i个测距传感器的距离系统偏差β_i的条件下，把第i个测距传感器的量测z_i投影到目标状态空间：

x_i表示在第i个测距传感器的量测下目标的状态空间位置；

步骤2-3，在多源测距场景下，只有单一的距离信息是无法将量测z_i投影到目标状态空间的，因为其量测方程是ζⁱ为传感器i的位置，其解不唯一。与为测距传感器i在笛卡尔坐标下的位置，x₁(k)与x₂(k)为目标在笛卡尔坐标下的位置。此时，关键是求取目标的位置状态，反推目标相对传感器的方位角，再把距离量测z_i投影到目标状态空间。为了求取在所有传感器量测下的目标位置状态，本发明使用了交互式多模型(IMM)目标跟踪算法，通过对各传感器的量测进行并行滤波，使用多模型对目标运动状态进行拟合，可以得到存在系统偏差下的目标位置状态。

由式(4)可知x_i是高斯随机变量通过非线性系统得到的随机变量，可以通过一阶泰勒线性化近似展开得到其概率密度函数近似表达式，其协方差的逆为

其中，

H_i其为h_i(·)在x处的雅可比矩阵，此时似然函数即公式(3)在目标状态空间下近似为：

K₂表示化简过程中的常量，与后续计算无关；

使用矩阵运算公式：

y^TAy-2y^TB+B^TA^-1B＝(y-A^-1B)A(y-A^-1B) (8)

其中，B为矩阵，A为对称矩阵，式(7)可重写为

式中，

步骤2-4，当式(9)中的第一项为零时，即时似然函数存在最大值，式(10)是目标在k时刻状态的最大似然估计，也是此时刻下不同测距传感器量测的融合结果，式(9)中包含了系统偏差的未知参数β。

式(9)中的第二项可重写为：

式中 表示k时刻第n个传感器对目标位置的估计，且：

block-diag表示为括号内各方块矩阵组成的块对角矩阵，{·}_ij表示n×n矩阵中的子矩阵，其中i,j＝1,2,…,n。

步骤2-5，将代入式(9)可得：

式中，K＝1/|2πΣ(k)|^1/2是归一化常量。X⁰(k)表示测距传感器在量测时刻k的目标初始位置。

步骤3包括如下步骤：

步骤3-1，对式(4)进行线性化，假设第i个测距传感器的系统偏差初始值为β_0i，该传感器对目标的状态估计初值为x_0i，可得

式中，上标-L表示矩阵H_i的左逆，对于多传感器系统，有

表示k时刻第n个传感器的量测矩阵的左逆，β_n表示第n个传感器的系偏差；

步骤3-2，式(15)可写为更简洁的方程：

式中，

式中为各传感器对目标的初始状态估计，如上面所提及的，本发明使用了交互式多模型(IMM)目标跟踪算法，通过对各传感器的量测进行并行滤波，使用多模型对目标运动状态进行拟合，可以得到存在系统偏差下的目标位置状态，然后通过反推目标相对各传感器的角度，用量测距离信息得到X₀(k)。β₀＝[β₀₁ β₀₂ …β_0n]^T为各测距传感器的初始系统偏差估计。

步骤3-3，在n个传感器全部采样时刻的量测下，系统偏差的估计为

表示目标在笛卡尔坐标下的位置估计，通过式(8)，(13)，(16)和(19)，可得

步骤3-4，通过矩阵运算公式(9)，式(20)可重写为

式中

式中，C是一与β无关的常量，是归一化常量，均与的估计无关，为β的最大似然估计。当迭代计算时，为本次迭代的估计值β^l。

步骤5包括：求取目标各量测时刻下的位置其中x_i为式(16)中的各测距传感器对共同目标的估计值，其是利用收敛的系统偏差估计β，代入目标位置估计等式(16)，得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

仿真分析：

在二维坐标系下，存在三个传感器对一个目标仅进行距离量测，三个传感器的坐标分别为：传感器1位置ζ¹＝(0,0)，传感器2位置ζ²＝(0,20000m)，传感器3位置ζ³＝(20000m,0)，其系统偏差分别为β₁＝500m，β₂＝-500m，β₃＝500m，测距噪声标准差均为σ_r＝10m。目标做变模型运动，初始状态为(3000m；60m/s；5000m；20m/s)，过程噪声标准差为I为单位矩阵。传感器的扫描周期为1s，仿真220个扫描周期。其中T₁＝50为匀速直线运动，T₂＝20为匀加速运动，T₃＝120为匀加速运动，T₄＝30为匀速直线运动，目标运动航迹如图2所示

在交互式多模型中，使用三个模型来进行跟踪，一个匀速(CV)，两个匀速拐弯(CT)，Markov转移概率矩阵设置为初始模型概率设置为[0.75 0.150.15]。

从图3、图4可以看出，配准前目标位置误差在700m左右，配准后位置误差在10m左右，精度上得到了很大程度上的提高。

从图5为传感器系统偏差估计值与真值对比，可以看出，经过迭代计算后的系统偏差趋于真值。

图6、图7、图8为传感器相对目标径向距离真值、传感器量测值和配准后的量测值，可以看出，配准后的量测值与真值非常接近，表明了系统偏差估计值与真实偏差接近。

本发明提供了一种多源测距下联合目标定位与传感器配准方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (5)

1.一种多源测距下联合目标定位与传感器配准方法，其特征在于，包括以下步骤：

步骤1，初始化测距传感器系统偏差估计值β⁰为零， 表示初始化时第n个测距传感器的系统偏差估计值；

步骤2，在l-1次迭代得到的系统偏差估计值β^l-1下，l为当前进行的迭代次数，计算各测距传感器量测时刻k的目标初始位置X⁰(k)， 表示第n个测距传感器量测时刻k的目标初始位置；

步骤3，求取本次迭代的系统偏差估计β^l：利用步骤2得到的目标初始位置计算得到其本次迭代的系统偏差估计值；

步骤4，判断系统偏差估计值是否收敛，即|β^l-β^l-1|≤ε，ε为一较小的正实数，如果收敛，各个测距传感器的系统偏差估计为β＝[β₁ β₂ … β_n]^T，执行步骤5，β_n表示第n个测距传感器的系统偏差估计值；如果没有收敛，返回步骤2；

步骤5，求取目标位置：利用收敛的系统偏差估计β，计算得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。

2.根据权利要求1所述的方法，其特征在于，步骤2包括如下步骤：

步骤2-1，测距传感器的量测模型如下：

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

其中，k＝1,2,…,N，N表示各测距传感器时间采样总数，且量测时间同步，第k时刻测距传感器的量测为z(k)，第k时刻第n个测距传感器的量测信息为径向距离r_n(k)，z(k)＝[z₁(k) z₂(k) … z_n(k)]^T＝[r₁(k) r₂(k) … r_n(k)]^T，h(·)是已知的非线性量测方程，x(k)为第k时刻测距传感器获取的目标真实的位置状态，β为各测距传感器的距离系统偏差，β＝[β₁ β₂,…,β_n]，β_n表示第n个测距传感器的距离系统偏差，w(k)为第k时刻测距传感器零均值的高斯噪声，其方差矩阵为n表示第n个测距传感器，各测距传感器的量测噪声均独立；

步骤2-2，通过各测距传感器的量测信息Z求解测距传感器的测距系统偏差，对量测进行误差消除，Z＝[z(k)；k＝1,…,N]，从而得到对目标位置的正确估计：通过最大化似然函数p(Z|X,β)来联合估计距离系统偏差β和目标位置X＝{x(k)；k＝1,…,N}：

<mrow> <mtable> <mtr> <mtd> <mrow> <mo>{</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>&amp;beta;</mi> <mo>^</mo> </mover> <mo>}</mo> <mo>=</mo> <munder> <mrow> <mi>arg</mi> <mi>max</mi> </mrow> <mrow> <mi>X</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </munder> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mn>1</mn> <mo>)</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mn>2</mn> <mo>)</mo> <mo>...</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mi>N</mi> <mo>)</mo> <mo>|</mo> <mi>X</mi> <mo>,</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <munder> <mrow> <mi>arg</mi> <mi>max</mi> </mrow> <mi>&amp;beta;</mi> </munder> <mo>{</mo> <munderover> <mo>&amp;Pi;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>max</mi> <mi> </mi> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>|</mo> <mi>x</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>,</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

根据各测距传感器之间的量测噪声相互独立的假设，省去时间标号k，得到：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>,</mo> <mn>...</mn> <mo>,</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>|</mo> <mi>x</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mi>&amp;Pi;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mi>p</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mi>i</mi> </msub> <mo>|</mo> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>&amp;beta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mi>exp</mi> <mrow> <mo>{</mo> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msup> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>z</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> </mrow> <mo>)</mo> </mrow> </mrow> <mo>}</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>

式中，K₁表示简化后一常量，z_n表示第n个测距传感器的量测，x(k)表示k时刻目标位置，在假设已知第i个测距传感器的距离系统偏差β_i的条件下，把第i个测距传感器的量测z_i投影到目标状态空间：

<mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>=</mo> <msubsup> <mi>h</mi> <mi>i</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>&amp;beta;</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>n</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>

x_i表示在第i个测距传感器的量测下目标的状态空间位置；

步骤2-3，量测z_i的量测方程是 与为测距传感器i在笛卡尔坐标下的位置，x₁(k)与x₂(k)为目标在笛卡尔坐标下的位置，由式(4)得知x_i是高斯随机变量通过非线性系统得到的随机变量，通过一阶泰勒线性化近似展开得到其概率密度函数近似表达式，其协方差的逆为：

<mrow> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>H</mi> <mi>i</mi> <mi>T</mi> </msubsup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>z</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>H</mi> <mi>i</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

其中，

H_i为h_i(·)在x处的雅可比矩阵，此时似然函数即公式(3)在目标状态空间下近似为：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>,</mo> <mn>...</mn> <mo>,</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>|</mo> <mi>x</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <msub> <mi>K</mi> <mn>2</mn> </msub> <mi>exp</mi> <mrow> <mo>{</mo> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>}</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msub> <mi>K</mi> <mn>2</mn> </msub> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>(</mo> <msup> <mi>x</mi> <mi>T</mi> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>x</mi> <mo>-</mo> <mn>2</mn> <msup> <mi>x</mi> <mi>T</mi> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>x</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>x</mi> <mi>i</mi> <mi>T</mi> </msubsup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> <mo>)</mo> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>

K₂表示化简过程中的常量；

使用矩阵运算公式：

y^TAy-2y^TB+B^TA^-1B＝(y-A^-1B)A(y-A^-1B) (8)

其中，B为矩阵，A为对称矩阵，式(7)重写为：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>,</mo> <mn>...</mn> <mo>,</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>|</mo> <mi>x</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <msub> <mi>K</mi> <mn>2</mn> </msub> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>(</mo> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>x</mi> <mi>i</mi> <mi>T</mi> </msubsup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>&amp;times;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> <mo>)</mo> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

其中，

步骤2-4，当式(9)中的第一项为零时，即时似然函数存在最大值，式(10)是目标在k时刻状态的最大似然估计，也是此时刻下不同测距传感器量测的融合结果，式(9)中包含了距离系统偏差β，式(9)中的第二项重写为：

<mrow> <mtable> <mtr> <mtd> <mrow> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>x</mi> <mi>i</mi> <mi>T</mi> </msubsup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msup> <mi>X</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

其中， 表示k时刻第n个传感器对目标位置的估计，且：

<mrow> <mtable> <mtr> <mtd> <mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>b</mi> <mi>l</mi> <mi>o</mi> <mi>c</mi> <mi>k</mi> <mo>-</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mn>1</mn> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mn>2</mn> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>n</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mrow> <mo>&amp;lsqb;</mo> <msub> <mrow> <mo>{</mo> <mrow> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>i</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mrow> <mo>&amp;lsqb;</mo> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>l</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>l</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>&amp;Sigma;</mi> <msub> <mi>x</mi> <mi>j</mi> </msub> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>}</mo> </mrow> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>

block-diag表示为括号内各方块矩阵组成的块对角矩阵，{·}_ij表示n×n矩阵中的子矩阵，其中i,j＝1,2,…,n；

步骤2-5，将代入式(9)得到：

<mrow> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>|</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>,</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>K</mi> <mi> </mi> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mi>X</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>

其中，K是归一化常量，X⁰(k)表示测距传感器在量测时刻k的目标初始位置。

3.根据权利要求2所述的方法，其特征在于，步骤2-5中，K＝1/|2πΣ(k)|^1/2。

4.根据权利要求3所述的方法，其特征在于，步骤3包括如下步骤：

步骤3-1，对式(4)进行线性化，假设第i个测距传感器的系统偏差初始值为β_0i，该测距传感器对目标的状态估计初值为x_0i，得到：

<mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mn>0</mn> <mi>i</mi> </mrow> </msub> <mo>&amp;ap;</mo> <msubsup> <mi>H</mi> <mi>i</mi> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mo>&amp;CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>&amp;beta;</mi> <mrow> <mn>0</mn> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>&amp;beta;</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>

其中，上标-L表示矩阵H_i的左逆，对于多传感器系统，有：

<mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>01</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>02</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mn>0</mn> <mi>n</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mn>1</mn> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mn>01</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mn>2</mn> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mn>02</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mi>n</mi> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mrow> <mn>0</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mn>1</mn> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mn>2</mn> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mtable> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>H</mi> <mi>n</mi> <mrow> <mo>-</mo> <mi>L</mi> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mi>n</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

表示k时刻第n个传感器的量测矩阵的左逆，β_n表示第n个传感器的系偏差；

步骤3-2，将式(15)简化为如下方程

<mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>Q</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>&amp;beta;</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>

其中，

<mrow> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>Q</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;beta;</mi> <mn>0</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>

为各测距传感器对目标的初始状态估计，β₀＝[β₀₁β₀₂ … β_0n]^T为各测距传感器的初始系统偏差估计；

步骤3-3，在n个测距传感器全部采样时刻的量测下，系统偏差的估计为：

<mrow> <mover> <mi>&amp;beta;</mi> <mo>^</mo> </mover> <mo>=</mo> <munder> <mi>argmax</mi> <mi>&amp;beta;</mi> </munder> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mn>1</mn> <mo>)</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mn>2</mn> <mo>)</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mi>N</mi> <mo>)</mo> <mo>|</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mo>,</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>

表示目标在笛卡尔坐标下的位置估计，通过式(8)，(13)，(16)和(19)，得到：

<mrow> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mn>1</mn> <mo>)</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mn>2</mn> <mo>)</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>z</mi> <mo>(</mo> <mi>N</mi> <mo>)</mo> <mo>|</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mo>,</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Pi;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>K</mi> <mi> </mi> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mi>X</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>

步骤3-4，通过矩阵运算公式(9)，式(20)重写为：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <mrow> <mi>z</mi> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>,</mo> <mi>z</mi> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>z</mi> <mrow> <mo>(</mo> <mi>N</mi> <mo>)</mo> </mrow> <mo>|</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mover> <mi>K</mi> <mo>&amp;OverBar;</mo> </mover> <mi>exp</mi> <mo>{</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>(</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>&amp;beta;</mi> <mo>-</mo> <mover> <mi>&amp;beta;</mi> <mo>^</mo> </mover> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>)</mo> <mo>&amp;lsqb;</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mi>Q</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mrow> <mo>(</mo> <mi>&amp;beta;</mi> <mo>-</mo> <mover> <mi>&amp;beta;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>+</mo> <mi>C</mi> <mo>}</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

其中，

<mrow> <mover> <mi>&amp;beta;</mi> <mo>^</mo> </mover> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mi>Q</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>Q</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>&amp;lsqb;</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mi>Q</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;Sigma;</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mover> <mi>X</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

其中，C是与β无关的常量，是归一化常量，均与β的估计无关，为β的最大似然估计，当迭代计算时，为本次迭代的系统偏差估计值β^l。

5.根据权利要求4所述的方法，其特征在于，步骤5包括：求取目标各量测时刻下的位置其中x_i为式(16)中的各测距传感器对共同目标的估计值，其是利用收敛的系统偏差估计β，代入目标位置估计等式(16)，得到其最大似然估计，即不同测距传感器在相同量测时刻下的融合位置。