CN104713560A - 基于期望最大化的多源测距传感器空间配准方法 - Google Patents

Abstract

本发明提供一种基于期望最大化的多源测距传感器空间配准方法，针对利用多源测距传感器进行目标定位的问题，通过目标的状态方程和量测方程，构造似然函数，对似然函数求取期望，并使其最大化，可以得到隐含的系统偏差参数估计值，在追求实时性、更小的数据存储空间和计算耗时情况下，可以使用滑窗期望最大化算法。仿真结果表明，本发明提出的方法具有抗噪声更强，所需数据较少，稳定性、精度更高的优点。

Description

基于期望最大化的多源测距传感器空间配准方法

技术领域

本发明属于导航与定位技术领域，涉及在多个传感器只有测距信息条件下的定位方法，具体涉及一种基于期望最大化(Expectation Maximization，EM)算法的多源测距传感器空间配准方法。

背景技术

多传感器空间配准主要是为了消除传感器的系统偏差。目前，针对空间配准的方法主要包括序贯处理方法和批处理方法，序贯处理方法主要是基于扩展卡尔曼滤波、无味滤波的实时估计方法，其计算量较小。批处理配准方法主要包括最小二乘法、广义最小二乘法、极大似然法和精确最大似然法等。该类算法需要对一段时间内的数据进行集中处理，因此计算量相对比较大。

扩展卡尔曼滤波、无味滤波的实时估计方法，其主要是将系统偏差当做待估的状态量进行滤波估计求解，通过构造等效量测方程和系统状态方程，进行递推的滤波算法，实时得到系统偏差的估计值。扩展卡尔曼滤波是将非线性的状态方程和量测方程进行泰勒展开，其实质上是一种在线线性化的算法，即按名义轨线进行线性化处理，再利用卡尔曼滤波公式进行计算。无味滤波的方法是为了改善非线性问题进行滤波的效果而提出来的，该方法在处理状态方程时首先进行了无味(unscented)变换，然后使用无味变换后的状态变量进行滤波估计，以减少估计误差。

传统的批处理方法，如最小二乘法、广义最小二乘法、极大似然法和精确最大似然法等，通过某一时间段内的数据进行集中处理，求取本段时间内的最优估计值，计算量随着数据的增多而增大，且其主要是针对同类传感器空间配准。在多传感器只存在某一量测信息的条件下，如量测信息只有测距信息，上述方法往往无法进行配准或是配准效果不理想。

发明内容

为了克服上述现有技术中存在的问题，本发明的目的在于提供一种基于期望最大化的多源测距传感器空间配准方法。

为了达到上述目的，本发明所采用的技术方案是：

基于期望最大化的多源测距传感器空间配准方法，该空间配准方法包括以下步骤：

根据批处理期望最大化算法或滑窗期望最大化算法，通过目标的状态方程和量测方程构造似然函数，对似然函数求取期望，并使期望最大化，得到隐含的系统偏差参数估计值，从而实现多源测距下的空间配准以及目标准确定位。

所述空间配准方法具体包括以下步骤(批处理期望最大化算法)：

1)初始化迭代扩展卡尔曼平滑一轮计算过程中需要的平滑值，该平滑值在第一轮计算过程中通过在估计偏差为零的条件下使用传感器全部量测数据进行一次扩展卡尔曼滤波得到，该平滑值在后续计算过程中使用上一轮计算过程得到的系统偏差估计值计算得到；

2)经过步骤1)后，使用传感器全部量测数据进行所述迭代扩展卡尔曼平滑一轮计算过程中的滤波和平滑过程，得到计算期望时所需的目标状态和协方差；

3)经过步骤2)后，进行期望最大化的求解，得到本轮计算过程的系统偏差估计值；

4)重复步骤1)至步骤3)，在设定的迭代次数或者收敛阈值的条件下，得到一个收敛的系统偏差估计值。

所述空间配准方法具体包括以下步骤(滑窗期望最大化算法)：

1)初始化迭代扩展卡尔曼平滑在当前时间窗口内需要的平滑值，该平滑值在第一个时间窗口对应的迭代扩展卡尔曼平滑计算过程中通过在估计偏差为零的条件下使用该时间窗口内传感器量测数据进行一次扩展卡尔曼滤波得到，该平滑值在后续时间窗口对应的迭代扩展卡尔曼平滑计算过程中使用在上一个时间窗口计算得到的最终系统偏差估计值计算得到；

