一种采用多模型多系统对目标跟踪定位的方法与流程

本申请涉及滤波技术领域，特别地，涉及通过多传感器数据融合对目标进行跟踪定位的方法。

背景技术：

传统的导航定位算法主要通过建立机动目标的运动状态模型和外界系统对目标的观测模型来进行滤波，获得机动目标的准确定位信息。但是一般情况下，机动目标的运动方式存在随机性，基本很难通过一种特定的模型完整对机动目标的运动进行建模描述。此外单一的定位系统对目标进行观测时，起作用范围有限，比如gnss卫星导航在城市高楼间、树荫下、峡谷中等复杂环境下，定位性能受限。

卫星导航系统受到各种因素的影响，包括摄动引起的星历误差(轨位、姿态)，星载原子钟的钟差，导航信号在传输过程中的电离层、对流层误差等。这些误差使得用户在伪距测量时存在一定的误差。

临近空间浮空器一般部署在平流层，运动环境较为稳定，浮空器平台运动较慢，信号传输过程中不经过电离层，用浮空器平台来搭载导航增强系统，可以增加地面用户的可见导航源数量，实现对卫星星座的补充，同时信号传播衰减小，可作为卫星导航系统有效补充。此外，可在浮空器平台搭载转发器，将地面的导航增强信息(差分修正信息、完好性信息)进行转发，实现一定区域的导航增强。

与卫星导航类似，搭载导航增强源的浮空器平台拓扑构型直接影响着用户的导航定位性能，良好的拓扑构型维持及动态调整需要浮空器平台之间高精度的相互测距；搭载导航增强系统的浮空器平台需要维持统一的时间频率。因此浮空器平台之间的相互测距和时频统一成为影响导航增强性能的关键因素。

而浮空器平台要想实现导航增强功能，就要有效构建平台间测距和时频统一的机制，这需要平台节点之间构建起高效、高鲁棒性的网络和通信机制。从另一方面看，浮空器飞行高度有限，覆盖范围较小，要想实现大范围的导航增强服务，需要大量的浮空器部署，而大量浮空器部署时，浮空器之间的互联互通也无法再用简单的广播机制来实现，需要一套有效的路由机制来完成大量浮空器之间的互联互通，有效组网。

目前关于浮空器组网的方案国内已有一些研究，但大多还是从满足地面用户通信的角度出发来考虑组网问题，对导航增强的需求考虑较少，同时也没有考虑浮空器平台增强导航网络安全的问题。

技术实现要素：

本申请的目的是针对单一运动模型不匹配和单一定位系统覆盖范围有限限制了机动目标导航定位性能的问题，提出了一种基于贝叶斯理论的多系统多模型交互式定位融合机制，该机制包括相应的处理架构、处理算法和融合算法。

具体地，本申请的目的是针对浮空器导航增强的组网问题，提出了一种基于网络编码理论的空基平台导航增强组网方案，通过构建基于位置坐标的拓扑路由实现空基平台的导航增强一体化网络。该方案包括拓扑路由的构建，路由信息的发送以及路由信息的接收。

一种采用多模型多系统对目标跟踪定位的方法，其特征在于，包括：

多系统多模型建模；

多系统多模型交互式定位滤波及矢量融合，包括：

多模型融合概率计算及多模型状态矢量融合；

多模型多系统概率计算及多模型多系统滤波；

各模型多系统融合；

多系统下的各模型概率计算及各模型状态矢量融合。

优选地，所述的多模型融合概率计算及多模型状态矢量融合包括，

计算融合概率；

多模型状态矢量融合，包括在当前t时刻，针对每个运动模型，采用贝叶斯先验概率，将该运动模型滤波器t-1时刻输出的状态矢量和状态误差分别与其它n-1个运动模型进行融合，得到融合后t-1时刻该模型的状态矢量和状态误差，其中第n个运动模型与其它运动模型状态矢量的融合方法为：求n个模型中的每个模型的状态矢量与各自加权因子的乘积，再将该n个乘积求和，其中，每个模型的加权因子是贝叶斯先验概率。

优选地，所述的计算融合概率包括，

其中p融合后|融合前(x2|x1)为从第一模型转移到第二模型的概率，这个概率是提前给定的；p融合前(x1)是融合前的模型概率，初次滤波时认为各个模型概率相等在后续滤波的过程中，不断更新；

所述的多模型状态矢量融合具体包括，

第一模型融合后的数据来自于融合前两个模型的数据按照条件概率加权后的和：

