本发明涉及一种深空探测器测角/差分测速/差分测距组合导航方法。首先根据轨道动力学建立航天器的状态模型，再分别利用测角敏感器获得星光角距量测量，利用X射线脉冲星探测器获得脉冲到达时间量测量，利用光谱仪获得天文多普勒速度量测量，之后根据这些量测量分别建立星光角距量测模型、差分脉冲到达时间量测模型及差分天文多普勒速度量测模型。离散化后使用UKF滤波估计航天器的位置和速度。本发明属于航天器自主导航领域，本发明估计精度高，对航天器自主导航具有重要的实际意义。
1.一种深空探测器测角/差分测速/差分测距组合导航方法，其特征在于：根据轨道动力学建立航天器的状态模型，利用测角敏感器获得星光角距量测量，利用X射线脉冲星探测器获得脉冲到达时间量测量，利用光谱仪获得天文多普勒速度量测量，根据这些量测量分别建立星光角距量测模型、差分脉冲到达时间量测模型及差分天文多普勒速度量测模型；
离散化后使用UKF滤波估计航天器的位置和速度；具体包括以下步骤：
将航天器在火星接近段的运动描述为以太阳为中心天体的受摄三体模型，将其他扰动视为过程噪声；在太阳中心惯性坐标系下的动力学模型可写为：其中||·||表示矢量的2范数，||·||3表示||·||的立方，r和v是航天器相对太阳的位置和速度；μs和μm分别是太阳和火星的引力常数，rm是火星相对太阳的位置矢量，rsm＝r-rm是航天器相对火星的位置矢量；w是各种扰动造成的过程噪声；可由上式得到状态模型如下：其中状态量X＝[r,v]T为航天器在太阳惯性坐标系下的位置及速度， 为状态量X的导数， 为时刻t的 f(X(t),t)为系统非线性连续状态转移函数，w为过程噪声，w(t)为时刻t的w；
由于脉冲信号需要的观测周期较长，固与星光角距量测量及天文多普勒速度量测量相比，脉冲到达时间量测量的采样周期较长；因此，以星光角距量测量和天文多普勒速度量测量的采样周期作为滤波周期；当滤波时刻没有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差；当滤波时刻有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型、差分脉冲到达时间量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差；
利用测角敏感器获得航天器与火卫一、火卫二及其背景恒星间的星光角距，以这些星光角距作为量测量建立量测模型：其中αp1及αp2分别是航天器与火卫一及两颗背景恒星间的星光角距，αd1及αd2分别是航天器与火卫二及两颗背景恒星间的星光角距，rsp、rsd分别是火卫一、火卫二相对于航天器的位置矢量，s1、s2分别是惯性系下两颗恒星的方向矢量，rp、rd分别是火卫一、火卫二相对于太阳的位置矢量；
把这些星光角距作为量测量Z1＝[αp1,αp2,αd1,αd2]T，可建立星光角距量测模型的表达式：Z1＝[αp1,αp2,αd1,αd2]T＝h1[X(t),t]+v1(t) (4)其中h1(·)表示星光角距的非线性连续量测函数，v1(t)表示t时刻星光角距的量测噪声；
利用光谱仪获得太阳光谱频移，并根据频移获得航天器相对太阳的径向速度，以此作为量测量建立量测模型：其中vr表示航天器相对太阳的径向速度量测量，vrt表示航天器相对太阳的径向速度真实值，υp表示由于太阳光谱频率波动造成的扰动项，υm表示天文多普勒速度量测噪声；
其中vr(t)及vr(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度量测量，vrt(t)及vrt(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度真实值，υp(t)及υp(t-1)分别是t时刻及t-1时刻太阳光谱频率波动造成的扰动项，υm(t)及υm(t-1)分别是t时刻及t-1时刻的量测噪声，Δυp(t)＝υp(t)-υp(t-1)是差分后υp的残差，Δυm(t)＝υm(t)-υm(t-1)是差分后υm的残差；
把差分天文多普勒速度作为量测量Z2＝[vr(t)-vr(t-1)]，可建立差分天文多普勒速度量测模型的表达式：Z2＝[vr(t)-vr(t-1)]＝h2[X(t),X(t-1)]+v2(t) (7)其中h2(·)表示差分天文多普勒速度的非线性连续量测函数，v2(t)表示t时刻差分天文多普勒速度的量测误差；用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：Z2＝h2[X(t),t]+v2(t) (8)⑤建立差分脉冲到达时间的量测模型
利用X射线脉冲星探测器获得脉冲到达时间量测量，以脉冲到达时间作为量测量建立量测模型：其中tb表示脉冲星脉冲到达太阳系质心的时间，tSC表示脉冲星脉冲到达航天器的时间，rS表示航天器相对太阳系质心的位置矢量，c表示光速，n表示脉冲星在惯性系下的方向矢量，D0表示脉冲星到太阳系质心的距离，b表示太阳系质心相对太阳的位置矢量；
其中τ(t)表示t时刻的差分脉冲到达时间，tb(t)及tb(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达太阳系质心的时间，tSC(t)及tSC(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达航天器的时间；
把差分脉冲到达时间作为量测量Z3＝[τ(t)]，可建立差分脉冲到达时间量测模型的表达式：Z3＝[τ(t)]＝h3[X(t),X(t-1)]+v3(t) (11)其中h3(·)表示差分脉冲到达时间的非线性连续量测函数，v3(t)表示t时刻差分脉冲到达时间的量测噪声；用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：Z3＝h3[X(t),t]+v3(t) (12)⑥进行离散化
当滤波时刻没有脉冲到达时间量测量时，设此时导航系统的量测量Z12＝[Z1,Z2]T，量测噪声v12＝[v1,v2]T，导航系统模型为：其中h12(·)表示没有脉冲到达时间量测量时导航系统的非线性连续量测函数；对式进行离散化：其中Xk及Z12k分别表示k时刻系统的状态量及没有脉冲到达时间量测量时系统的量测量，F(Xk-1,k-1)为f(X(t),t)离散后的非线性状态转移函数，H12(Xk,k)为h12[X(t),t]离散化后的非线性量测函数，Wk及V12k分别表示w(t)及v12(t)离散后的等效噪声；
当滤波时刻有脉冲到达时间量测量时，设此时导航系统的量测量Z＝[Z1,Z2,Z3]T，量测噪声v＝[v1,v2,v3]T，导航系统模型为：其中h(·)表示有脉冲到达时间量测量时导航系统的非线性连续量测函数；对式进行离散化：其中Zk表示k时刻系统的量测量，H(Xk,k)为h[X(t),t]离散后的非线性量测函数，Vk表示v(t)离散后的等效噪声；
当滤波时刻没有脉冲到达时间量测量时，对离散化后的系统模型式通过UKF进行滤波获得惯性系下航天器相对太阳的后验状态估计 以及后验误差协方差 其中 分别为第k时刻航天器相对太阳的位置、速度后验估计；当滤波时刻有脉冲到达时间量测量时，对离散化后的系统模型式通过UKF进行滤波获得 及 将 及 输出，同时将这些估计值返回滤波器，用于获得k+1时刻的输出。
一种深空探测器测角/差分测速/差分测距组合导航方法
技术领域
[0001] 本发明属于航天器自主导航领域，涉及一种基于星光角距、差分脉冲到达时间、差分天文多普勒速度等量测量的自主天文导航方法。
背景技术
[0002] 我国计划于2020年发射一颗火星探测器，因此火星探测将吸引越来越多的关注。对于深空探测任务而言，导航精度对于任务的成败有着重要影响。目前主要通过地面测控站为航天器提供导航信息。随着航天器与地球之间距离的增加，通过地面测控站进行信号传输的双程时延将越来越大。另外，日凌将造成信号中断。因此，需要提高航天器的自主导航能力。
[0003] 可用于深空探测器的自主导航方法主要有天文测角导航方法、X射线脉冲星测距导航方法及天文多普勒测速导航方法。天文测角导航方法通过测量航天器与近天体及导航恒星之间的夹角获得航天器的位置信息。但是，这种方法的导航精度随着航天器与近天体间距离的增加而下降。X射线脉冲星测距导航可以提供高精度的位置信息，且导航精度不受航天器与天体间位置的影响。但是，由于X射线脉冲星的信号比较微弱，通常需要通过较长时间的历元折叠以得到量测量。另外，脉冲星角位置误差及星载原子钟钟差将影响导航精度。天文多普勒测速导航通过测量由航天器与恒星间径向运动造成的光谱频移获得航天器的速度信息。但是，这种方法无法提供位置信息，且恒星光谱的动态变化将影响其导航精度。
发明内容
[0004] 本发明提出一种深空探测器测角/差分测速/差分测距组合导航方法，利用星光角距量测量提供绝对位置信息，利用时间差分X射线脉冲星脉冲到达时间量测量减弱系统误差的影响，提供相对位置信息，利用时间差分天文多普勒速度量测量减弱恒星光谱动态变化的影响，提供速度信息。将三种量测量通过UKF滤波组合，为深空探测器提供高精度的导航信息。
[0005] 本发明解决其技术问题所采用的技术方案为：根据轨道动力学建立航天器的状态模型，利用测角敏感器获得星光角距量测量，利用X射线脉冲星探测器获得脉冲到达时间量测量，利用光谱仪获得天文多普勒速度量测量，根据这些量测量分别建立星光角距量测模型、差分脉冲到达时间量测模型及差分天文多普勒速度量测模型。离散化后使用UKF滤波估计航天器的位置和速度。
[0006] 具体包括以下步骤：
[0007] 1.建立基于轨道动力学的系统状态模型
[0008] 将航天器在火星接近段的运动描述为以太阳为中心天体的受摄三体模型，将其他扰动视为过程噪声。在太阳中心惯性坐标系(J2000.0)下的动力学模型可写为：
[0009]
[0010] 其中||·||表示矢量的2范数，||·||3表示||·||的立方，r和v是航天器相对太阳的位置和速度。μs和μm分别是太阳和火星的引力常数，rm是火星相对太阳的位置矢量，rsm＝r-rm是航天器相对火星的位置矢量。w是各种扰动造成的过程噪声。可由上式得到状态模型如下：
[0011]
[0012] 其中状态量X＝[r,v]T为航天器在太阳惯性坐标系下的位置及速度， 为状态量X的导数， 为时刻t的 f(X(t),t)为系统非线性连续状态转移函数，w为过程噪声，w(t)为时刻t的w。
[0013] 2.判断是否有脉冲到达时间量测量
[0014] 由于脉冲信号需要的观测周期较长，固与星光角距量测量及天文多普勒速度量测量相比，脉冲到达时间量测量的采样周期较长。因此，以星光角距量测量和天文多普勒速度量测量的采样周期作为滤波周期。当滤波时刻没有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差。当滤波时刻有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型、差分脉冲到达时间量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差。
[0015] 3.建立星光角距的量测模型
[0016] 利用测角敏感器获得航天器与火卫一、火卫二及其背景恒星间的星光角距，以这些星光角距作为量测量建立量测模型：
[0017]
[0018] 其中αp1及αp2分别是航天器与火卫一及两颗背景恒星间的星光角距，αd1及αd2分别是航天器与火卫二及两颗背景恒星间的星光角距，rsp、rsd分别是火卫一、火卫二相对于航天器的位置矢量，s1、s2分别是惯性系下两颗恒星的方向矢量，rp、rd分别是火卫一、火卫二相对于太阳的位置矢量。
[0019] 把这些星光角距作为量测量Z1＝[αp1,αp2,αd1,αd2]T，可建立星光角距量测模型的表达式：
[0020] Z1＝[αp1,αp2,αd1,αd2]T＝h1[X(t),t]+v1(t) (4)
[0021] 其中h1(·)表示星光角距的非线性连续量测函数，v1(t)表示t时刻星光角距的量测噪声。
[0022] 4.建立差分天文多普勒速度的量测模型
[0023] 利用光谱仪获得太阳光谱频移，并根据频移获得航天器相对太阳的径向速度，以此作为量测量建立量测模型：
[0024]
[0025] 其中vr表示航天器相对太阳的径向速度量测量，vrt表示航天器相对太阳的径向速度真实值，υp表示由于太阳光谱频率波动造成的扰动项，υm表示天文多普勒速度量测噪声。
[0026] 建立差分天文多普勒速度的量测模型：
[0027]
[0028] 其中vr(t)及vr(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度量测量，vrt(t)及vrt(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度真实值，υp(t)及υp(t-1)分别是t时刻及t-1时刻太阳光谱频率波动造成的扰动项，υm(t)及υm(t-1)分别是t时刻及t-1时刻的量测噪声，△υp(t)＝υp(t)-υp(t-1)是差分后υp的残差，△υm(t)＝υm(t)-υm(t-1)是差分后υm的残差。
[0029] 把差分天文多普勒速度作为量测量Z2＝[vr(t)-vr(t-1)]，可建立差分天文多普勒速度量测模型的表达式：
[0030] Z2＝[vr(t)-vr(t-1)]＝h2[X(t),X(t-1)]+v2(t) (7)
[0031] 其中h2(·)表示差分天文多普勒速度的非线性连续量测函数，v2(t)表示t时刻差分天文多普勒速度的量测误差。用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：
[0032] Z2＝h2[X(t),t]+v2(t) (8)
[0033] 5.建立差分脉冲到达时间的量测模型
[0034] 利用X射线脉冲星探测器获得脉冲到达时间量测量，以脉冲到达时间作为量测量建立量测模型：
[0035]
[0036] 其中tb表示脉冲星脉冲到达太阳系质心的时间，tSC表示脉冲星脉冲到达航天器的时间，rS表示航天器相对太阳系质心的位置矢量，c表示光速，n表示脉冲星在惯性系下的方向矢量，D0表示脉冲星到太阳系质心的距离，b表示太阳系质心相对太阳的位置矢量。
[0037] 建立差分脉冲到达时间的量测模型：
[0038]
[0039] 其中τ(t)表示t时刻的差分脉冲到达时间，tb(t)及tb(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达太阳系质心的时间，tSC(t)及tSC(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达航天器的时间。
[0040] 把差分脉冲到达时间作为量测量Z3＝[τ(t)]，可建立差分脉冲到达时间量测模型的表达式：
[0041] Z3＝[τ(t)]＝h3[X(t),X(t-1)]+v3(t) (11)
[0042] 其中h3(·)表示差分脉冲到达时间的非线性连续量测函数，v3(t)表示t时刻差分脉冲到达时间的量测噪声。用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：
[0043] Z3＝h3[X(t),t]+v3(t) (12)
[0044] 6.进行离散化
[0045] 当滤波时刻没有脉冲到达时间量测量时，设此时导航系统的量测量Z12＝[Z1,Z2]T，量测噪声v12＝[v1,v2]T，导航系统模型为：
[0046]
[0047] 其中h12(·)表示没有脉冲到达时间量测量时导航系统的非线性连续量测函数。对式(13)进行离散化：
[0048]
[0049] 其中Xk及Z12k分别表示k时刻系统的状态量及没有脉冲到达时间量测量时系统的量测量，F(Xk-1,k-1)为f(X(t),t)离散后的非线性状态转移函数，H12(Xk,k)为h12[X(t),t]离散化后的非线性量测函数，Wk及V12k分别表示w(t)及v12(t)离散后的等效噪声。
[0050] 当滤波时刻有脉冲到达时间量测量时，设此时导航系统的量测量Z＝[Z1,Z2,Z3]T，量测噪声v＝[v1,v2,v3]T，导航系统模型为：
[0051]
[0052] 其中h(·)表示有脉冲到达时间量测量时导航系统的非线性连续量测函数。对式(15)进行离散化：
[0053]
[0054] 其中Zk表示k时刻系统的量测量，H(Xk,k)为h[X(t),t]离散后的非线性量测函数，Vk表示v(t)离散后的等效噪声。
[0055] 7.进行UKF滤波获得航天器的位置速度估计
[0056] 当滤波时刻没有脉冲到达时间量测量时，对离散化后的系统模型式(14)通过UKF进行滤波获得惯性系下航天器相对太阳的后验状态估计 以及后验误差协方差 其中 分别为第k时刻航天器相对太阳的位置、速度后验估计。当滤波时刻有脉冲到达时间量测量时，对离散化后的系统模型式(16)通过UKF进行滤波获得 及 将及 输出，同时将这些估计值返回滤波器，用于获得k+1时刻的输出。
[0057] 本发明的原理是：利用星光角距获得完全可观测的航天器位置信息，但精度不高。利用前后时刻获得的脉冲到达时间量测量通过差分得到差分脉冲到达时间，减弱脉冲星角位置误差及星载原子钟钟差对导航精度的影响，获得高精度的位置信息。利用前后时刻获得的天文多普勒速度量测量通过差分得到差分天文多普勒速度，减弱恒星光谱动态变化对导航精度的影响，获得高精度的速度信息。根据轨道动力学建立航天器的状态模型，分别建立星光角距量测模型、差分脉冲到达时间量测模型及差分天文多普勒速度量测模型，离散化后使用UKF滤波得到航天器高精度的位置和速度。
[0058] 本发明与现有技术相比的优点在于：(1)实现对航天器的高精度自主导航。(2)利用差分脉冲到达时间获得高精度的位置信息。(3)利用差分天文多普勒速度获得高精度的速度信息。
附图说明
[0059] 图1为本发明中深空探测器测角/差分测速/差分测距组合导航方法流程图。
[0060] 图2为本发明中星光角距量测模型示意图。
[0061] 图3为本发明中X射线脉冲星测距导航原理示意图。
具体实施方式
[0062] 图1给出了深空探测器测角/差分测速/差分测距组合导航方法系统流程图。这里以火星探测接近段为例，详细说明本发明的具体实施过程：
[0063] 1.建立基于轨道动力学的系统状态模型
[0064] 将航天器在火星接近段的运动描述为以太阳为中心天体的受摄三体模型，将其他扰动视为过程噪声。在太阳中心惯性坐标系(J2000.0)下的动力学模型可写为：
[0065]
[0066] 其中||·||表示矢量的2范数，||·||3表示||·||的立方，r和v是航天器相对太阳的位置和速度。μs和μm分别是太阳和火星的引力常数，rm是火星相对太阳的位置矢量，rsm＝r-rm是航天器相对火星的位置矢量。w是各种扰动造成的过程噪声。可由上式得到状态模型如下：
[0067]
[0068] 其中状态量X＝[r,v]T为航天器在太阳惯性坐标系下的位置及速度， 为状态量X的导数， 为时刻t的 f(X(t),t)为系统非线性连续状态转移函数，w为过程噪声，w(t)为时刻t的w。
[0069] 2.判断是否有脉冲到达时间量测量
[0070] 由于脉冲信号需要的观测周期较长，固与星光角距量测量及天文多普勒速度量测量相比，脉冲到达时间量测量的采样周期较长。因此，以星光角距量测量和天文多普勒速度量测量的采样周期作为滤波周期。当滤波时刻没有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差。当滤波时刻有脉冲到达时间量测量时，对由状态模型及星光角距量测模型、差分天文多普勒速度量测模型、差分脉冲到达时间量测模型构成的系统模型通过UKF滤波获得惯性系下航天器相对太阳的后验状态估计以及后验误差协方差。
[0071] 3.建立星光角距的量测模型
[0072] 利用测角敏感器获得航天器与火卫一、火卫二及其背景恒星间的星光角距，以这些星光角距作为量测量建立量测模型。图2给出了星光角距量测模型示意图。其中rsp、rsd分别是火卫一、火卫二相对于航天器的位置矢量，s1、s2分别是惯性系下两颗恒星的方向矢量，rp、rd分别是火卫一、火卫二相对于太阳的位置矢量。星光角距量测模型可写为：
[0073]
[0074] 其中αp1及αp2分别是航天器与火卫一及两颗背景恒星间的星光角距，αd1及αd2分别是航天器与火卫二及两颗背景恒星间的星光角距。把这些星光角距作为量测量Z1＝[αp1,αp2,αd1,αd2]T，可建立星光角距量测模型的表达式：
[0075] Z1＝[αp1,αp2,αd1,αd2]T＝h1[X(t),t]+v1(t) (4)
[0076] 其中h1(·)表示星光角距的非线性连续量测函数，v1(t)表示t时刻星光角距的量测噪声。
[0077] 4.建立差分天文多普勒速度的量测模型
[0078] 利用光谱仪获得太阳光谱频移，并根据频移获得航天器相对太阳的径向速度，以此作为量测量建立量测模型：
[0079]
[0080] 其中vr表示航天器相对太阳的径向速度量测量，vrt表示航天器相对太阳的径向速度真实值，υp表示由于太阳光谱频率波动造成的扰动项，υm表示天文多普勒速度量测噪声。
[0081] 建立差分天文多普勒速度的量测模型：
[0082]
[0083] 其中vr(t)及vr(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度量测量，vrt(t)及vrt(t-1)分别是t时刻及t-1时刻航天器相对太阳的径向速度真实值，υp(t)及υp(t-1)分别是t时刻及t-1时刻太阳光谱频率波动造成的扰动项，υm(t)及υm(t-1)分别是t时刻及t-1时刻的量测噪声，△υp(t)＝υp(t)-υp(t-1)是差分后υp的残差，△υm(t)＝υm(t)-υm(t-1)是差分后υm的残差。
[0084] 把差分天文多普勒速度作为量测量Z2＝[vr(t)-vr(t-1)]，可建立差分天文多普勒速度量测模型的表达式：
[0085] Z2＝[vr(t)-vr(t-1)]＝h2[X(t),X(t-1)]+v2(t) (7)
[0086] 其中h2(·)表示差分天文多普勒速度的非线性连续量测函数，v2(t)表示t时刻差分天文多普勒速度的量测误差。用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：
[0087] Z2＝h2[X(t),t]+v2(t) (8)
[0088] 5.建立差分脉冲到达时间的量测模型
[0089] 图3给出了X射线脉冲星测距导航原理示意图。其中tSC和tb分别表示第i颗脉冲星脉冲到达航天器和太阳系质心(SSB)的时间，n为日心惯性系下第i颗脉冲星的方向矢量，rS为探测器相对太阳系质心的位置矢量，可表示为rS＝r-b，b为SSB相对太阳的位置矢量，c表示光速。由图可看出，c·(tb-tSC)可认为rS在n上的投影。考虑相对论效应和几何效应，时间转换模型可表示为：
[0090]
[0091] 其中D0表示脉冲星到太阳系质心的距离，等式右边第一项为几何距离产生的Doppler延迟，第二项表示X射线平行到达太阳系引起的时间延迟，通常把前两项统称为Roemer延迟，第三项表示在太阳引力场作用下光线弯曲产生的时间延迟，称为Shapiro延迟。
[0092] 建立差分脉冲到达时间的量测模型：
[0093]
[0094] 其中τ(t)表示t时刻的差分脉冲到达时间，tb(t)及tb(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达太阳系质心的时间，tSC(t)及tSC(t-1)分别表示t时刻及t-1时刻脉冲星脉冲到达航天器的时间。
[0095] 把差分脉冲到达时间作为量测量Z3＝[τ(t)]，可建立差分脉冲到达时间量测模型的表达式：
[0096] Z3＝[τ(t)]＝h3[X(t),X(t-1)]+v3(t) (11)
[0097] 其中h3(·)表示差分脉冲到达时间的非线性连续量测函数，v3(t)表示t时刻差分脉冲到达时间的量测噪声。用t-1时刻的后验状态估计 代替X(t-1)，则差分脉冲到达时间量测模型的表达式可写为：
[0098] Z3＝h3[X(t),t]+v3(t) (12)
[0099] 6.进行离散化
[0100] 当滤波时刻没有脉冲到达时间量测量时，设此时导航系统的量测量Z12＝[Z1,Z2]T，量测噪声v12＝[v1,v2]T，导航系统模型为：
[0101]
[0102] 其中h12(·)表示没有脉冲到达时间量测量时导航系统的非线性连续量测函数。对式(13)进行离散化：
[0103]
[0104] 其中Xk及Z12k分别表示k时刻系统的状态量及没有脉冲到达时间量测量时系统的量测量，F(Xk-1,k-1)为f(X(t),t)离散后的非线性状态转移函数，H12(Xk,k)为h12[X(t),t]离散化后的非线性量测函数，Wk及V12k分别表示w(t)及v12(t)离散后的等效噪声。
[0105] 当滤波时刻有脉冲到达时间量测量时，设此时导航系统的量测量Z＝[Z1,Z2,Z3]T，量测噪声v＝[v1,v2,v3]T，导航系统模型为：
[0106]
[0107] 其中h(·)表示有脉冲到达时间量测量时导航系统的非线性连续量测函数。对式(15)进行离散化：
[0108]
[0109] 其中Zk表示k时刻系统的量测量，H(Xk,k)为h[X(t),t]离散后的非线性量测函数，Vk表示v(t)离散后的等效噪声。
[0110] 7.进行UKF滤波获得航天器的位置速度估计
[0111] 对离散化后的系统模型通过UKF进行滤波，具体步骤如下。
[0112] A.初始化状态量 和状态误差方差阵P0
[0113]
[0114] 式中， 是第0时刻(初始时刻)航天器位置及速度的估计值，X0是第0时刻航天器位置及速度的真实值。
[0115] B.选取sigma采样点
[0116] 在 附近选取一系列采样点，这些样本点的均值和协方差分别为 和状态变量为6×1维，那么选取13个样本点 及其权重w0,w1…,w12分别如下：
[0117]
[0118] 其中τ表示缩放参数， 表示取平方根矩阵的第i行或列。
[0119] C.传递sigma采样点并获得先验估计及先验误差协方差
[0120] 每个采样点的一步预测 为：
[0121]
[0122] 合并所有 获得先验状态估计 为：
[0123]
[0124] 先验误差协方差 为：
[0125]
[0126] 式中，Qk为k时刻状态模型噪声协方差阵。
[0127] D.量测更新
[0128] 根据量测方程，计算每个采样点 的预测量测量 当滤波时刻没有脉冲到达时间量测量时：
[0129]
[0130] 当滤波时刻有脉冲到达时间量测量时：
[0131]
[0132] 合并所有 获得预测量测Yk为：
[0133]
[0134] 计算预测量测协方差Pyy,k及互协方差Pxy,k：
[0135]
[0136] 其中Rk为k时刻系统的量测噪声协方差阵。计算滤波增益Kk为：
[0137]
[0138] 计算后验状态估计
[0139]
[0140] 计算后验误差协方差
[0141]
[0142] 将 及 输出，同时将这些估计值返回滤波器，用于获得k+1时刻的输出。
[0143] 本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
