CN110426689B - 一种基于em-cks的机载多平台多传感器系统误差配准算法 - Google Patents

Abstract

本发明提供了一种基于EM‑CKS的机载多平台多传感器系统误差配准算法，该算法包括如下步骤：步骤一：数学建模；步骤二：容积卡尔曼滤波器和平滑器实现；步骤三：基于EM‑CKS的多传感器系统误差配准算法。本发明所提出的方法对传统多传感器系统误差配准算法做了改进，解决了传统在线联合估计法需要精确已知系统误差状态空间模型的问题。其次，本发明所提出的方法具有很强的数值稳定性，解决了传统非线性高斯滤波器在量测高精度、系统高维数、复杂强非线性情况下对系统状态后验概率密度函数近似精度不高这一问题，有效实现了对机载多平台多传感器的系统误差配准，提高了多平台多传感器对目标的跟踪精度，同时还提升了目标跟踪的可靠性和稳定性。

Description

一种基于EM-CKS的机载多平台多传感器系统误差配准算法

技术领域

本发明属于多传感器信息融合及目标跟踪领域，具体涉及量测高精度、系统高维度、复杂强非线性条件下的机载多平安协同跟踪领域。

背景技术

机载多平台多传感器信息融合技术能够充分利用各类数据的特点，获取不同视角、不同传感器、不同特征的互补数据，大大扩展了探测区域的覆盖面积，提高了信息利用效率，加快了数据更新速率，从而能够显著提升飞机编队对目标的跟踪能力，提供更快捷的航迹起始速度，更短的航迹收敛时间，更精确的航迹跟踪精度和更稳定的航迹连续性。

然而在舰载机编队飞行等复杂的目标跟踪场景中，系统误差的存在会导致航迹关联混乱、融合精度降低等问题。导致产生这种现象的原因是不同机载平台上的传感器测量信息都是在各自的测量坐标系下获得的，当飞机编队对目标信息进行融合跟踪时，需要将这些测量信息转换到同一参考坐标系中才能进行关联滤波。由于受到机载平台自身定位误差、传感器测量误差及安装误差等诸多系统误差因素的影响，加之坐标转换又不可避免地会放大上述误差，导致融合中心在公共参考坐标系下难以将源于同一目标的测量值关联起来，严重影响目标航迹的稳定性和跟踪精度。因此，系统误差配准是多源信息融合的前提，为保证后继多平台多传感器信息融合的精度，有必要开展多平台系统误差配准技术研究。

随着对系统误差配准研究的不断深入，一些消除系统误差影响的方法逐渐发展起来，主要分为2类：离线估计补偿法和在线联合估计法。离线估计补偿法首先估计出系统误差，然后对目标量测进行修正，并基于修正后的量测估计目标的真实状态，如最小二乘法、广义最小二乘法、极大似然算法等。此类算法都是假设系统误差是缓慢变化的，从而对某一时间段内的量测数据进行批处理，但不能对系统误差的变化做出实时地调整，缺乏实时性。在线联合估计法考虑到系统误差和目标状态的相互作用，将系统误差作为目标状态的分量同时对二者进行估计，如扩维高斯滤波算法等。但此类算法需要精确已知系统误差的状态空间模型，然而在实际工程应用中，系统误差的状态空间模型可能是不准确的或者是未知的，从而恶化扩维高斯滤波器的性能，甚至导致滤波器发散。

发明内容

发明目的：

为了解决上述问题，本发明紧密结合工程应用背景，提出一种基于EM-CKS的机载多平台多传感器系统误差配准算法。

技术方案：

为了达到上述目的，本发明提供了如下技术方案：

首先将传感器量测系统误差视为系统待估计的未知参数，构建了新的传感器量测方程，从而将机载平台多传感器系统误差配准问题转化成含未知参数的非线性系统辨识问题。其次，该方法引入期望最大化算法框架，并利用容积卡尔曼滤波器和平滑器近似计算完整的对数似然函数期望。最后，将近似计算的对数似然函数期望进行最大化处理，并通过解析更新反复迭代的方式获得各传感器系统误差的参数估计。

步骤一：期望最大化算法实现

考虑如下状态空间形式的离散非线性系统：

其中，x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态向量和量测向量，

n和m分别表示状态维数和量测维数。

f_k-1(x_k-1)和h_k(x_k，η)分别表示系统的状态转移函数和量测函数。