x1(t-1)融合后＝x1(t-1)融合前×p融合前|融合后(x1|x1)+x2(t-1)融合前×p融合前|融合后(x2|x1)

第二模型融合后的数据来自于融合前两个模型的数据按照条件概率加权后的和：

x2(t-1)融合后＝x1(t-1)融合前×p融合前|融合后(x1|x2)+x2(t-1)融合前×p融合前|融合后(x2|x2)

其中，x1(t-1)混合前、x1(t-1)混合后分别是第一模型混合前、混合后的状态矢量；

x2(t-1)混合前、x2(t-1)混合后分别是第二模型混合前、混合后的状态矢量。

优选地，所述的多模型状态矢量融合还包括目标定位误差融合，其包括，

对定位误差方差p＝(x-x真值)²进行融合，融合后模型误差是“融合前各模型误差及融合前后信息偏差平方”的加权平均。

优选地，所述的目标定位误差融合具体包括，

第一模型融合后的误差方差：

第二模型融合后的误差方差：

(x1(t-1)融合前-x1(t-1)融合后)²表示融合前后两状态间的误差偏差。

优选地，所述的各模型多系统融合包括，

针对某一个模型下对应的所有观测系统滤波获得的的新息和新息方差，通过贝叶斯全概率公式，计算该模型下各系统的概率；以系统概率为加权因子，将各系统状态向量进行加权求和，获得多系统融合后各个模型的融合结果；

以系统概率为加权因子，对系统状态误差进行加权求和，获得多系统融合后各个系统的滤波误差矩阵；

将每个模型对应的系统状态向量和值及系统状态误差和值计算出来。

优选地，所述的各模型多系统融合包括，

在双模型双系统的情况下，匀速直线运动模型下的“多系统交互”包括将对应的两个并行滤波器gps定位系统、移动基站定位系统所获得的定位信息按照系统概率进行加权融合：

xcv(t|t)＝xgps|cv(t|t)×p{εgps(t)|mcv(t),z^t}+x基站|cv(t|t)×p{ε基站(t)|mcv(t),z^t}

xgps|cv(t|t)是cv模型下，gps观测系统滤波器的定位结果；

x基站|cv(t|t)是cv模型下，基站系统滤波器的定位结果；

p{εgps(t)|mcv(t),z^t}是匀速直线运动模型下gps的系统概率。

xca(t|t)＝xgps|ca(t|t)×p{εgps(t)|mca(t),z^t}+x基站|ca(t|t)×p{ε基站(t)|mca(t),z^t}

xgps|ca(t|t)是ca模型下，gps观测系统滤波器的定位结果；

x基站|ca(t|t)是ca模型下，基站系统滤波器的定位结果；

p{ε基站(t)|mca(t),z^t}是匀加速直线运动模型下gps的系统概率。

优选地，所述的多系统下的各模型概率计算及各模型状态矢量融合包括，

在双模型双系统的情况下，分别将匀速直线模型下gps和移动基站滤波融合结果xcv(t|t)，和ca模型下gps和移动基站滤波融合结果xca(t|t)，按照模型后验概率进行加权融合：

x(t|t)＝xcv(t|t)×p{mcv(t)|z^t}+xca(t|t)×p{mcv(t)|z^t}

xcv(t|t)是cv模型下gps和基站系统的定位融合结果，xca(t|t)是ca模型下gps和基站两系统定位融合结果；

p{mcv(t)|z^t}是t时刻cv模型后验概率；

p{mca(t)|z^t}是t时刻ca模型后验概率。

本申请针对单一系统定位范围有限，单一模型描述运动状态不准确引起的导航定位性能受限，提出了从运动模型、观测系统两个维度，采用多模型多系统对目标进行跟踪定位的算法。该算法将多模型和多系统的定位结果按照模型概率、系统概率进行加权融合，很好的发挥了多系统共同定位和多模型并行建模定位的性能优势。该算法的思想不限于导航定位领域，也可以扩展到其他的滤波技术领域中。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。

图1是根据本申请实施方式的多模型多系统交互处理的算法流程图；

图2是根据本申请实施方式的多模型多系统交互处理的算法中多模型融合步骤示意图。

具体实施方式

以下描述和附图充分地示出本申请的具体实施方案，以使本领域的技术人员能够实践它们。

本申请的主要发明点是基于“多模型”和“多系统”的多维跟踪定位并行处理架构，以及并行滤波中涉及到的预处理、融合处理算法，如图1所示，其可以用于“多模型多系统交互处理”。

