CN101692001B - 一种借力飞行轨道上深空探测器的自主天文导航方法 - Google Patents

Abstract

本发明涉及一种借力飞行轨道上深空探测器的自主天文导航方法。根据距离行星的远近建立相应的状态方程，利用星敏感器敏感的星光角距和用纯天文几何解析方法解算得到的位置信息，通过一种模糊多模自适应UKF(FMMUKF)方法进行导航系统参数的估计。本发明适用于借力飞行轨道上的深空探测器的导航定位，属于航天导航技术领域，可应用于多天体交会借力飞行轨道深空探测器导航参数的确定。

Description

一种借力飞行轨道上深空探测器的自主天文导航方法

技术领域

本发明涉及一种深空探测器在借力飞行轨道上的导航方法，可用于在多天体交会借力飞行轨道上深空探测器导航参数的精确确定。

背景技术

借力飞行轨道不同于自由飞行轨道，它利用第二引力体来改变探测器相对中心引力体的能量，从而改变探测器速度的大小或方向，以节省发射能量，用较小的初始速度，在没有任何动力消耗的情况下实现对探测器的加速，完成探测任务，甚至可以实现目前利用运载火箭搭载的探测器所无法实现的远距离探测任务。借力飞行轨道虽然具有要求的发射能量小，一次发射可实现多个任务等优点，但其缺点是设计很复杂，探测器在借力时轨道变化很快，其轨道模型难以精确建立。

目前常用的纯天文几何解析方法可以通过在探测器上观测的天体方位信息和近天体的位置信息，确定探测器的姿态和位置信息，但是不能直接获得探测器的速度信息，而且计算得到的位置信息的精度受量测噪声的影响较大。虽然轨道精度可以满足轨道初始设计与任务验证的要求，但是对导航滤波则是不够的，会导致滤波发散，带来巨大的导航误差。

现有的借力飞行轨道上的深空探测器纯天文几何解析方法与滤波方法的结合可以解决系统模型的问题，对探测器在借力时的轨道变化建立了多个模型，并通过模型的切换，反映探测器的运动规律。但仍存在很多不足：(1)状态模型的切换与实际的模型有误差，滤波器对模型自身精度自适应能力低，模型精度变化时，滤波器并不能跟踪这种变化，导致不能保证最终的滤波精度；(2)滤波器对量测噪声的自适应能力低，这是因为使用的量测信息是通过纯天文几何解算得到的位置信息，而不是直接的天文量测量，导致滤波器中的量测噪声协方差阵不只是测量仪器的量测噪声方差，还存在其他各种计算误差，而这些误差会受到测量误差、导航恒星之间及其与探测器之间的几何关系和探测器相对太阳、火星的距离等多种因素的影响，这些因素使得量测噪声协方差阵不是一个定常矩阵，导致滤波器不能实时跟踪量测噪声的变化。

综上，由于目前借力飞行轨道上深空探测器导航信息的确定常用纯天文几何解析方法单独导航定位，而且不同状态模型精度不同，模型自身精度也在不断变化，导致导航精度低；且由于纯天文几何解析方法精度受量测噪声影响大，导致滤波方法不能实时跟踪量测噪声的变化，不能保证导航精度。

发明内容

本发明要解决的技术问题是：克服纯天文几何解析方法、传统滤波方法、以及现有二者结合的方法的不足，利用星光角距和纯天文解析方法解算得到的位置信息，通过模糊多模自适应UKF方法进行导航系统参数的估计，提供了一种精度高、自适应能力强的借力飞行轨道上深空探测器的导航方法。

本发明解决其技术问题所采用的技术方案为：针对借力飞行轨道上的深空探测器，根据距离行星的远近建立相应的状态方程，利用星敏感器敏感的星光角距和用纯天文几何解析方法解算得到的位置信息，通过一种模糊多模自适应UKF(FMMUKF)方法进行导航系统参数的估计。

具体包括以下步骤：

1.通过判断深空探测器距离行星的远近，分别建立借力飞行轨道上深空探测器的导航系统第一状态方程和第二状态方程；若深空探测器在天体影响球之外，建立基于四体轨道动力学模型的第一状态方程为

若深空探测器在天体影响球之内，建立基于二体受摄轨道动力学模型的第二状态方程

式中，

分别为两个模型状态变量的微分项，f₁(X，t)、f₂(X，t)分别为两个模型的非线性连续状态转移函数，w₁(t)、w₂(t)分别为两个状态模型的噪声。

2.建立以星光角距和纯天文几何解析解算得到的位置信息为量测量的量测方程为：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\arccos (-\frac{r_{p1}\·s}{r_{p1}})+v_{{\mathrm{\α}}_1}\\ {\mathrm{\α}}_2=\arccos (-\frac{r_{p2}\·s}{r_{p2}})+v_{{\mathrm{\α}}_2}\\ {\mathrm{\α}}_3=\arccos (-\frac{r_{p3}\·s}{r_{p3}})+v_{{\mathrm{\α}}_3}\\ X_c=x+v_x\\ Y_c=y+v_y\\ Z_c=z+v_z\end{array}\right.---\left(1\right)$