2)经过步骤1)后，在当前时间窗口内进行迭代扩展卡尔曼平滑一轮计算过程中的滤波和平滑过程，得到计算期望时所需的目标状态和协方差，然后进行期望最大化的求解，得到系统偏差估计值，以该系统偏差估计值计算在当前时间窗口下一轮迭代扩展卡尔曼平滑过程中需要的平滑值；

3)重复步骤2)，在设定的迭代次数或者是收敛阈值的条件下得到当前窗口最终的系统偏差估计值；

4)重复步骤2)至步骤3)，依次计算后续时间窗口对应的系统偏差估计值。

所述似然函数期望表示为：

$\begin{array}{c}Q(\mathrm{\η},{\widehat{\mathrm{\η}}}^{\left(l\right)})=-\frac{1}{2}\mathrm{Tr}\left\{Q^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(A-B-B^T+C)\right\}-\\ \frac{1}{2}\mathrm{Tr}\left\{R^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(H_k^T{P}_{k|N}{H}_k+D-E-E^T+G)\right\}\end{array}$

其中，Tr表示矩阵求迹运算，Q为目标过程噪声方差阵，R为量测噪声方差阵，N为量测采样个数，H_k为k时刻的雅克比矩阵，P_k|N为k时刻的协方差矩阵， $A=P_{k|N}+{\hat{X}}_{k|N}{\hat{X}}_{k|N}^T,B=P_{k,k-1|N}{F}^T+{\hat{X}}_{k|N}{\hat{X}}_{k-1|N}^T{F}^T,C=F{P}_{k-1|N}{F}^T+F{\hat{X}}_{k-1|N}{\hat{X}}_{k-1|N}^T{F}^T,$ $D=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]{[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]}^T,E=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]{[\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)}]}^T{H}_{\mathrm{\η}},$ F为已知的目标状态转移矩阵，H_η为量测矩阵对系统偏差的求导，为上一次迭代得到的系统偏差估计值，z_k为k时刻多源测距传感器的量测，h(·)为已知的量测矩阵，为k时刻目标状态的平滑值。

所述期望最大化的解析解表示为：

${\widehat{\mathrm{\η}}}^{\left(l\right)}={\widehat{\mathrm{\η}}}^{(l-1)}+{\left[\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}{H}_{\mathrm{\η}}^T\right]}^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]$

其中，为上一次迭代得到的系统偏差估计值，为本次迭代得到的系统偏差估计值，H_η为量测矩阵对系统偏差的求导，z_k为k时刻多源测距传感器的量测，h(·)为已知的量测矩阵，为k时刻目标状态的平滑值，N为量测采样个数。

本发明的有益效果是：

本发明针对多源测距传感器组网定位系统进行空间配准和定位，通过目标状态方程和量测方程构造似然函数对数，求取似然函数的期望，并使其最大化，求解出隐含在量测中的系统偏差值，在多传感器只存在测距信息的条件下，能对系统偏差进行估计求解，从而提高对目标的估计精度。在引入滑窗处理后，实时性得到提高，存储空间和时间代价得到了有效的减少。总体而言，本发明所提出的配准方法具有抗噪声强，所需数据较少，稳定性较高的特点。

附图说明

图1是多源测距传感器误差配准的几何关系示意图。

图2是批处理EM算法的实现流程图。

图3是迭代卡尔曼滤波平滑器(IEKS)结构图。

图4是滑窗EM算法结构图。

图5是滑窗EM算法流程图。

图6是仿真场景中传感器与目标分布图。

图7是三种方法对目标航迹估计的RMSE随采样步数(discrete time instant)的变化曲线。

图8为系统偏差估计结果；其中，(a)～(e)给出了滑窗EM对系统偏差在不同时刻下的估计，(a)传感器1，(b)传感器2，(c)传感器3，(d)传感器4，(e)传感器5；(f)～(j)给出了各传感器系统偏差的估计曲线随迭代步数的变化，(f)传感器1，(g)传感器2，(h)传感器3，(i)传感器4，(j)传感器5。

具体实施方式

下面结合附图和实施例对本发明作详细说明。

基于期望最大化的多源测距传感器空间配准算法，包括以下步骤：