为便于理解，以下实施方式中，假设目标只作两种运动——“匀速直线运动和匀加速直线运动”；只有两个系统对目标进行观测——“gps和移动基站”，如图1所示，包括如下步骤。

步骤1多系统多模型建模

一般情况下，对一个目标进行导航定位(或者称作跟踪滤波)，需要两个方程——“运动模型”及“观测模型”。

“运动模型”就是对目标的运动过程的一种数学描述，比如一个正在进行匀速直线运动的目标可以描述为：x(t+δt)＝x(t)+vδt做匀加速直线运动的目标可以描述为：

“观测模型”是目标外的其他系统对目标观测的一种数学描述，即建立从目标状态信息到观测信息之间的一种函数关系，比如通过gps卫星观测运动目标可以建立卫星观测信息“伪距”和目标状态信息“x,y,z坐标”之间的函数关系：z(t)＝f(x(t),v(t))；再如通过移动基站观测运动目标可以建立基站观测信息“移动信号强度”和目标状态信息“x，y，z坐标”之间的函数关系。

本申请所指“多模型”是说针对特定的运动目标，一般很难用一种运动模型描述。比如路面上的运动车辆在δt时间内，可能先做匀速直线运动，然后在路况较好的时候开始加速，看到前方红灯又进行减速运动，在十字路口进行了拐弯运动，整个过程中涉及到四种运动。所以用单一的一个运动模型去描述已经不够。所以考虑采用多个运动模型对目标运动状态进行描述，描述的状态方程统一记为：

上标p表示一个运动集合，针对上边的例子这里p表示集合；

{匀速直线运动，匀加速直线运动，转弯运动}

a表示t-1目标状态到t时刻目标状态函数关系(用矩阵表示)。例如当p为匀速直线运动时，状态矢量根据运动学定律，矩阵则：

w^p(t)是噪声，g^p是噪声作用到状态变量的函数变化关系。

本申请所指“多系统”是说针对某一运动目标，一个系统的观测很难覆盖，需要多个系统。比如，运动车辆在比较开阔的道路上行进时，gps卫星导航系统和移动基站信号可以对其进行定位，但是当车辆进入隧道后这两种信号都消失，但是如果车上装有wifi芯片，可以通过隧道里边的ap热点进行定位，本申请采用多个定位系统对目标定位；这里将第i个系统的观测信息(gps系统伪距，移动基站的信号强度)和目标状态信息(xyz坐标)之间的函数关系统一表示为：

zi(t)＝hix(t)+vi(t)

下标i表示观测系统集合，针对上边的例子这里i表示：

{gps系统，移动基站，wifi系统}

h表示从目标状态到系统观测信息之间的函数关系(用矩阵表示)，例如当i为gps系统时，矩阵h表示从目标状态x(t)(xyz坐标)到卫星观测伪距之间的函数关系。

步骤2多系统多模型交互式定位滤波及矢量融合

一般情况下，导航定位的滤波过程是一个迭代过程——根据上一时刻的目标状态信息x(t-1)(如xyz坐标)，和下一时刻的系统观测信息z(t)(如gps观测得到的目标伪距)，根据之前所述两个方程“状态方程”和“观测方程”进行滤波。

步骤2.1多模型融合概率计算及多模型状态矢量融合

多模型融合就是将上一时刻各个滤波器的定位结果进行融合，融合的方法是按照条件概率加权。

步骤2.1.1多模型融合概率计算

在进行矢量融合之前，先计算融合概率。加权条件概率表示从融合前某一模型到融合后某一模型的概率，如p融合前|融合后(x1|x1)表示的是融合后x1来自融合前x1的概率。

融合概率即加权因子是通过计算得来的，其依据的是贝叶斯概率理论，以p融合前|融合后(x1|x2)的推导为例：

其中p融合后|融合前(x2|x1)为从一个模型转移到另一个模型的概率，简单起见，这个概率是提前给定的；p融合前(x1)是融合前的模型概率，初次滤波时认为各个模型概率相等在后续滤波的过程中，不断更新。

步骤2.1.2多模型状态矢量融合

a)目标定位状态信息融合(即矢量x融合)：

在当前t时刻，针对每个运动模型，采用贝叶斯先验概率，将该运动模型滤波器t-1时刻输出的状态矢量和状态误差分别与其它n-1个运动模型进行融合，得到融合后t-1时刻该模型的状态矢量和状态误差，其中第n个运动模型与其它运动模型状态矢量的融合方法为：求n个模型中的每个模型的状态矢量与各自加权因子的乘积，再将该n个乘积求和，其中，每个模型的加权因子是贝叶斯先验概率，即：在该模型下其它模型的条件概率。