w_k∈Rⁿ和v_k∈R^m分别表示系统的过程噪声向量和量测噪声向量，二者分别服从均值为零，方差为Q_k和R_k的高斯分布，且互不相关。

初始状态向量x₀服从均值为

方差为P_0|0的高斯分布，且它与w_k及v_k互不相关。

η表示未知的量测系统误差向量。

我们的目的是估计未知的量测系统误差向量η。根据最大似然判据，我们可以首先通过最大化如下的联合对数似然函数获得一个近似的未知参数向量η的极大似然解，即：

其中，N表示量测的样本数量，

p_η(g)表示依赖于未知系统误差η的概率密度函数，

log表示对数函数，

表示从0到N时刻的状态向量集合，

表示从1到N时刻的量测集合，

p_η(x_0:N,z_1:N)表示状态向量集合与量测向量集合的联合概率密度函数。

然而，由于状态向量集合x_0:N是不可观测的，导致上式不能直接利用。

为了解决p_η(x_0:N,z_1:N)中状态向量集合x_0:N不可观测的问题，我们借助期望最大化算法的思想，构造p_η(x_0:N,z_1:N)的一个近似目标函数

其中/>

表示p_η(x_0:N,z_1:N)在给定量测集合z_1:N和未知系统误差向量η的第j步近似估计值η_j条件下的最小方差估计，即：

基于期望最大化算法的未知系统误差向量η估计的具体实施过程被总结如下：

1)选择一个未知参数的初始估计η₀；

2)计算

3)最优化计算

4)重复上面两个步骤，直到满足收敛要求。

步骤二：容积卡尔曼滤波器和平滑器实现

表1容积卡尔曼滤波器和平滑器的基本框架

其中，

和P_k-1|k-1分别表示k-1时刻系统状态的估计值和估计误差协方差；

和P_k|k-1分别表示系统状态的预测值和预测误差协方差；

P_k-1,k|k-1表示k-1时刻状态估计与状态预测之间的互协方差；

表示状态经量测函数传播的预测值；

表示新息协方差；

表示状态预测与量测预测的互协方差；

K_k表示滤波增益；

和P_k|k分别表示k时刻系统状态的估计值和估计误差协方差；

和P_k-1|N分别表示k-1时刻系统状态的平滑值和平滑误差协方差；

和P_k|N分别表示k时刻系统状态的平滑值和平滑误差协方差；

G_k-1表示平滑增益。

从表1中的步骤1)～6)可以看出，在容积卡尔曼滤波器和平滑器中涉及到求解带高斯分布的非线性状态函数和量测函数的多维积分。由于状态函数和量测函数的非线性一般会导致计算多维积分难以实现，因此通常情况下只能采用数值近似的方法来获得次优的高斯近似滤波器和平滑器。

本发明采用三阶球径容积准则来计算这些含高斯权重的非线性函数积分从而得到容积卡尔曼滤波器及平滑器，具体实施过程如下：

1)选取容积采样点ξ⁽ⁱ⁾：

其中，

表示Rⁿ空间内的单位正交基向量。

2)计算含高斯权重的非线性函数积分：

其中，f(x)表示任意的非线性函数；

m和P分别表示高斯分布的均值和协方差矩阵；

表示协方差矩阵P的一个Cholesky分解因子。

步骤三：基于EM-CKS的多传感器系统误差配准算法

根据步骤一所给出的离散非线性系统模型，

可以重新整理为：

其中，p_η(x_k+1|x_k)表示状态转移概率密度分布函数；

p_η(z_k+1|x_k+1)表示似然概率密度分布函数；

表示状态向量集合的平滑概率密度分布函数；

表示联合平滑概率密度分布函数；

表示k+1时刻状态向量x_k+1的平滑概率密度分布函数。

根据步骤二所给出的高斯近似平滑器的一般结构，平滑概率密度

和联合平滑概率密度/>

可被近似表示为：

其中，

和P_k+1|N分别表示k+1时刻系统状态的平滑值和平滑误差协方差；

G_k表示平滑增益。

根据步骤一所给出的状态空间形式的离散非线性系统模型，状态转移概率密度分布p_η(x_k+1|x_k)和似然概率密度分布p_η(z_k+1|x_k+1)又可以分别表示如下：