第一步，初始化第一轮的平滑值。本空间配准算法针对多源测距定位环境下的传感器配准和目标定位，如图1所示，该算法将迭代卡尔曼滤波平滑器(IEKS)嵌入到EM算法，而IEKS算法需要一个初始化的状态值和协方差。在整个空间配准算法流程中，滤波、平滑、EM求解依次进行，在滤波过程中需要用到上一轮的平滑值。在实际情况当中，目标的状态是不可以通过量测直接得到的，因为多传感器系统的量测信息只有距离信息。因此，为了得到第一轮的平滑值以此用来进行下一轮的滤波，可以采用量测信息在系统偏差估计为零的情况下进行一次扩展卡尔曼滤波，而目标初始速度可以通过两点法得到，初始位置可以通过最小二乘迭代求解得到。如此，经过一轮的滤波，可以得到目标第一轮的平滑值。

第二步，进行IEKS中的滤波。IEKS算法是对应于EKS的迭代平滑过程，它通过迭代使用EKS算法来获得状态的极大后验估计，每次迭代都是在前一次迭代的平滑值处线性化。由于扩展卡尔曼滤波算法主要应用于非线性系统，利用泰勒公式线性化的过程给系统引入了舍入误差，而IEKS算法通过逐步迭代可以减小这种误差，因此EM算法中的平滑值可以通过IEKS算法求得。

下面介绍一下期望最大化算法的原理，定义条件概率p(Z|η)，其中η为待估偏差，Z＝{z₁,z₂,…,z_N}为量测数据集，N表示传感器量测采样个数。对η的极大似然估计MLE即为：

$\widehat{\mathrm{\η}}=\arg {\max }_{\mathrm{\η}}\log p\left(Z\right|\mathrm{\η})---\left(1\right)$

对式(1)而言，由于Z为不完全数据集，logp(Z|η)及其最大值很难直接求解。解决的方法是假设存在一个不可逆变换F，实现从完全数据集到不完全数据集的一种映射，即F:C→Z，C＝{X,Z}为完全数据集，X表示缺失数据。根据EM算法的原理，式(1)可以替换为：

$\widehat{\mathrm{\η}}=\arg {\max }_{\mathrm{\η}}\log p\left(C\right|\mathrm{\η})---\left(2\right)$

由于C为完全数据集，相对于式(1)而言，式(2)中似然函数的求解和最大化得到了很大的简化。在算法中将目标状态作为缺失数据X，即X＝{X₁,X₂,…,X_N}。

批处理EM算法通过迭代计算实现，每一个迭代周期由以下两步组成：E-step： 为第l次迭代的偏差估计值；M-step： ${\widehat{\mathrm{\η}}}^{(l+1)}=\arg {\max }_{\mathrm{\η}}Q(\mathrm{\η},{\widehat{\mathrm{\η}}}^{\left(l\right)}).$

E-step是在已知测量数据Z和上次迭代所得参数的条件下，计算条件期望在这一步可以得到缺失数据即目标状态X的估计值；M-step是极大化步，计算关于系统偏差的极大似然估计因此EM算法可以同时估计目标状态和系统偏差，且目标状态和系统偏差是在两步中分离估计的。

下面针对传感器系统偏差的估计问题，对EM算法的两个步骤作详细推导：

1)E-step：

$\begin{array}{c}\log p\left(C\right|\mathrm{\η})=\log p(X,Z|\mathrm{\η})\\ =\log \left\{p\left(Z\right|X,\mathrm{\η})p\left(X\right)\right\}\\ =\log p\left(X_0\right)+\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\log p\left(X_k\right|X_{k-1})+\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\log p\left(z_k\right|X_k,\mathrm{\η})\end{array}---\left(3\right)$

假设目标的初始状态X₀服从高斯分布，则：

$p\left(X_0\right)=\frac{1}{\sqrt{{\left(2\mathrm{\π}\right)}^m\left|P_0\right|}}\exp \{-\frac{1}{2}{(X_0-{\stackrel{\‾}{X}}_0)}^T{P}_0^{-1}(X_0-{\stackrel{\‾}{X}}_0)\}---\left(4\right)$

式中，m表示目标状态矢量的维数。

从目标的状态方程可得：

$p\left(X_k\right|X_{k-1})=\frac{1}{\sqrt{{\left(2\mathrm{\π}\right)}^m\left|Q\right|}}\exp \{-\frac{1}{2}{(X_k-F{X}_{k-1})}^T{Q}^{-1}(X_k-F{X}_{k-1})\}---\left(5\right)$

式中，Q表示目标过程噪声方差矩阵，F表示已知的目标状态转移矩阵。

从量测方程可得：