如图2所示，模型1(融合后)的数据来自于融合前两个模型的数据按照条件概率加权后的和：

x1(t-1)融合后＝x1(t-1)融合前×p融合前|融合后(x1|x1)+x2(t-1)融合前×p融合前|融合后(x2|x1)

同理，模型2(融合后)的数据来自于融合前两个模型的数据按照条件概率加权后的和：

x2(t-1)融合后＝x1(t-1)融合前×p融合前|融合后(x1|x2)+x2(t-1)融合前×p融合前|融合后(x2|x2)

其中，初次滤波时状态矢量是初始值，后续滤波的过程中，状态矢量来自步骤2.3各模型多系统融合的状态矢量反馈。

b)目标定位误差融合

由于滤波中不光用到目标的定位状态信息x，还要用到误差方差p＝(x-x真值)²，所以也需要对定位误差方差进行融合，融合后模型误差是“融合前各模型误差及融合前后信息偏差平方”的加权平均，比如融合后模型1的误差方差为：

p1(t-1)融合后＝{p1(t-1)融合前+(x1(t-1)融合前-x1(t-1)融合后)²}×p融合前|融合后(x1|x1)+{p2(t-1)融合前+(x2(t-1)融合前-x1(t-1)融合后)²}×p融合前|融合后(x2|x1)

融合后模型2的误差方差：

p2(t-1)融合后＝{p1(t-1)融合前+(x1(t-1)融合前-x2(t-1)融合后)²}×p融合前|融合后(x1|x2)+{p2(t-1)融合前+(x2(t-1)融合前-x2(t-1)融合后)²}×p融合前|融合后(x2|x2)

(x1(t-1)融合前-x1(t-1)融合后)²表示融合前后两状态间的误差偏差；

步骤2.2多模型多系统概率计算及多模型多系统滤波

步骤2.2.1多模型多系统概率计算

p{εgps(t)|mcv(t),z^t}——匀速直线运动模型下gps的系统概率，其计算推导是本申请的关键点之一主要依据是贝叶斯理论。其计算方法为：

其中：

p{z(t)|εgps(t),mcv(t),z^t-1}～φ(vgps,cv(t),sgps,cv(t))是在cv模型下，以gps观测系统观测量为输入,进行滤波所得的似然函数。φ代表高斯分布，vgps,cv(t),sgps,cv(t)代表滤波器滤波所获得的新息和新息协方差。同理，p{z(t)|ε基站(t),mcv(t),z^t-1}～φ(v基站,cv(t),s基站,cv(t))，其中v基站,cv(t),s基站,cv(t)为cv模型下，基站观测输入所对应的滤波器的滤波新息和新息协方差。

p{εgps(t)|mcv(t),z^t-1}是cv模型下gps的系统预测概率，其计算依据t-1时刻各系统的系统概率与各系统到gps系统的转移概率乘积之和即：

p{εgps(t)|mcv(t),z^t-1}＝p{εgps(t)|εgps(t-1)}×p{εgps(t-1)|z^t-1}+p{εgps(t)|ε基站(t-1)}×p{ε基站(t-1)|z^t-1}

同理，也可计算获得p{z(t)|ε基站(t),mcv(t),z^t-1}和p{ε基站(t)|mcv(t),z^t-1}；

最后，按照贝叶斯后验概率理论计算获得p{εgps(t)|mcv(t),z^t}，同理也可计算出p{ε基站(t)|mcv(t),z^t}。

步骤2.2.2多模型多系统滤波

针对各个模型对应的融合后状态矢量和状态误差矩阵，分别输入到m个观测系统对应的滤波器中，获得t时刻该模型下各系统的状态矢量；滤波后获得该模型下各观测系统所对应的新息以及新息方差。

以步骤2.1中的双系统为例，融合后的两模型状态信息x1(t-1)融合后和x2(t-1)融合后，分别和两个观测系统的观测信息(gps、移动基站)z1(t)和z2(t)进行滤波。

这里滤波器可以自由选择，不属于本申请的内容，这里采用最简单的扩展卡尔曼滤波器(extendedkalmanfilter-ekf)，该滤波器的详细算法流程在任何一本导航定位的书籍中都能找到，这里只给出滤波计算公式，不展开讲解。

xi(t|t-1)＝aixi(t-1)i＝1,…,n

vi,j(t)＝zj(k)-aixi(t-1)j＝1,…,m

xi(t)＝xi(t|t-1)+ki,j(t)vi,j(t)