可简写为：

Z(t)＝h(X(t)，t)+v(t)(2)

由于量测噪声方差阵不确定，量测方程则使用模型集：Z_i(t)＝h_i(X_i(t)，t)+v_i(t)，其中量测模型噪声的协方差阵E[v_i(k)v_i(k)^T]＝R_i，i＝1，2，…，N N为设置的模型数目，由量测噪声的种类决定，为2-20个。

式中，α₁，α₂，α₃分别为是观测的三颗恒星与探测器之间的星光角距，r_p1、r_p2和r_p3分别为观测的三颗恒星质心到探测器的位置矢量，s为恒星星光方向的单位矢量，由星敏感器识别，

为星光角距量测噪声；X_c，Y_c，Z_c为纯天文几何解算得到的位置信息；v_x，v_y，v_z为计算误差，Z(t)＝[α₁，α₂，α₃，X_c，Y_c，Z_c]^T为量测量，h(X(t)，t)为非线性连续量测函数，

为量测噪声，式中所有变量均为与t有关的变量。

利用深空探测器的纯天文解析方法解算的位置信息和星敏感器量测得到的星光角距共同作为量测信息；在轨道模型不精确时，利用纯天文几何解算获得的结果修正轨道模型；反之，则利用轨道模型修正几何解算的计算误差。

3.对第一状态方程和第二状态方程及量测方程进行离散化

X₁(k+1)＝F₁(X(k)，k)+w₁(k)(3)

或X₂(k+1)＝F₂(X(k)，k)+w₂(k)(4)

Z_i(k)＝H_i(X_i(k)，k)+v_i(k)，i＝1，2…N    (5)

式中，k＝1，2，…，F₁(X(k)，k)，F₂(X(k)，k)分别为f₁(X，t)，f₂(X，t)离散化后的非线性状态转移函数，H_i(X_i(k)，k)为h_i(X_i(t)，t)离散化后的非线性量测函数，w₁(k)，w₂(k)和v_i(k)互不相关。

4.用状态方程与量测方程进行FMMUKF滤波，以获得借力飞行轨道上深空探测器的导航定位信息。FMMUKF滤波方法，采用模糊方法计算权重，即以标称状态值和量测值定义模糊集合，然后根据各子模型得到的状态估计值

和量测值Z_i(k)计算各个模型的隶属度w_qi(k)，然后把这个隶属度作为所使用模型的权重，与实际模型相符合的子模型就获得了较大的权重；对权值较小的子模型进行舍弃，并对权值进行归一化；可以实现在保证滤波精度的前提下实现两个状态模型的切换；提高滤波器对状态模型精度以及对量测噪声的自适应能力。

5.输出位置，速度信息。

$\hat{X}\left(k\right)=\underset{i}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right){\hat{X}}_i\left(k\right)---\left(6\right)$

$P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right)[P_i\left(k\right)+\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}\·{\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}}^T]---\left(7\right)$

式中，

和P(k)分别为k时刻的状态变量估计值和估计方差，其中

分别是对借力飞行轨道上深空探测器在X、Y、Z三个方向的位置和速度x，y，z，v_x，v_y，v_z的估计；并输出估计方差阵

其中

分别是借力飞行轨道上深空探测器在X、Y、Z三个方向位置和速度的估计方差；

P_i(k)分别为k时刻的第i个子模型的状态估计值和估计方差，w_qi(k)为第i个子模型的隶属度，

和P(k)分别为k时刻的状态变量估计值和估计方差。

本发明的原理：本发明适用于借力飞行轨道上的深空探测器，利用第二引力体来改变探测器相对中心引力体的能量，从而改变探测器速度的大小或方向，以节省发射能量，用较小的初始速度，在没有任何动力消耗的情况下实现对探测器的加速，完成探测任务；其中探测器借力飞行所利用的第二引力体可以为太阳系中的任意行星；发明以火星为第二引力体，探测器向火星借力为例，以清楚的说明本发明原理。

首先精确建立深空探测器在借力飞行轨道上的状态方程，然后以星光角距和纯天文几何解算得到的位置信息建立系统的量测方程，最后采用模糊多模自适应UKF(FMMUKF)方法进行状态估计，以下以对火星借力的探测器为例对原理进行描述，当探测器借力其他天体时可采用类似的方法进行分析：

状态模型：

深空探测器对火星进行借力飞行时，其轨道运动可近似分为两段：①当探测器在火星影响球之外时，为以太阳为主引力体的多体运动模型，②当探测器在火星影响球之内时，为以火星为主引力体的受摄二体运动模型。

当探测器在火星影响球之外时，选取历元(J2000.0)日心惯性坐标系，此时探测器的第一状态方程(四体轨道动力学模型，第一模型)可表示为

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.---\left(8\right)$