令关于任意概率密度函数p(x)的数学期望表示为<g>＝∫gp(x)dx，并利用矩阵运算性质a^TCb＝Tr(Cba^T)，则

和/>

可以被重新整理为：

其中，

和/>

分别表示如下：

将h_k+1(x_k+1,η)分别在

处和η＝η_j处进行一阶泰勒级数展开，得：

其中，H_k+1为量测函数h_k+1(x_k+1,η)在

处的雅克比矩阵，/>

为量测函数h_k+1(x_k+1,η)在η＝η_j处的雅克比矩阵，即：

令

ε＝η-η_j，则/>

可以重新表示为：

根据矩阵求导的数学公式

则有：

为了最优化计算

需要满足如下条件：

因此，只需满足如下方程即可：

继而，可以推出：

基于EM-CKS的多传感器系统误差配准算法的具体实施过程被总结如下：

1)选择一个初始估计η₀，迭代次数M，并设置初始迭代步数i＝0；

2)基于当前的未知参数估计η_j，运行容积卡尔曼滤波器和平滑器，最后得到经过平滑后的状态估计值

3)计算雅克比矩阵

并将该雅克比矩阵

和状态平滑估计值/>

一同带入到方程：

从而更新得到η_j+1；

4)i+1→i并检查迭代终止条件i>M。如果这个条件满足，那么迭代终止；否则迭代继续，并返回2)。

有益效果：

本发明所提出的方法对传统多传感器系统误差配准算法做了改进。首先将各传感器量测系统误差视为系统待估计的未知参数，构建了新的传感器量测方程，从而将机载多平台多传感器系统误差配准问题转化成含未知参数的非线性系统辨识问题，解决了传统在线联合估计法需要精确已知系统误差状态空间模型这一问题。

其次，本发明所提出的方法引入了期望最大化算法框架，并利用容积卡尔曼滤波器和平滑器进行递推运算，具有很强的数值稳定性，解决了传统非线性高斯滤波器在量测高精度、系统高维数、复杂强非线性情况下对系统状态后验概率密度函数近似精度不高这一问题，有效实现了对机载多平台多传感器的系统误差配准，提高了多平台多传感器对目标的跟踪精度，同时还提升了目标跟踪的可靠性和稳定性。

本发明提出的基于EM-CKS的机载多平台多传感器系统误差配准算法是多传感器信息融合领域多机协同定位跟踪亟待解决的难题。本发明具有良好的扩展性和适应性，可广泛应用于对稳定目标跟踪有较高要求的多平台有源/无源雷达信息融合跟踪系统，应用前景广阔，应用价值巨大。

具体实施方式

下面结合实施例对本发明进行详细说明，

首先将传感器量测系统误差视为系统待估计的未知参数，构建了新的传感器量测方程，从而将机载平台多传感器系统误差配准问题转化成含未知参数的非线性系统辨识问题。

其次，该方法引入期望最大化算法框架，并利用容积卡尔曼滤波器和平滑器近似计算完整的对数似然函数期望。

最后，将近似计算的对数似然函数期望进行最大化处理，并通过解析更新反复迭代的方式获得各传感器系统误差的参数估计。

步骤一：期望最大化算法实现

考虑如下状态空间形式的离散非线性系统：

其中，x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态向量和量测向量，

n和m分别表示状态维数和量测维数；

f_k-1(x_k-1)和h_k(x_k，η)分别表示系统的状态转移函数和量测函数；

w_k∈Rⁿ和v_k∈R^m分别表示系统的过程噪声向量和量测噪声向量，二者分别服从均值为零，方差为Q_k和R_k的高斯分布，且互不相关；

初始状态向量x₀服从均值为

方差为P₀₀的高斯分布，且它与w_k及v_k互不相关；

η表示未知的量测系统误差向量。

我们的目的是估计未知的量测系统误差向量η。根据最大似然判据，我们可以首先通过最大化如下的联合对数似然函数获得一个近似的未知参数向量η的极大似然解，即：

其中，N表示量测的样本数量，

p_η(g)表示依赖于未知系统误差η的概率密度函数，

log表示对数函数，

表示从0到N时刻的状态向量集合，

表示从1到N时刻的量测集合，

p_η(x_0:N,z_1:N)表示状态向量集合与量测向量集合的联合概率密度函数。

然而，由于状态向量集合x_0:N是不可观测的，导致上式不能直接利用。