步骤2.3各模型多系统融合

“多系统交互”是本申请的关键算法之一，其基本思想是将某一模型下的多个观测系统所对应的滤波结果进行融合。

针对某一个模型下对应的所有观测系统滤波获得的新息和新息方差，通过贝叶斯全概率公式，计算该模型下各系统的概率；以系统概率为加权因子，将各系统状态矢量进行加权求和，获得多系统融合后各个模型的融合结果；同理，以系统概率为加权因子，对系统状态误差进行加权求和，获得多系统融合后各个系统的滤波误差矩阵；按照本方法，将每个模型对应的系统状态矢量和值及系统状态误差和值计算出来；以双模型双系统为例，匀速直线运动模型下的“多系统交互”就是将对应的两个并行滤波器(gps定位系统、移动基站定位系统)所获得的定位信息按照系统概率进行加权融合：

xcv(t|t)＝xgps|cv(t|t)×p{εgps(t)|mcv(t),z^t}+x基站|cv(t|t)×p{ε基站(t)|mcv(t),z^t}

xgps|cv(t|t)是cv模型下，gps观测系统滤波器的定位结果；

x基站|cv(t|t)是cv模型下，基站系统滤波器的定位结果；

xca(t|t)＝xgps|ca(t|t)×p{εgps(t)|mca(t),z^t}+x基站|ca(t|t)×p{ε基站(t)|mca(t),z^t}

xgps|ca(t|t)是ca模型下，gps观测系统滤波器的定位结果；

x基站|ca(t|t)是ca模型下，基站系统滤波器的定位结果；

其中的概率p的计算来自于步骤2.2.1。

其后，将上述获得的xcv(t|t)和xca(t|t)，一方面作为状态矢量反馈到2.1.2多模型状态矢量融合步骤，另一方面作为状态矢量代入步骤2.4.2各模型状态矢量融合中，用于计算多系统多模型的融合结果x(t|t)。

步骤2.4多系统下的各模型概率计算及各模型状态矢量融合

通过步骤2.3所述，实现了在特定模型下的多系统融合，步骤2.4是最后一个步骤，也是本申请的最后一个关键点，就是在多系统融合的条件下，进行多模型的融合。其融合的基本思路是将之前针对每个模型的结果按照模型的后验概率进行加权求和。

步骤2.4.1各模型概率计算

p{mcv(t)|z^t}是t时刻cv模型后验概率；

p{mca(t)|z^t}是t时刻ca模型后验概率；

在2.4.2的加权融合之前，先进行各模型概率计算，模型后验概率的计算依据贝叶斯公式，以p{mcv(t)|z^t}为例进行说明(p{mca(t)|z^t}的推导类似，不再赘述)：

其中：

①p{z(t)|mcv(t),z^t-1}是cv模型的似然概率，可根据贝叶斯全概率公式推导获得：

p{z(t)|mcv(t),z^t-1}＝p{z(t)|εgps,mcv(t),z^t-1}×p{εgps|mcv(t),z^t-1}+p{z(t)|ε基站,mcv(t),z^t-1}×p{ε基站|mcv(t),z^t-1}

而p{z(t)|εgps,mcv(t),z^t-1}和p{εgps|mcv(t),z^t-1}的推导参考2.3节。

②p{mcv(t)|z^t-1}是cv模型的预测概率，可根据贝叶斯全概率公式推导获得

p{mcv(t)|z^t-1}＝p{mcv(t)|mcv(t-1)}×p{mcv(t-1)|z^t-1}+p{mcv(t)|mca(t-1)}×p{mca(t-1)|z^t-1}

步骤2.4.2各模型状态矢量融合

以双系统双模型为例，分别将cv模型下gps和移动基站滤波融合结果xcv(t|t)，和ca模型下gps和移动基站滤波融合结果xca(t|t)，按照模型后验概率进行加权融合：

x(t|t)＝xcv(t|t)×p{mcv(t)|z^t}+xca(t|t)×p{mca(t)|z^t}

xcv(t|t)是cv模型下gps和基站系统的定位融合结果，xca(t|t)是ca模型下gps和基站两系统定位融合结果；概率p来自2.4.1各模型概率计算，x(t|t)是模型后验概率进行加权融合结果。

以上关于多系统多模型融合交互定位融合方法的说明中，采用两模型两系统(ca、cv模型，gps系统、基站系统)进行举例。但该发明适用范围不仅限于两系统两模型，再处理多系统多模型的情况时，方法类似。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本申请的其他实施例。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由权利要求指出。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限。