式中μ_s，μ_m和μ_e分别为太阳、火星和地球的引力常数。在日心惯性坐标系中，r_se和r_sm分别为地球和火星的位置矢量，r_pe为地心到探测器的位置矢量，r_pm为火星质心到探测器的位置矢量，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和火星探测器的位置坐标，(v_x，v_y，v_z)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，w_x，w_y，w_z为状态模型误差。

当探测器在火星影响球之内时，选取历元(J2000.0)火星质心惯性坐标系，此时探测器的第二状态模型(二体受摄轨道动力学模型，第二模型)

$\left\{\begin{array}{c}{\stackrel{\·}{x}}^{\′}=v_x^{\′}+w_x\\ {\stackrel{\·}{y}}^{\′}=v_y^{\′}+w_y\\ {\stackrel{\·}{z}}^{\′}=v_z^{\′}+w_z\\ {\stackrel{\·}{v}}_x^{\′}=-{\mathrm{\μ}}_m\frac{x^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{x^{\′}-x_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{x_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{x^{\′}-x_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{x_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_x}\\ {\stackrel{\·}{v}}_y^{\′}=-{\mathrm{\μ}}_m\frac{y^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{y^{\′}-y_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{y_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{y^{\′}-y_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{y_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_y}\\ {\stackrel{\·}{v}}_z^{\′}=-{\mathrm{\μ}}_m\frac{z^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{z^{\′}-z_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{z_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{z^{\′}-z_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{z_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_z}\end{array}\right.---\left(9\right)$

在火心惯性坐标系中，r′_ms和r′_me分别为太阳和地球的位置矢量，r′_ps为太阳质心到探测器的位置矢量，r′_pe为地心到探测器的位置矢量，(x′₁，y′₁，z′₁)，(x′₂，y′₂，z′₂)和(x′，y′，z′)分别为太阳、地球和火星探测器的位置坐标，(v_x′，v_y′，v_z′)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，

为状态模型误差。

由于两个模型使用的坐标系不同，在导航计算时必须转换到统一的坐标系中，本发明选择日心惯性坐标系作为基准坐标系。

分别简写为

${\stackrel{\·}{X}}_1\left(t\right)=f_1(X,t)+w_1\left(t\right)---\left(10\right)$

或 ${\stackrel{\·}{X}}_2\left(t\right)=f_2(X,t)+w_2\left(t\right)---\left(11\right)$

分别为两个模型状态变量的微分项，f₁(X，t)、f₂(X，t)分别为两个模型的状态转移函数，X＝(x，y，z，v_x，v_y，v_z)^T，w₁(t)、w₂(t)分别为两个状态模型噪声，两个状态模型噪声协方差阵分别为E[w₁(k)w₁(k)^T]＝Q₁，E[w₂(k)w₂(k)^T]＝Q₂；当探测器在火星影响球之外时，即当r_pm＞5.773×10⁶km时，使用第一模型；当探测器在火星影响球之内，即当r_pm≤5.773×10⁶km时，使用第二模型，同时Q₁，Q₂均随r_pm的减小成比例增大。

量测模型：

本发明是以火星借力的深空探测器为例，因此在利用纯天文几何解析方法进行解算时，观测量选为火星以及火卫一(Phobos)和恒星之间的星光角距。本发明使用纯天文几何解算得到的位置信息，即探测器在日心惯性坐标系中的三轴位置辅助星光角距作为观测量，如图2所示，图中r_ps、r_pe、r_pm分别为日心、地心、火星质心到探测器的位置矢量，s为恒星星光方向的单位矢量，由星敏感器测量得到，α_s，α_e，α_m为太阳、地球、火星与导航星之间的星光角距，因此导航系统的量测模型为

$\left\{\begin{array}{c}{\mathrm{\α}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s}{r_{\mathrm{ps}}})+v_{{\mathrm{\α}}_s}\\ {\mathrm{\α}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s}{r_{\mathrm{pe}}})+v_{{\mathrm{\α}}_e}\\ {\mathrm{\α}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s}{r_{\mathrm{pm}}})+v_{{\mathrm{\α}}_m}\\ X_c=x+v_x\\ Y_c=y+v_y\\ Z_c=z+v_z\end{array}\right.---\left(12\right)$

式中，α_s，α_e，α_m分别为太阳、地球、火星与导航星之间的星光角距，s为恒星星光方向的单位矢量，由星敏感器识别；

为星光角距量测噪声；X_c，Y_c，Z_c为纯天文几何解算得到的位置信息；v_x，v_y，v_z为计算误差，式中所有变量均为与t有关的变量；

可简写为

Z(t)＝h(X(t)，t)+v(t)(13)

式中，Z(t)＝[α₁，α₂，α₃，X_c，Y_c，Z_c]^T为量测量，h(X(t)，t)为非线性连续量测函数，

为量测噪声；由于量测噪声方差阵不确定，量测方程则使用模型集：Z_i(t)＝h_i(X_i(t)，t)+v_i(t)，其中量测模型噪声的协方差阵E[v_i(k)v_i(k)^T]＝R_i，i＝1，2，…，N，N为设置的模型数目，由量测噪声的种类决定，为2-20个，需满足导航精度和实时性的要求，一般导航精度中的位置精度为7km，速度精度为0.2m/s，本实施例按照此要求取N为10。

对第一状态方程和第二状态方程及量测方程进行离散化

X₁(k+1)＝F₁(X(k)，k)+w₁(k)(14)

或X₂(k+1)＝F₂(X(k)，k)+w₂(k)(15)

Z_i(k)＝H_i(X_i(k)，k)+v_i(k)(16)

式中，k＝1，2，…，i＝1，2，…，N，F₁(X(k)，k)，F₂(X(k)，k)分别为F₁(X，t)，F₂(X，t)离散化后的非线性状态转移函数，H_i(X_i(k)，k)为h_i(X_i(t)，t)离散化后的非线性量测函数，w₁(k)，w₂(k)和v_i(k)互不相关。

对于上述的多模型系统，由于状态模型为非线性，为减小其线性化误差，对应k＝1，2…的每一个时刻，采用UKF对N个模型进行状态估计，得到N个状态估计值和状态估计值的估计方差

${\hat{X}}_i\left(k\right)={\left[\begin{array}{cccccc}{\hat{x}}_i\left(k\right)& {\hat{y}}_i\left(k\right)& {\hat{z}}_i\left(k\right)& {\hat{v}}_{\mathrm{xi}}\left(k\right)& {\hat{v}}_{\mathrm{yi}}\left(k\right)& {\hat{v}}_{\mathrm{zi}}\left(k\right)\end{array}\right]}^T,i=\mathrm{1,2},\·\·\·,N---\left(17\right)$

$P_i\left(k\right)={\left[\begin{array}{cccccc}{p}_{\mathrm{xi}}\left(k\right)& p_{\mathrm{yi}}\left(k\right)& p_{\mathrm{zi}}\left(k\right)& p_{v_{\mathrm{xi}}}\left(k\right)& p_{v_{\mathrm{yi}}}\left(k\right)& p_{v_{\mathrm{zi}}}\left(k\right)\end{array}\right]}^T,i=\mathrm{1,2},\·\·\·,N---\left(18\right)$

然后需要根据各子模型与实际模型的匹配程度确定权值，进行全局融合估计。传统求取权值的方法是取

$w_{\mathrm{qi}}\left(k\right)=\frac{p\left(Z_i\left(k\right)\right|{\hat{X}}_i\left(k\right))p\left({\hat{X}}_i\left(k\right)\right|\hat{X}(k-1))}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}p\left(Z_i\left(k\right)\right|{\hat{X}}_i\left(k\right))p\left({\hat{X}}_i\left(k\right)\right|\hat{X}(k-1))},i=\mathrm{1,2},\·\·\·,N---\left(19\right)$

其中w_qi(k)为模型的权值，为k时刻的状态变量估计值，

分别是k时刻对借力飞行轨道上深空探测器在X、Y、Z三个方向的位置和速度x(k)，y(k)，z(k)，v_x(k)，v_y(k)，v_z(k)的估计；

为满足

条件时，Z_i(k)的概率，

为满足

条件时，

的概率。

但对本发明的应用对象而言，在一段时间内，虽然仅有少数子模型与实际模型相符合，但由于各子模型的差异仅在于量测噪声的误差阵不同，因此采用上面的权值计算方法所得到的权值不能较好的反映各子模型与实际模型的匹配程度。为了解决该问题，本发明采用模糊方法计算权重，即以标称状态值和量测值定义模糊集合，然后根据各子模型得到的状态估计值和量测值计算各个模型的隶属度，然后把这个隶属度作为所使用模型的权重。即令

$w_{\mathrm{qx}}^i\left(k\right)=G\{{\hat{X}}_i\left(k\right);X(k-1),Q\}=e^{-\frac{1}{2}{\left(\frac{{|\hat{X}}_i\left(k\right)-X(k-1)|}{\left|Q\right|}\right)}^2}---\left(20\right)$

$w_{\mathrm{qz}}^i\left(k\right)=G\left\{H\right[{\hat{X}}_i\left(k\right),k];Z_i\left(k\right),R_i\}=e^{-\frac{1}{2}{\left(\frac{|Z_i\left(k\right)-H[{\hat{X}}_i\left(k\right),k\left]\right|}{\left|R_i\right|}\right)}^2}---\left(21\right)$

$w_{\mathrm{qi}}\left(k\right)=w_{\mathrm{qx}}^i\left(k\right)w_{\mathrm{qz}}^i\left(k\right)---\left(22\right)$

式中，

为第i个模型状态变量的隶属度，

为第i个模型量测值的隶属度，G代表高斯函数，R_i为量测模型噪声的协方差阵E[v_i(k)v_i(k)^T]＝R_i，Q为状态模型噪声协方差阵E[W(k)W(k)^T]＝Q。并对权值进行归一化后，系统最终的导航信息，包括位置和速度信息可由下式得到

$\hat{X}\left(k\right)=\underset{i}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right){\hat{X}}_i\left(k\right)---\left(23\right)$

$P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right)[P_i\left(k\right)+\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}\·{\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}}^T]---\left(24\right)$

其中，

的估计，其中分别是对借力飞行轨道上深空探测器在X、Y、Z三个方向的位置和速度x，y，z，v_x，v_y，v_z的估计；并输出估计方差矩阵

其中

分别是借力飞行轨道上深空探测器在X、Y、Z三个方向位置和速度的估计方差。P_i(k)分别为k时刻的第i个子模型的状态估计值和估计方差；w_qi(k)为第i个子模型的隶属度。

本发明与现有技术相比的优点在于：(1)本发明将纯天文几何解析方法和滤波方法相结合利用FMMUKF滤波方法，在轨道模型不精确时，利用纯天文几何解算获得的结果修正轨道模型；反之，当轨道模型精度较高时则利用轨道模型修正几何解算的计算误差；可以有效地克服当探测器由于近距离飞越行星借力飞行时所带来的状态模型不准确的问题，实现自适应跟踪状态模型的变化，改善了导航精度；(2)通过根据量测噪声不同，设置多个量测模型，实现自适应跟踪量测噪声变化，保证滤波收敛，提高导航系统的精度和可靠性，实现借力飞行轨道上深空探测器的精确定位。

附图说明

图1为本发明的流程图。

图2为探测器天文导航的观测量示意图。

具体实施方式

如图1所示，本发明的具体实施方法如下：

1、首先初始化探测器位置和速度信息，通过判断深空探测器距离行星的远近，确定是否在天体影响球内，分别建立借力飞行轨道上深空探测器的导航系统两种状态方程。

以对火星借力的探测器为例，当探测器借力其他天体时可采用类似的方法进行分析。深空探测器对火星进行借力飞行时，其轨道运动可近似分为两段：①当探测器在火星影响球之外时，为以太阳为主引力体的多体运动模型，②当探测器在火星影响球之内时，为以火星为主引力体的受摄二体运动模型。

初始化位置、速度，按如下方程建立轨道动力学模型(系统状态方程)：

当探测器在火星影响球之外时，选取历元(J2000.0)日心惯性坐标系，此时探测器的第一状态方程(四体轨道动力学模型，第一模型)可表示为

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.---\left(25\right)$

式中，μ_s，μ_m和μ_e分别为太阳、火星和地球的引力常数。在日心惯性坐标系中，r_se和r_sm分别为地球和火星的位置矢量，r_pe为地心到探测器的位置矢量，r_pm为火星质心到探测器的位置矢量，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和火星探测器的位置坐标，(v_x，v_y，v_z)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，w_x，w_y，w_z为状态模型噪声。

当探测器在火星影响球之内时，选取历元(J2000.0)火星质心惯性坐标系，此时探测器的第二状态方程(二体受摄轨道动力学模型，第二模型)

$\left\{\begin{array}{c}{\stackrel{\·}{x}}^{\′}=v_x^{\′}+w_x\\ {\stackrel{\·}{y}}^{\′}=v_y^{\′}+w_y\\ {\stackrel{\·}{z}}^{\′}=v_z^{\′}+w_z\\ {\stackrel{\·}{v}}_x^{\′}=-{\mathrm{\μ}}_m\frac{x^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{x^{\′}-x_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{x_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{x^{\′}-x_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{x_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_x}\\ {\stackrel{\·}{v}}_y^{\′}=-{\mathrm{\μ}}_m\frac{y^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{y^{\′}-y_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{y_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{y^{\′}-y_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{y_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_y}\\ {\stackrel{\·}{v}}_z^{\′}=-{\mathrm{\μ}}_m\frac{z^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{z^{\′}-z_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{z_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{z^{\′}-z_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{z_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_z}\end{array}\right.---\left(26\right)$

在火心惯性坐标系中，r′_ms和r′_me分别为太阳和地球的位置矢量，r′_ps为太阳质心到探测器的位置矢量，r′_pe为地心到探测器的位置矢量，(x′₁，y′₁，z′₁)，(x′₂，y′₂，z′₂)和(x′，y′，z′)分别为太阳、地球和火星探测器的位置坐标，(v_x′，v_y′，v_z′)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，

为状态模型噪声。

由于两个模型使用的坐标系不同，在导航计算时必须转换到统一的坐标系中，本发明选择日心惯性坐标系作为基准坐标系。

分别简写为

${\stackrel{\·}{X}}_1\left(t\right)=f_1(X,t)+w_1\left(t\right)---\left(27\right)$

或 ${\stackrel{\·}{X}}_2\left(t\right)=f_2(X,t)+w_2\left(t\right)---\left(28\right)$

其中，

分别为两个模型状态变量的微分项，f₁(X₁，t)、f₂(X₂，t)分别为两个模型的非线性连续状态转移函数，X＝(x，y，z，v_x，v_y，v_z)^T，w₁(t)、w₂(t)分别为两个状态模型噪声，协方差阵分别为E[w₁(k)w₁(k)^T]＝Q₁，E[w₂(k)w₂(k)^T]＝Q₂；当探测器在火星影响球之外时，即当r_pm＞5.773×10⁶km时，使用第一模型；当探测器在火星影响球之内，即当r_pm≤5.773×10⁶km时，使用第二模型，同时Q₁，Q₂均随r_pm的减小成比例增大。

2、建立以星光角距和纯天文几何解析解算得到的位置信息为量测量的量测方程。

本发明是以火星借力的深空探测器为例，因此在利用纯天文几何解析方法进行解算时，观测量选为火星以及火卫一(Phobos)和恒星之间的星光角距。本发明提出的新方法，使用纯天文几何解算得到的位置信息，即探测器在日心惯性坐标系中的三轴位置辅助星光角距作为观测量，如图2所示，因此导航系统的量测模型为

$\left\{\begin{array}{c}{\mathrm{\α}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s}{r_{\mathrm{ps}}})+v_{{\mathrm{\α}}_s}\\ {\mathrm{\α}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s}{r_{\mathrm{pe}}})+v_{{\mathrm{\α}}_e}\\ {\mathrm{\α}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s}{r_{\mathrm{pm}}})+v_{{\mathrm{\α}}_m}\\ X_c=x+v_x\\ Y_c=y+v_y\\ Z_c=z+v_z\end{array}\right.---\left(29\right)$

式中，α_s，α_e，α_m分别为太阳、地球、火星与导航星之间的星光角距，s为恒星星光方向的单位矢量，由星敏感器识别；

为星光角距量测噪声；X_c，Y_c，Z_c为纯天文几何解算得到的位置信息；v_x，v_y，v_z为计算误差；

可简写为

Z(t)＝h(X(t)，t)+v(t)(30)

式中，Z(t)＝[α₁，α₂，α₃，X_c，Y_c，Z_c]^T为量测量，h(X(t)，t)为非线性连续量测函数，

为量测噪声

由于量测噪声方差阵不确定，量测方程则使用模型集：

Z_i(t)＝h_i(X(t)，t)+V_i(t)(31)

其中量测模型噪声的协方差阵E[v_i(k)V_i(k)^T]＝R_i，i＝1，2，…，N。

3、对以上状态方程及两个量测方程进行离散化。

X_i(k+1)＝F₁(X(k)，k)+w₁(k)(32)

或X₂(k+1)＝F₂(X(k)，k)+w₂(k)(33)

Z_i(k)＝H_i(X_i(k)，k)+v_i(k)(34)

式中，k＝1，2，…，i＝1，2…N，F₁(X(k)，k)，F₂(X(k)，k)分别为f₁(X，t)，f₂(X，t)离散化后的非线性状态转移函数，H_i(X_i(k)，k)为h_i(X_i(t)，t)离散化后的非线性量测函数，w₁(k)，w₂(k)和v_i(k)互不相关。

4、用状态方程与量测方程进行FMMUKF滤波。

对于上述的多模型系统，由于状态模型为非线性，为减小其线性化误差，对应k＝1，2…的每一个时刻，采用UKF对N个模型进行状态估计，得到N个状态估计值和状态估计值的估计方差

${\hat{X}}_i\left(k\right)={\left[\begin{array}{cccccc}{\hat{x}}_i\left(k\right)& {\hat{y}}_i\left(k\right)& {\hat{z}}_i\left(k\right)& {\hat{v}}_{\mathrm{xi}}\left(k\right)& {\hat{v}}_{\mathrm{yi}}\left(k\right)& {\hat{v}}_{\mathrm{zi}}\left(k\right)\end{array}\right]}^T,i=\mathrm{1,2},\·\·\·,N---\left(35\right)$

$P_i\left(k\right)={\left[\begin{array}{cccccc}{p}_{\mathrm{xi}}\left(k\right)& p_{\mathrm{yi}}\left(k\right)& p_{\mathrm{zi}}\left(k\right)& p_{v_{\mathrm{xi}}}\left(k\right)& p_{v_{\mathrm{yi}}}\left(k\right)& p_{v_{\mathrm{zi}}}\left(k\right)\end{array}\right]}^T,i=\mathrm{1,2},\·\·\·,N---\left(36\right)$

然后需要根据各子模型与实际模型的匹配程度确定权值，进行全局融合估计。

为满足

条件时，Z_i(k)的概率，

为满足条件时，的概率。

本发明采用模糊方法计算权重，即以标称状态值和量测值定义模糊集合，然后根据各子模型得到的状态估计值和量测值计算各个模型的隶属度，然后把这个隶属度作为所使用模型的权重。即令

$w_{\mathrm{qx}}^i\left(k\right)=G\{{\hat{X}}_i\left(k\right);X(k-1),Q\}=e^{-\frac{1}{2}{\left(\frac{{|\hat{X}}_i\left(k\right)-X(k-1)|}{\left|Q\right|}\right)}^2}---\left(37\right)$

$w_{\mathrm{qz}}^i\left(k\right)=G\left\{H\right[{\hat{X}}_i\left(k\right),k];Z_i\left(k\right),R_i\}=e^{-\frac{1}{2}{\left(\frac{|Z_i\left(k\right)-H[{\hat{X}}_i\left(k\right),k\left]\right|}{\left|R_i\right|}\right)}^2}---\left(38\right)$

$w_{\mathrm{qi}}\left(k\right)=w_{\mathrm{qx}}^i\left(k\right)w_{\mathrm{qz}}^i\left(k\right)---\left(39\right)$

式中，为第i个模型状态变量的隶属度，

为第i个模型量测值的隶属度，G代表高斯函数，R_i为量测模型噪声的协方差阵E[v_i(k)v_i(k)^T]＝R_i，Q为状态模型噪声协方差阵E[W(k)W(k)^T]＝Q。这样那些与实际模型相符合的子模型就获得了较大的权重。对权值较小的子模型进行舍弃(令其w_i(k)＝0)，并对权值进行归一化后，

5、输出位置，速度信息。

按照上述步骤1～4，建立轨道动力学方程，量测方程利用信息融合即可完成对借力飞行轨道上深空探测器的位置、速度估计。

系统输出的导航信息可由下式得到

$\hat{X}\left(k\right)=\underset{i}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right){\hat{X}}_i\left(k\right)---\left(40\right)$

$P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right)[P_i\left(k\right)+\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}\·{\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}}^T]---\left(41\right)$

输出状态矢量X＝[x y z v_x v_y v_z]^T的估计值

其中

分别是对借力飞行轨道上深空探测器在X、Y、Z三个方向的位置和速度x，y，z，v_x，v_y，v_z的估计；并输出估计方差矩阵

其中

分别是借力飞行轨道上深空探测器在X、Y、Z三个方向位置和速度的估计方差。

P_i(k)分别为k时刻的第i个子模型的状态估计值和估计方差；w_qi(k)为第i个子模型的隶属度。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种借力飞行轨道上深空探测器的自主天文导航方法，其特征在于：针对借力飞行轨道上的深空探测器根据距离行星的远近，建立相应的状态方程，其中利用星敏感器敏感的星光角距和用纯天文几何解析方法解算得到的位置信息，最后通过模糊多模自适应UKF方法进行导航系统位置速度的最优估计；具体包括以下步骤：

(1)通过判断深空探测器距离行星的远近，分别建立借力飞行轨道上深空探测器的导航系统第一状态方程和第二状态方程；若深空探测器在天体影响球之外，选取历元J2000.0日心惯性坐标系，建立以太阳为主引力体的基于四体轨道动力学模型的第一状态方程：

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.$

式中，μ_s，μ_m和μ_e分别为太阳、火星和地球的引力常数；在所述日心惯性坐标系中，r_se和r_sm分别为地球和火星的位置矢量，r_pe为地心到探测器的位置矢量，r_pm为火星质心到探测器的位置矢量，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和火星探测器的位置坐标，(v_x，v_y，v_z)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，w_x，w_y，w_z为状态模型噪声，上述第一状态方程简写为

若深空探测器在天体影响球之内，选取历元J2000.0火星质心惯性坐标系，建立基于二体受摄轨道动力学模型的第二状态方程：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}^{\′}=v_x^{\′}+w_x\\ {\stackrel{\·}{y}}^{\′}=v_y^{\′}+w_y\\ {\stackrel{\·}{z}}^{\′}=v_z^{\′}+w_z\\ {\stackrel{\·}{v}}_x^{\′}=-{\mathrm{\μ}}_m\frac{x^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{x^{\′}-x_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{x_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{x^{\′}-x_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{x_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_x}\\ {\stackrel{\·}{v}}_y^{\′}=-{\mathrm{\μ}}_m\frac{y^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{y^{\′}-y_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{y_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{y^{\′}-y_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{y_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_y}\\ {\stackrel{\·}{v}}_z^{\′}=-{\mathrm{\μ}}_m\frac{z^{\′}}{{r_{\mathrm{pm}}^{\′}}^3}-{\mathrm{\μ}}_s[\frac{z^{\′}-z_1^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{z_1^{\′}}{r_{\mathrm{ms}}^{\′3}}]-{\mathrm{\μ}}_e[\frac{z^{\′}-z_2^{\′}}{r_{\mathrm{pe}}^{\′3}}+\frac{z_2^{\′}}{r_{\mathrm{me}}^{\′3}}]+w_{v_z}\end{array}\right.$

在所述火星质心惯性坐标系中，式中r′_ms和r′_me分别为太阳和地球的位置矢量，r′_ps为太阳质心到探测器的位置矢量，r′_pe为地心到探测器的位置矢量，(x′₁，y′₁，z′₁)，(x′₂，y′₂，z′₂)和(x′，y′，z′)分别为太阳、地球和火星探测器的位置坐标，(v_x′，v_y′，v_z′)为火星探测器的速度坐标，

为状态变量的微分项，即火星探测器的速度和加速度，

为状态模型噪声，上述第二状态方程简写为

式中，X为模型的状态量，X＝(x，y，z，v_x，v_y，v_z)^T，包括x，y，z探测器在日心惯性坐标系中的三轴位置和v_x，v_y，v_z探测器在日心惯性坐标系中的三轴速度；

分别为两个模型状态变量的微分项，f₁(X，t)、f₂(X，t)分别为两个模型的非线性连续状态转移函数，w₁(t)、w₂(t)分别为两个状态模型的噪声，且这两个状态模型噪声协方差阵E[w₁(k)w₁(k)^T]＝Q₁和E[w₂(k)w₂(k)^T]＝Q₂均随r_pm的减小成比例增大；

(2)建立N个以星光角距和纯天文几何解析解算得到的位置信息为量测量的量测子模型为：

$\left\{\begin{array}{c}{\mathrm{\α}}_{1i}=\arccos (-\frac{r_{p1i}\·s}{r_{p1i}})+v_{{\mathrm{\α}}_{1}i}\\ {\mathrm{\α}}_{2i}=\arccos (-\frac{r_{p2i}\·s}{r_{p2i}})+v_{{\mathrm{\α}}_{2}i}\\ {\mathrm{\α}}_{3i}=\arccos (-\frac{r_{p3i}\·s}{r_{p3i}})+v_{{\mathrm{\α}}_{3}i}\\ X_{\mathrm{ci}}=x_i+v_{x_i}\\ Y_{\mathrm{ci}}=y_i+v_{\mathrm{yi}}\\ Z_{\mathrm{ci}}=z_i+v_{z_i}\end{array}\right.$

式中，α_1i，α_2i，α_3i分别为是第i个子模型观测的三颗星与导航星之间的星光角距，r_p1i、r_p2i和r_p3i分别为第i个子模型观测的三颗星质心到探测器的位置矢量，s为导航星星光方向的单位矢量，由星敏感器识别，

为第i个子模型由星光角距得到的量测噪声；X_ci，Y_ci，Z_ci为第i个子模型纯天文几何解算得到的位置信息；x_i，y_i，z_i为第i个子模型中探测器在日心惯性坐标系中的三轴位置；v_xi，v_yi，v_zi为第i个子模型的纯天文解析的计算误差，式中所有变量均为与t有关的变量；

量测方程则使用模型集：

Z_i(t)＝h_i(X_i(t)，t)+v_i(t)，i＝1，2，…，N；

X_i(t)＝[α_1i，α_2i，α_3i，X_ci，Y_ci，Z_ci]^T为第i个子模型的量测量，h_i[X_i(t)，t]为第i个子模型的非线性连续量测函数，

为第i个子模型的量测噪声，N为设置的模型数目，由量测噪声的种类决定，为2-20个；

(3)对第一状态方程和第二状态方程及量测方程进行离散化；

X₁(k+1)＝F₁(X(k)，k)+w₁(k)

或X₂(k+1)＝F₂(X(k)，k)+w₂(k)

Z_i(k)＝H_i(X_i(k)，k)+v_i(k)

式中，k＝1，2，…，i＝1，2…N，F₁(X(k)，k)，F₂(X(k)，k)分别为f₁(X，t)，f₂(X，t)离散化后的非线性状态转移函数，H_i(X_i(k)，k)为h_i(X_i(t)，t)离散化后的非线性量测函数，w₁(k)，w₂(k)和v_i(k)互不相关；

(4)用状态方程与量测方程进行模糊多模自适应UKF滤波，以获得借力飞行轨道上深空探测器的导航定位信息；其中模糊多模自适应UKF滤波方法，以标称状态值和量测值定义模糊集合，然后根据各子模型得到的状态估计值

和量测值Z_i(k)计算各个模型的隶属度w_qi(k)，将隶属度作为所使用模型的权重，并对权值进行归一化；

(5)输出位置，速度信息；

$\hat{X}\left(k\right)=\underset{i}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right){\hat{X}}_i\left(k\right)$

$P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{\mathrm{qi}}\left(k\right)[P_i\left(k\right)+\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}\·{\{{\hat{X}}_i\left(k\right)-\hat{X}\left(k\right)\}}^T]$

式中，

和P(k)分别为k时刻的状态变量估计值和估计方差，其中

分别是对借力飞行轨道上深空探测器在X、Y、Z三个方向的位置和速度x，y，z，v_x，v_y，v_z的估计；并输出估计方差阵

其中

分别是借力飞行轨道上深空探测器在X、Y、Z三个方向位置和速度的估计方差；

P_i(k)分别为k时刻的第i个子模型的状态估计值和估计方差；w_qi(k)为第i个子模型的隶属度。

Images