[发明专利]基于交互式多模型的多平台无序量测处理方法

权利要求书

1.一种基于交互式多模型的多平台无序量测处理方法，其特征在于包括下述步骤：

假定给定系统的状态向量为x，量测向量为z，其线性状态方程和非线性量测方程分别为X_k＝x(k)＝F(k,k-1)x(k-1)+v(k,k-1)和z(k)＝h(x(k),k)+w(k)，其中，F(k,k-1)是状态转移矩阵；v(k,k-1)是服从Gauss分布的零均值过程噪声，h是非线性量测函数，w(k)是服从Gauss分布的零均值量测噪声；过程噪声和量测噪声互不相关，其协方差阵分别为Q(k)和R(k)，状态向量的协方差阵为P_k；

交互式多模型滤波中采用了四种运动模型，包括匀速模型CV、常加速度模型CA、转弯模型CT和当前统计模型CS，四种运动模型的状态转移矩阵依次为F1、F2、F3、F4，过程噪声协方差矩阵依次为Q1、Q2、Q3、Q4；

假设融合周期为T，在起始时刻为t_k-1、结束时刻为t_k的第k个处理周期，融合中心到达了多个量测，若量测产生时刻的时戳早于起始时刻t_k-1，则定义其为逆序点迹，若量测产生时刻的时戳大于等于起始时刻t_k-1，则定义其为顺序点迹；选择顺序点迹中距离结束时刻t_k最近的量测标记为选择逆序点迹中距离起始时刻t_k-1最近的量测标记为其中为传感器1的量测点迹，为传感器2的量测点迹，t代表量测产生时刻的时戳，D表示距离，A表示方位，E表示俯仰；两个传感器在距离、方位、俯仰上的系统误差分别为和t_k＞t₁＞t_k-1＞t₂；

根据各传感器的系统误差，初始化测量噪声协方差矩阵和

根据初始量测z(0)，对初始状态向量X₀和初始协方差P₀进行设置；

针对三维雷达量测，状态向量采用9维列向量，相应的协方差阵即为9*9维方阵，具体地：

X₀＝[x1 0 0 x4 0 0 x7 0 0]；

P₀＝diag([1e6,1e3,1,1e6,1e3,1,1e6,1e3,1])；

其中x1、x4、x7通过对初始量测z₀＝(t₀,D₀,A₀,E₀)进行坐标转换得出，即(x1,x4,x7)＝POLE2XYZ(D₀,A₀,E₀)，POLE2XYZ表示将量测值从极坐标系转至直角坐标系；

对各个顺序点迹按照时间顺序依次利用IMM算法进行滤波；对利用IMM算法进行滤波得到状态估计向量X'_k和协方差向量P′_k，并更新各个模型的模型概率；

基于状态估计向量X'_k和协方差向量P′_k，对利用基于交互式多模型的逆序滤波算法进行滤波，对当前状态估计向量X'_k和协方差向量P′_k进行更新，具体过程如下：

a)利用求逆函数对CA、CS、CT三个模型的状态转移矩阵直接求逆，求CV模型的非奇异子矩阵求逆，然后补维生成最终的状态转移逆矩阵；

b)基于交互式多模型的框架，在各个运动模型的滤波更新阶段，根据一步延迟逆序滤波EKF-A1算法，求从时刻t_k到时刻t₂的向后预测状态向量和协方差阵，对当前状态估计向量X'_k和协方差向量P′_k进行更新，最终求出状态估计向量X_k和协方差向量P_k，模型概率不变；

5)输出滤波结果，即状态估计向量X_k和协方差向量P_k。