$p\left(z_k\right|X_k,\mathrm{\η})=\frac{1}{\sqrt{{\left(2\mathrm{\π}\right)}^d\left|R\right|}}\exp \{-\frac{1}{2}{(z_k-h(X_k,\mathrm{\η}))}^T{R}^{-1}(z_k-h(X_k,\mathrm{\η}))\}---\left(6\right)$

式中，d表示量测向量的维数。R为量测噪声方差阵，h(·)为已知的量测矩阵。

将式(4)、(5)、(6)带入式(3)中，忽略常数项，得似然函数为：

$\begin{array}{c}\log p\left(C\right|\mathrm{\η})=-\frac{1}{2}\log \left|P_0\right|-\frac{N}{2}\log \left|Q\right|-\frac{N}{2}\log \left|R\right|-\\ \frac{1}{2}{(X_0-{\stackrel{\‾}{X}}_0)}^T{P}_0^{-1}(X_0-{\stackrel{\‾}{X}}_0)-\\ \frac{1}{2}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{(X_k-F{X}_{k-1})}^T{Q}^{-1}(X_k-F{X}_{k-1})-\\ \frac{1}{2}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{(z_k-h(X_k,\mathrm{\η}))}^T{R}^{-1}(z_k-h(X_k,\mathrm{\η}))\end{array}---\left(7\right)$

由于式(7)中目标初始状态、过程噪声和量测噪声已知，为简单起见，省略掉已知项，求取期望可得：

$\begin{array}{c}Q(\mathrm{\η},{\widehat{\mathrm{\η}}}^{\left(l\right)})=-\frac{1}{2}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}E\left[{(X_k-F{X}_{k-1})}^T{Q}^{-1}(X_k-F{X}_{k-1})\right|Z]-\\ \frac{1}{2}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}E\left\{{[z_k-h(X_k,\mathrm{\η})]}^T{R}^{-1}\right[z_k-h(X_k,\mathrm{\η})\left]\right|Z,\mathrm{\η}\}\\ =-\frac{1}{2}\mathrm{Tr}\left\{Q^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}E\right[(X_k-F{X}_{k-1}){(X_k-F{X}_{k-1})}^T\left|Z\right]\}-\\ \frac{1}{2}\mathrm{Tr}\left\{R^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}E\right[(z_k-h(X_k,\mathrm{\η})){(z_k-h(X_k,\mathrm{\η}))}^T|Z,\mathrm{\η}]\}\\ =-\frac{1}{2}\mathrm{Tr}\left(Q^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_1\right)-\frac{1}{2}\mathrm{Tr}\left(R^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ϵ}}_2\right)\end{array}---\left(8\right)$

式中，Tr表示矩阵的求迹运算，E表示求取期望。

$\begin{array}{c}{\mathrm{\ϵ}}_1=E\left[(X_k-F{X}_{k-1}){(X_k-F{X}_{k-1})}^T\right|Z]\\ =E\left(X_k{X}_k^T\right|Z)+F\·E\left(X_{k-1}{X}_{k-1}^T\right|Z)\·F^T-\\ E\left(X_k{X}_{k-1}^T\right|Z)\·F^T-F\·E\left(X_{k-1}{X}_k^T\right|Z)\end{array}---\left(9\right)$

令 ${\tilde{X}}_k=X_k-{\hat{X}}_{k|N},$ 从而有 ${\tilde{X}}_k\⊥{\hat{X}}_{k|N},$ 即 $E({\tilde{X}}_k,{\hat{X}}_{k|N})=0,$ 式(9)可进一步推导为：

$\begin{array}{c}{\mathrm{\ϵ}}_1=E\left[({\tilde{X}}_k+{\hat{X}}_{k|N}){({\tilde{X}}_k+{\hat{X}}_{k|N})}^T\right|Z]+\\ F\·E\left(({\tilde{X}}_{k-1}+{\hat{X}}_{k-1|N}){({\tilde{X}}_{k-1}+{\hat{X}}_{k-1|N})}^T\right|Z)\·F^T-\\ E\left(({\tilde{X}}_k+{\hat{X}}_{k|N}){({\tilde{X}}_{k-1}+{\hat{X}}_{k-1|N})}^T\right|Z)\·F^T-\\ F\·E\left(({\tilde{X}}_{k-1}+{\hat{X}}_{k-1|N}){({\tilde{X}}_k+{\hat{X}}_{k|N})}^T\right|Z)\\ =A-B-B^T+C\end{array}---\left(10\right)$