为了解决p_η(x_0:N,z_1:N)中状态向量集合x_0:N不可观测的问题，我们借助期望最大化算法的思想，构造p_η(x_0:N,z_1:N)的一个近似目标函数

其中/>

表示p_η(x_0:N,z_1:N)在给定量测集合z_1:N和未知系统误差向量η的第j步近似估计值η_j条件下的最小方差估计，即：/>

基于期望最大化算法的未知系统误差向量η估计的具体实施过程被总结如下：

1)选择一个未知参数的初始估计η₀；

2)计算

3)最优化计算

4)重复上面两个步骤，直到满足收敛要求。

步骤二：容积卡尔曼滤波器和平滑器实现

表1容积卡尔曼滤波器和平滑器的基本框架

其中，

和P_k-1|k-1分别表示k-1时刻系统状态的估计值和估计误差协方差；

和P_k|k-1分别表示系统状态的预测值和预测误差协方差；

P_k-1,k|k-1表示k-1时刻状态估计与状态预测之间的互协方差；

表示状态经量测函数传播的预测值；

表示新息协方差；

表示状态预测与量测预测的互协方差；

K_k表示滤波增益；

和P_k|k分别表示k时刻系统状态的估计值和估计误差协方差；

和P_k-1|N分别表示k-1时刻系统状态的平滑值和平滑误差协方差；

和P_k|N分别表示k时刻系统状态的平滑值和平滑误差协方差；

G_k-1表示平滑增益。

从表1中的步骤1)～6)可以看出，在容积卡尔曼滤波器和平滑器中涉及到求解带高斯分布的非线性状态函数和量测函数的多维积分。由于状态函数和量测函数的非线性一般会导致计算多维积分难以实现，因此通常情况下只能采用数值近似的方法来获得次优的高斯近似滤波器和平滑器。

本发明采用三阶球径容积准则来计算这些含高斯权重的非线性函数积分从而得到容积卡尔曼滤波器及平滑器，具体实施过程如下：

1)选取容积采样点ξ⁽ⁱ⁾：

其中，

表示Rⁿ空间内的单位正交基向量。

2)计算含高斯权重的非线性函数积分：

其中，f(x)表示任意的非线性函数；

m和P分别表示高斯分布的均值和协方差矩阵；

表示协方差矩阵P的一个Cholesky分解因子。

步骤三：基于EM-CKS的多传感器系统误差配准算法

根据步骤一所给出的离散非线性系统模型，

可以重新整理为：

其中，p_η(x_k+1|x_k)表示状态转移概率密度分布函数；

p_η(z_k+1|x_k+1)表示似然概率密度分布函数；

表示状态向量集合的平滑概率密度分布函数；

表示联合平滑概率密度分布函数；

表示k+1时刻状态向量x_k+1的平滑概率密度分布函数。

根据步骤二所给出的高斯近似平滑器的一般结构，平滑概率密度

和联合平滑概率密度/>

可被近似表示为：

其中，

和P_k+1|N分别表示k+1时刻系统状态的平滑值和平滑误差协方差；

G_k表示平滑增益。

根据步骤一所给出的状态空间形式的离散非线性系统模型，状态转移概率密度分布p_η(x_k+1|x_k)和似然概率密度分布p_η(z_k+1|x_k+1)又可以分别表示如下：

令关于任意概率密度函数p(x)的数学期望表示为<g>＝∫gp(x)dx，并利用矩阵运算性质a^TCb＝Tr(Cba^T)，则

和/>

可以被重新整理为：

其中，

和/>

分别表示如下：

将h_k+1(x_k+1,η)分别在

处和η＝η_j处进行一阶泰勒级数展开，得：

其中，H_k+1为量测函数h_k+1(x_k+1,η)在

处的雅克比矩阵，/>

为量测函数h_k+1(x_k+1,η)在η＝η_j处的雅克比矩阵，即：

令

ε＝η-η_j，则/>

可以重新表示为：/>

根据矩阵求导的数学公式

则有：

为了最优化计算

需要满足如下条件：

因此，只需满足如下方程即可：

继而，可以推出：

基于EM-CKS的多传感器系统误差配准算法的具体实施过程被总结如下：

1)选择一个初始估计η₀，迭代次数M，并设置初始迭代步数i＝0；

