[发明专利]一种带自适应因子的交互式多模型UKF的卫星导航方法

权利要求书

1.一种带自适应因子的交互式多模型UKF的卫星导航方法，其特征在于：具体包括如下步骤：

步骤1：建立系统模型；

步骤1.1：建立基于全球定位系统GPS的地面载体的状态方程；

为全面描述载体的运动状态和钟差漂移频率，用符号X表示全球定位系统GPS状态方程的状态变量，X=[x,x·,x,··y,y·,y··,z,z·,z,··δtu,δt·u]T;]]>其中，x，分别代表WGS-84坐标系下X_T轴上的位置、速度和加速度；y，分别代表WGS-84坐标系下Y_T轴上的位置、速度和加速度；z，分别代表WGS-84坐标系下Z_T轴上的位置、速度和加速度；δt_u和分别代表载体接收机钟差和钟差漂移频率；建立模型集M，模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT，即M＝[CS,CA,CT]；接收机钟差变化率用一阶马尔科夫模型描述，当前统计模型CS的状态方程如公式(1)所示；常加速度模型CA的状态方程如公式(2)所示；常速转弯模型CT的状态方程如公式(3)所示；

Xk1=Φ1Xk-11+Ψψk-11wk-11---(1)]]>

Xk2=Φ2Xk-12+Ψk-12wk-12---(2)]]>

Xk3=Φ3Xk-13+Ψk-13wk-13---(3)]]>

其中，分别表示系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态，其中i＝1,2,3，即：表示系统在当前统计模型CS下k时刻的状态；表示系统在常加速度模型CA下k时刻的状态；表示系统在常速转弯模型CT下k时刻的状态；k为正整数；Φ_i为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵，Φ_i＝diag(Φ_i,x,Φ_i,y,Φ_i,z,Φ_t,u)，Φ_i，x表示WGS-84坐标系下X_T轴上的状态转移矩阵；Φ_i,y表示WGS-84坐标系下Y_T轴上的状态转移矩阵；Φ_i,z表示WGS-84坐标系下Z_T轴上的状态转移矩阵；Φ_t,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵；是系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子；是系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵；

用符号分别表示系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k-1)时刻的过程噪声协方差矩阵，E[·]表示期望值运算，表示WGS-84坐标系下描述X_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Y_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS-84坐标系下描述Z_T轴上载体运动状态的过程噪声协方差矩阵；表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵；

步骤1.2：建立基于全球定位系统GPS的地面载体的观测方程；

采用伪距作为观测量建立观测方程；在k时刻，接收机观测到的GPS卫星数量用N表示，则建立此时刻CS模型，CA模型和CT模型的伪距观测方程如公式(4)所示；

ρnk=(xki-xnk)2+(yki-ynk)2+(zki-znk)2+δtuk+ϵnk---(4)]]>

其中，表示在k时刻观测到的第n颗GPS卫星的伪距，n＝1,2…N，N表示k时刻观测到的GPS卫星的总数；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下X_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Y_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下Z_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下X_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Y_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下Z_T轴上的位置；表示k时刻地面载体接收机钟差；表示k时刻观测到的第n颗GPS卫星的观测误差；

为下文描述算法方便，建立CS模型，CA模型和CT模型的观测方程为公式(5)；

yki=h(Xki)+ϵk---(5)]]>

其中，，为系统的观测量，h(Xki)=(xki-x1k)2+(yki-y1k)2+(zki-z1k)2+δtuk(xki-x2k)2+(yki-y2k)2+(zki-z2k)2+δtuk···(xki-xnk)2+(yki-ynk)2+(zki-znk)2+δtuk,]]>观测方程中的非线性函数，为系统的观测误差；

用R_k表示公式(5)的观测误差的协方差矩阵，

步骤2：设置CS模型，CA模型和CT模型0时刻的滤波初始值，同时设置当前定位任务的结束时间；

在步骤1操作的基础上，分别设置CS模型、CA模型和CT模型的状态的估计初值，用符号和表示；然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值，用符号和表示；并设置系统运行的结束时间；

步骤3：得到CS模型，CA模型和CT模型在k时刻的滤波初始值；

使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k-1)时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合，分别得到CS模型，CA模型和CT模型中k时刻的状态的估计初值，分别用和表示，以及CS模型，CA模型和CT模型中k时刻的估计误差协方差矩阵初值，分别用和表示；

X^k-10j=Σi=13X^i=1iuk-1|k-1i|j---(6)]]>

其中，分别表示CS模型，CA模型和CT模型在k时刻滤波的状态初值；j＝1,2,3，分别代表CS模型、CA模型和CT模型；i＝1,2,3，分别表示CS模型，CA模型和CT模型在k-1时刻状态的最优估计值；表示k-1时刻模型i和模型j的混合概率，π_ij为模型i到模型j的模型转移概率，其值由人为设定；为模型i在k时刻的概率，的初值由人为设定，并在以后各个时刻由步骤6更新；代表模型j的归一化常数，

Pk-10j=Σi=13uk-1|k-1i|j[Pk-1i+(X^k-1i-X^k-10j)(X^k-1i-X^k-10j)T]---(7)]]>

其中，i＝1,2,3，分别表示CS模型、CA模型和CT模型在k-1时刻滤波误差的方差矩阵；分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值；

步骤4：对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF，得到各模型在k时刻的滤波值；

步骤5：计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵

步骤6：在步骤5操作基础上，通过公式(27)至公式(29)对模型概率更新；

ukj=Λkjc‾jc---(8)]]>

其中，为模型j在k时刻的似然函数，可通过公式(28)计算得到；c为归一化常数，可通过公式(29)计算得到；

Λkj12π|Pyyj|exp[-0.5(υkj)T(Pyyj)-1υkj]---(9)]]>

其中exp[·]表示以自然常数e为底的指数函数；

c=Σj=13Λkjc‾j---(10)]]>

为模型j的归一化常数，π_ij为模型i到模型j的转移概率，其值由认为设定，为k-1时刻的模型概率，在0时刻由人为设定，并在以后每一时刻通过公式(27)更新；

步骤7：在步骤6操作基础上，进行信息融合,得到k时刻滤波器的输出；

X^k=Σj=13ukjX^kj---(11)]]>

Pk=Σj=13ukj[Pkj+(X^kj-X^k)(X^kj-X^k)T]---(12)]]>

其中为采用交互式多模型自适应UKF算法得到的系统最优估计值，P_k为采用IMM-AUKF算法得到的状态最优估计值的误差协方差矩阵；

步骤8：判断当前时刻是否到达步骤2设置的当前定位任务的结束时间，如果到达，则结束操作；否则，在步骤6操作基础上，将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值，重复执行步骤2至步骤7的操作，计算下一时刻滤波器的输出值。