式中， $A=P_{k|N}+{\hat{X}}_{k|N}{\hat{X}}_{k|N}^T,B=P_{k,k-1|N}{F}^T+{\hat{X}}_{k|N}{\hat{X}}_{k-1|N}^T{F}^T,$ P_k,k-1|N为k-1时刻协方差的一步预测；

利用泰勒公式对式(8)中等号右边第二部分展开，可得：

$\begin{array}{c}{z}_k-h(X_k,\mathrm{\η})\≈z_k-h({\hat{X}}_{k|N},\mathrm{\η})-H_k^T(X_k-{\hat{X}}_{k|N})\\ \≈z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})-H_{\mathrm{\η}}^T(\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)})-H_k^T(X_k-{\hat{X}}_{k|N})\end{array}---\left(11\right)$

式中，

$H_k=\frac{\∂h(X,\mathrm{\η})}{\∂X^T}{|}_{X={\hat{X}}_{k|N},\mathrm{\η}={\widehat{\mathrm{\η}}}^{(l-1)}}---\left(12\right)$

$H_{\mathrm{\η}}=\frac{\∂h(X,\mathrm{\η})}{\∂{\mathrm{\η}}^T}{|}_{X={\hat{X}}_{k|N},\mathrm{\η}={\widehat{\mathrm{\η}}}^{(l-1)}}---\left(13\right)$

从而可得：

$\begin{array}{c}{\mathrm{\ϵ}}_2=E\left[(z_k-h(X_k,\mathrm{\η})){(z_k-h(X_k,\mathrm{\η}))}^T\right|Z,\mathrm{\η}]\\ =H_k^T{P}_{k|N}{H}_k+D-E-E^T+G\end{array}---\left(14\right)$

式中，

$D=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]{[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]}^T---\left(15\right)$

$E=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]{\left[\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)}\right]}^T{H}_{\mathrm{\η}}---\left(16\right)$

$G=H_{\mathrm{\η}}^T[\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)}]{\left[\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)}\right]}^T{H}_{\mathrm{\η}}---\left(17\right)$

将(10)和式(14)代入式(8)中，可得：

$\begin{array}{c}Q(\mathrm{\η},{\widehat{\mathrm{\η}}}^{\left(l\right)})=-\frac{1}{2}\mathrm{Tr}\left\{Q^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(A-B-B^T+C)\right\}-\\ \frac{1}{2}\mathrm{Tr}\left\{R^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(H_k^T{P}_{k|N}{H}_k+D-E-E^T+G)\right\}\end{array}---\left(18\right)$

式中状态估计及协方差阵P_k|N可以通过迭代扩展卡尔曼平滑(IEKS)算法求得。

2)M-step：在这一步中，可以通过极大化式(18)，求得待估参数的估计值。

由以上分析可知，计算条件期望需要知道目标的状态及协方差。通过IEKS算法便可得到系统的状态及协方差，公式如(19)，其流程如图3所示。

其包含前向滤波器(k＝1,2,…,N)：

$\begin{array}{c}{\mathrm{\ϵ}}_k^{\left(l\right)}=z_k-h\left(x_{k/l-1}\right)-H_k\left(x_{k/l-1}\right)(x_{k/k-1}^{\left(l\right)}-x_{k/l-1})\\ P_{k/k}^{\left(l\right)}=P_{k/k-1}^{\left(l\right)}-P_{k/k-1}^{\left(l\right)}{H}_k{\left(x_{k/l-1}\right)}^T{(H_k\left(x_{k/l-1}\right)P_{k/k-1}^{\left(l\right)}{H}_k{\left(x_{k/l-1}\right)}^T+R)}^{-1}{H}_k\left(x_{k/l-1}\right)P_{k/k-1}^{\left(l\right)}\\ x_{k/k}^{\left(l\right)}=x_{k/k-1}^{\left(l\right)}+P_{k/k}^{\left(l\right)}{H}_k{\left(x_{k/l-1}\right)}^T{R}^{-1}{\mathrm{\ϵ}}_k^{\left(l\right)}\\ x_{k+1/k}^{\left(l\right)}={\mathrm{Fx}}_{k/l-1}+F(x_{k/k}^{\left(l\right)}-x_{k/l-1})\\ P_{k+1/k}^{\left(l\right)}=F{P}_{k/k}^{\left(l\right)}{F}^T+Q\end{array}---\left(19\right)$

T表示矩阵的转置。