2)基于当前的未知参数估计η_j，运行容积卡尔曼滤波器和平滑器，最后得到经过平滑后的状态估计值

3)计算雅克比矩阵

并将该雅克比矩阵

和状态平滑估计值/>

一同带入到方程：

从而更新得到η_j+1；

4)i+1→i并检查迭代终止条件i>M。如果这个条件满足，那么迭代终止；否则迭代继续，并返回2)。

实施例一：

通过数值仿真详细说明该发明所提出的方法。具体仿真场景设置为由两架舰载直升机平台共同探测跟踪一架战斗机目标。其中，每架舰载直升机各携载一部三维传感器。战斗机飞行速度较快，飞行海拔高度为2km，舰载直升机飞行速度较慢，飞行海拔高度为1km，所有飞行器均沿经线飞行，三维传感器扫描时间间隔为1s。各传感器分别测量得到目标的距离、方位和俯仰角三维信息，且它们的测量噪声均为零均值高斯白噪声，对应的各传感器量测噪声协方差矩阵可以表示为：

R_k＝diag{(50m)²,(0.002rad)²,(0.002rad)²}

各传感器系统误差均设置为η＝[1000m,0.5°,0.5°]^T。在此仿真试验中，50次独立的蒙特卡洛仿真被执行，每次蒙特卡洛仿真试验的迭代次数设置为3000次。为了展示所提出的算法性能，表1给出了各传感器系统误差的最终估计。

表1真实的参数值和估计的参数值(均值±标准差)

从表1中可以看到本文所提出的算法具有良好的估计精度。以上这些仿真结果说明了基于EM-CKS的系统误差配准算法的有效性。

Claims (2)

1.一种基于EM-CKS的机载多平台多传感器系统误差配准算法，其特征在于，所述算法包括如下步骤：

步骤一：数学建模；

步骤二：容积卡尔曼滤波器和平滑器实现；

步骤三：基于EM-CKS的多传感器系统误差配准算法；

所述步骤一中，构建状态空间形式的离散非线性系统：：

其中：x_k∈Rⁿ和z_k∈R^m分别表示k时刻的状态向量和量测向量；

n和m分别表示状态维数和量测维数；

f_k-1(x_k-1)和h_k(x_k，η)分别表示系统的状态转移函数和量测函数；

w_k∈Rⁿ和v_k∈R^m分别表示系统的过程噪声向量和量测噪声向量，二者分别服从均值为零，方差为Q_k和R_k的高斯分布，且互不相关；

初始状态向量x₀服从均值为

方差为P_0|0的高斯分布，且它与w_k及v_k互不相关；

η表示未知的量测系统误差向量；

所述步骤二中：

其中，N为量测个数；

和P_k-1k-1分别表示k-1时刻系统状态的估计值和估计误差协方差；

和P_kk-1分别表示系统状态的预测值和预测误差协方差；

P_k-1,kk-1表示k-1时刻状态估计与状态预测之间的互协方差；

表示状态经量测函数传播的预测值；

表示新息协方差；

表示状态预测与量测预测的互协方差；

K_k表示滤波增益；

和P_k|k分别表示k时刻系统状态的估计值和估计误差协方差；

和P_k-1|N分别表示k-1时刻系统状态的平滑值和平滑误差协方差；

和P_k|N分别表示k时刻系统状态的平滑值和平滑误差协方差，G_k-1表示平滑增益；

f_k-1(x_k-1)为系统的状态转移函数；h_k(x_k，η)为系统的量测函数；x_k表示k时刻的状态向量；z_k表示k时刻的量测向量；

所述步骤三包括：

a)选择一个初始估计η₀，迭代次数M，并设置初始迭代步数i＝0；

b)基于当前的未知参数估计η_j，运行所述步骤二中的容积卡尔曼滤波器和平滑器，最后得到经过平滑后的状态估计值

c)计算雅克比矩阵H：

其中，

为量测函数h_k+1(x_k+1,η)在η＝η_j处的雅克比矩阵，并将该雅克比矩阵/>

和状态平滑估计值/>

一同带入到方程：

从而更新得到η_j+1；

4)i+1→i并检查迭代终止条件i>M，如果这个条件满足，那么迭代终止；否则迭代继续，并返回2)。

2.根据权利要求1所述的基于EM-CKS的机载多平台多传感器系统误差配准算法，其特征在于，所述雅克比矩阵为：