到此可得到计算期望时所需的目标状态和协方差，每一次的滤波都需要上一轮的平滑值，这也是上述需要对第一轮平滑值进行初始化的原因。扩展卡尔曼滤波得到的目标状态值以及协方差是实时的估计，而由上可知，在进行批处理的EM算法时，所要求取的条件期望是基于所有的量测进行的，如此，直接用滤波值进行待估参数的求解，效果不够理想。

第三步，进行IEKS中的平滑。由于条件期望是基于所有的量测得到的，扩展卡尔曼滤波只能给出近似的估计，其优点在于可以实现实时参数的估计。事实上，准确的条件期望可以通过扩展卡尔曼平滑得到：

其包含平滑器(k＝N,N-1,…,1)：

$\begin{array}{c}{x}_{k/l}=x_{k/k}^{\left(l\right)}-P_{k/k}^{\left(l\right)}{F}^T{\mathrm{\λ}}_k\\ S_k^{\left(l\right)}=H_k{\left(x_{k/l-1}\right)}^T{R}^{-1}{H}_k\left(x_{k/l-1}\right)\\ {\mathrm{\λ}}_{k-1}={(I-P_{k/k}^{\left(l\right)}{S}_k^{\left(l\right)})}^T(F^T{\mathrm{\λ}}_k-H_k{\left(x_{k/l-1}\right)}^T{R}^{-1}{\mathrm{\ϵ}}_k^{\left(l\right)}),{\mathrm{\λ}}_{k=N}=0\\ P_{k/l}=P_{k/k}^{\left(l\right)}-P_{k/k}^{\left(l\right)}{F}^T{\mathrm{\Λ}}_{k}F\left(P_{k/k}^{\left(l\right)}\right)\\ {\mathrm{\Λ}}_{k-1}={(I-P_{k/k}^{\left(l\right)}{S}_k^{\left(l\right)})}^T{F}^T{\mathrm{\Λ}}_{k}F(I-P_{k/k}^{\left(l\right)}{S}_k^{\left(l\right)}),{\mathrm{\Λ}}_{k=N}=0\end{array}---\left(20\right)$

从上式可以看出，在第l次迭代中，IEKS算法利用泰勒公式进行线性化的基准点是第l-1次迭代中的平滑估计值而EKS中的基准点是本轮迭代的一步预测值随着迭代步数的增加，比更接近于状态真值，因此IEKS比EKS具有更高的估计精度，因此在代入EM算法中进行参数估计时得到的结果更为精确。

通过平滑，可以得到较为精确的系统状态值，在进行条件期望的求解时实现理想的结果。

第四步，进行EM算法的求解。求取似然函数期望的最大值，令可得

${\widehat{\mathrm{\η}}}^{\left(l\right)}={\widehat{\mathrm{\η}}}^{(l-1)}+{\left[\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}{H}_{\mathrm{\η}}^T\right]}^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]---\left(21\right)$

代入上轮的系统偏差估计值和此轮的平滑值，可以得到此轮系统偏差的估计值。

第五步，进行循环迭代，最终得到系统偏差的估计结果。在每一轮的滤波平滑求解中，得到系统偏差的估计值，由于初始的系统偏差是设置为零的，因此，在最开始的几轮迭代中，系统偏差的估计与真值相差较大。在经过反复的迭代后，应用上一轮的估计值进行滤波平滑，目标的状态值趋于不断精确，因此，系统的偏差估计不断趋于稳定，最终会得到一个收敛的结果。

批处理EM算法的迭代次数L可以设定为固定值，或者选择一个收敛阈值τ，当时，就认为算法收敛，停止迭代。算法流程图如图2所示。

批处理EM算法在每次迭代中需要全部的量测数据集Z＝{z₁,z₂,…,z_N}，当量测采样个数N增加时，批处理EM算法就需要庞大的存储空间和时间代价，这对实时的多传感目标跟踪系统来说是不可行的。在对实时性要求较高、存储空间较少、时间代价较低的情况下，引入滑窗EM算法进行系统偏差的估计。

滑窗EM算法通过设定时间窗来限制每次估计所能使用的最大量测个数，每过一个采样时刻时间窗就向前滑动一个时刻。假设时间窗长度为s，当融合中心接收到k时刻的量测数据时，使用离k时刻最近的s个量测值{z_k-s+1,z_k-s+2,…,z_k}来计算偏差估计值当接收到新的量测数据z_k+1时，时间窗向前滑动一次，在时间窗{z_k-s+2,z_k-s+2,…,z_k+1}上再得到估计值依次递推。这样每次估计所处理的数据个数只有s个，大大减小了计算量。整个过程如图4所示：

在每个滑窗中对偏差进行估计时，滑窗EM的计算方法与批处理EM算法类似，其流程图如图5所示。

仿真表明，滑窗EM算法以细微的精度损失为代价，得到了实时性和存储空间的提升。

仿真分析

在二维坐标系下，存在五个传感器对一个目标仅进行距离量测，五个传感器的坐标分别为：传感器1(0,0)，传感器2(0,20km)，传感器3(20km,0)，传感器4(0,40km)，传感器5(40km,0)，其系统偏差分别为η₁＝0.5km，η₂＝0.4km，η₃＝0.3km，η₄＝0.3km，η₅＝0.4km，测距噪声均方差为σ_r＝0.005km。目标做匀速直线运动，初始状态为(-10km；0.3km/s；10km；0.3km/s)，过程噪声均方差为σ_ω＝0.0002km/s²，传感器的扫描周期为1s，仿真200个扫描周期。仿真场景如图6所示。

下面对目标状态和传感器偏差进行估计：

方法一：批处理EM算法

目标初始航迹通过EKF滤波算法求得，传感器初始系统偏差迭代终止阈值τ＝10^-7，最大迭代步数为200步，EM算法中和P_k|N采用IEKS算法估计。

方法二：滑窗EM算法

使用滑窗EM算法对目标状态和系统偏差进行估计，目标初始航迹和传感器初始系统偏差的取值同批处理EM算法，EM算法中的平滑值采用IEKS算法估计，滑窗EM算法窗口长度为20个采样周期，每个周期迭代次数为5次。

方法三：扩维无迹卡尔曼滤波算法(AUKF)

扩维的空间配准算法是将目标状态X_k与传感器系统偏差η叠加作为待估状态量X_a,k，从而对目标状态与系统偏差同时估计。

目标运动状态估计：

通过100次Monte-Carlo仿真，仿真结果如下：

三种算法位置和速度估计的平均RMSE

从上表可以看出：在X、Y轴位置的估计中，批处理EM算法(Batch EM)估计误差比滑窗EM算法(Sliding window EM)的估计误差要小，滑窗EM算法的估计误差约比批处理EM算法大0.04km，而AUKF估计效果最差；在X、Y轴速度的估计中，批处理EM误差也比滑窗EM算法小，AUKF估计最次。从图7中可以看出目标位置整体估计的RMSE也是同样的结论。

系统偏差估计：

批处理EM算法曲线(图8(f)～图8(j))给出了各传感器系统偏差的估计曲线随迭代步数的变化，从图中可以看出，批处理EM算法约在第100个迭代周期收敛，且距离偏差估计值与真值相差约0.01km。滑窗EM和AUKF曲线图(图8(a)～图8(e))给出了滑窗EM对系统偏差在不同时刻下的估计，从图中可以看出，滑窗EM算法约在第50s即可收敛，距离收敛误差约为0.04km，比批处理EM的估计误差大，扩维方法效果最次。

本发明提供一种基于期望最大化的多源测距传感器空间配准方法，主要思想是通过目标状态方程和量测方程，构造似然函数，对似然函数求取期望，并使其最大化，可以得到隐含的系统偏差参数估计值。本方法将IEKS(迭代扩展卡尔曼平滑)算法与EM算法结合，首先初始化IEKS中的第一轮平滑值，通过在估计偏差为零的条件下进行一次扩展卡尔曼滤波，得到第一轮的平滑值。然后在下一轮计算过程中，进行IEKS中的滤波和平滑过程，最后进行期望最大化的求解，得到本轮的系统偏差估计值。如此迭代，在设定的迭代次数或者是收敛阈值的条件下，最终会得到一个收敛的系统偏差估计值。在追求实时性、更小的数据存储空间和计算耗时情况下，可以使用滑窗EM算法。仿真结果表明，本发明提出的算法具有抗噪声更强，所需数据较少，稳定性、精度更高的优点。

Claims (5)

1.基于期望最大化的多源测距传感器空间配准方法，其特征在于：该空间配准方法包括以下步骤：

根据批处理期望最大化算法或滑窗期望最大化算法，通过目标的状态方程和量测方程构造似然函数，对似然函数求取期望，并使期望最大化，得到隐含的系统偏差参数估计值，从而实现多源测距下的空间配准以及目标准确定位。

2.根据权利要求1所述基于期望最大化的多源测距传感器空间配准方法，其特征在于：所述空间配准方法具体包括以下步骤：

1)初始化迭代扩展卡尔曼平滑一轮计算过程中需要的平滑值，该平滑值在第一轮计算过程中通过在估计偏差为零的条件下使用传感器全部量测数据进行一次扩展卡尔曼滤波得到，该平滑值在后续计算过程中使用上一轮计算过程得到的系统偏差估计值计算得到；

2)经过步骤1)后，使用传感器全部量测数据进行所述迭代扩展卡尔曼平滑一轮计算过程中的滤波和平滑过程，得到计算期望时所需的目标状态和协方差；

3)经过步骤2)后，进行期望最大化的求解，得到本轮计算过程的系统偏差估计值；

4)重复步骤1)至步骤3)，在设定的迭代次数或者收敛阈值的条件下，得到一个收敛的系统偏差估计值。

3.根据权利要求1所述基于期望最大化的多源测距传感器空间配准方法，其特征在于：所述空间配准方法具体包括以下步骤：

1)初始化迭代扩展卡尔曼平滑在当前时间窗口内需要的平滑值，该平滑值在第一个时间窗口对应的迭代扩展卡尔曼平滑计算过程中通过在估计偏差为零的条件下使用该时间窗口内传感器量测数据进行一次扩展卡尔曼滤波得到，该平滑值在后续时间窗口对应的迭代扩展卡尔曼平滑计算过程中使用在上一个时间窗口计算得到的最终系统偏差估计值计算得到；

2)经过步骤1)后，在当前时间窗口内进行迭代扩展卡尔曼平滑一轮计算过程中的滤波和平滑过程，得到计算期望时所需的目标状态和协方差，然后进行期望最大化的求解，得到系统偏差估计值，以该系统偏差估计值计算在当前时间窗口下一轮迭代扩展卡尔曼平滑过程中需要的平滑值；

3)重复步骤2)，在设定的迭代次数或者是收敛阈值的条件下得到当前窗口最终的系统偏差估计值；

4)重复步骤2)至步骤3)，依次计算后续时间窗口对应的系统偏差估计值。

4.根据权利要求1所述基于期望最大化的多源测距传感器空间配准方法，其特征在于：所述似然函数期望表示为：

$\begin{array}{c}Q(\mathrm{\η},{\widehat{\mathrm{\η}}}^{\left(l\right)})=-\frac{1}{2}\mathrm{Tr}\left\{Q^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(A-B-B^T+C)\right\}-\\ \frac{1}{2}\mathrm{Tr}\left\{R^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(H_k^T{P}_{k|N}{H}_k+D-E-E^T+G)\right\}\end{array}$

其中，Tr表示矩阵求迹运算，Q为目标过程噪声方差阵，R为量测噪声方差阵，N为量测采样个数，H_k为k时刻的雅克比矩阵，P_k|N为k时刻的协方差矩阵， $A=P_{k|N}+{\hat{X}}_{k|N}{\hat{X}}_{k|N}^T,B=P_{k,k-1|N}{F}^T+{\hat{X}}_{k|N}{\hat{X}}_{k-1|N}^T{F}^T,C={\mathrm{FP}}_{k-1|N}{F}^T+F{\hat{X}}_{k-1|N}{\hat{X}}_{k-1|N}^T{F}^T,$ $D=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})][z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)}){]}^T,E=[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})\left]\right[\mathrm{\η}-{\widehat{\mathrm{\η}}}^{(l-1)}{]}^T{H}_{\mathrm{\η}},$ F为已知的目标状态转移矩阵，H_η为量测矩阵对系统偏差的求导，上一次迭代得到的系统偏差估计值，z_k为k时刻多源测距传感器的量测，h(·)为已知的量测矩阵，为k时刻目标状态的平滑值。

5.根据权利要求1所述基于期望最大化的多源测距传感器空间配准方法，其特征在于：所述期望最大化的解析解表示为：

${\widehat{\mathrm{\η}}}^{\left(l\right)}={\widehat{\mathrm{\η}}}^{(l-1)}+{\left[\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}{H}_{\mathrm{\η}}^T\right]}^{-1}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{\mathrm{\η}}[z_k-h({\hat{X}}_{k|N},{\widehat{\mathrm{\η}}}^{(l-1)})]$

其中，为上一次迭代得到的系统偏差估计值，为本次迭代得到的系统偏差估计值，H_η为量测矩阵对系统偏差的求导，z_k为k时刻多源测距传感器的量测，h(·)为已知的量测矩阵，为k时刻目标状态的平滑值，N为量测采样个数。