基于日地月集成敏感器脉冲数据的卫星高精度自主导航方法

摘要

基于日地月集成敏感器脉冲数据的卫星高精度自主导航方法，它涉及卫星导航领域。它为了解决现有方法中脉冲数据作为导航系统的原始数据其中里面包含了噪声、地球扁率、上下弦月球光心和质心不重合的问题而提出的。步骤如下：一、向导航计算机提供脉冲数据；二：根据脉冲数据进行三个天体的方位确定：三：由计算日心方向矢量和地心方向矢量的内积，以及月心方向矢量和地心方向矢量的内积：四：进行双矢量粗定姿；五：导航初始化：六：进行粗导航运算得到导航结果；七：完成月球方位的精细化；八：进行双矢量精定姿，九：修正地心距和地心方位矢量；十：精导航运算得到最终的导航结果。它消除了脉冲数据里面包含的噪声、地球扁率、上下弦月球光心和质心不重合的带来的影响。

说明书

技术领域

本发明涉及卫星导航领域，具体涉及利用日地月集成敏感器测量得到的日、地、月光学脉冲数据，对卫星的位置、速度以及姿态进行自主确定的方法。

背景技术

日地月自主导航系统由导航敏感器和导航计算机组成。导航计算机对导航敏感器的测量值进行处理，经过一定的导航算法，实时定出卫星的位置和速度，从而实现卫星的自主导航。日地月集成敏感器如图1和图2所示，由地球红外双圆锥扫描式敏感器和两个扇形狭缝视场的扫描日月敏感器组成，地球红外双圆锥扫描式敏感器具有单一的光学扫描头部，利用反射镜结构得到两个红外视场，扫描后红外视场的轨迹是两个共轴的圆锥，光学头部扫描一圈，热电检测器最多可以检测到四个地平穿越信号，由信号出现的时刻可以确定地心方向矢量相对于卫星的方位，并可求得卫星到地心的距离。在地球红外双圆锥扫描式敏感器的基础上增加了两个可见光敏感器，在光学头部的扫描过程中，扇形狭缝视场扫过球带区域，利用硅光二极管检测器可以敏感到太阳和月球，根据太阳、月球在扇形狭缝视场中出现的时刻可以求得其方向矢量相对于卫星的方位。检测器具有多个光强阈值，可以辨别太阳和月球信号，并且可以剔除地球信号。日地月导航系统就是利用该类敏感器来确定日地月的方位，从而进行卫星自主导航的系统。

目前对日地月导航系统的研究途径主要是体现在利用夹角信息，来研究导航算法，没有考虑测量原理，因此也无法体现地球扁率、上下弦时候月球光心和质心不重合的这些重要因素，和工程实际相差较远。

脉冲数据作为导航系统的原始数据，里面包含了一切信息(噪声、地球扁率、上下弦月球光心和质心不重合，等等)，本发明的目的在于研究日地月集成敏感器的测量原理，并研究基于脉冲数据的高精度日地月导航算法，这种算法能够对地球扁率进行补偿，也能够对上下弦期间的月球脉冲进行补偿。

发明内容

本发明为了解决现有方法中脉冲数据作为导航系统的原始数据其中里面包含了噪声、地球扁率、上下弦月球光心和质心不重合的问题，而提出了基于日地月集成敏感器脉冲数据的卫星高精度自主导航方法。本发明的目的在于日地月集成敏感器的测量原理，并基于脉冲数据的高精度日地月导航算法，这种算法能够对地球扁率进行补偿，也能够对上下弦期间的月球脉冲进行补偿。

本发明基于日地月集成敏感器脉冲数据的卫星高精度自主导航方法的步骤如下：

步骤一：由地球红外双圆锥扫描式敏感器和两个扇形狭缝视场的扫描日月敏感器组成的导航敏感器向导航计算机提供脉冲数据；

步骤二：根据脉冲数据进行三个天体的方位确定：确定地心方向矢量在测量坐标系下的坐标E_SE、日心方向矢量在测量坐标系下的坐标S_S|SE和月心方向矢量在测量坐标系下的坐标M_S|SE，以及地心距r；

步骤三：由地心方向矢量在测量坐标系下的坐标E_SE、日心方向矢量在测量坐标系下的坐标S_S|SE和月心方向矢量在测量坐标系下的坐标M_S|SE计算出日心方向矢量和地心方向矢量的内积，以及月心方向矢量和地心方向矢量的内积：

步骤四：进行双矢量粗定姿，定义三个两两正交的矢量如下：

v₁：r_m×r_s/|r_m×r_s|

v₂：r_m/|r_m|                         (1)

v₃：v₁×v₂

其中，r_m为星月矢量，r_s为星日矢量；所述的星月矢量r_m和星日矢量r_s在惯性坐标系下的表示由地月矢量、地日矢量近似得到，而地月矢量和地日矢量可以由星历得到；设v₁、v₂、v₃在惯性坐标系下的表示为E₁，E₂，E₃，在测量坐标系下的表示为e₁，e₂，e₃，则惯性坐标系到测量坐标系的旋转矩阵：

R_SEI＝[e₁，e₂，e₃][E₁，E₂，E₃]^-1    (2)

步骤五：导航初始化，初始位置粗略估计用下式计算：

r₀＝-rR_ISEE_SE                       (3)

其中，由公式错误！未找到引用源。得到；初始速度粗略估计则有两个相邻时刻的位置做近似差分得到；

步骤六：采用最小二乘导航算法进行粗导航运算得到导航结果；

步骤七：利用步骤六中的粗导航的导航结果，修正星月矢量r_m的值，从而得到修正后的星月矢量r_m修完成了月球方位的精细化；

步骤八：进行双矢量精定姿，星月矢量r_m在惯性坐标系下的表示由上一步产生的修正后的星月矢量r_m修替代，星日矢量r_s不变，定义三个两两正交的矢量如下：

v₁′：r_m修×r_s/|r_m修×r_s|

v₂′：r_m修/|r_m修|

v₃′：v₁×v₂

设v₁′、v₂′、v₃′在惯性系下的表示为E₁′，E₂′，E₃′，在本体坐标系下的表示为e₁′，e₂′，e₃′也是可求的；则修正的双矢量定姿的惯性坐标系到测量坐标系的旋转矩阵：

R_SEI′＝[e₁′，e₂′，e₃′][E₁′，E₂′，E₃′]^-1    (4)

步骤九：考虑地球扁率基于步骤八中修正的双矢量定姿的惯性坐标系到测量坐标系的旋转矩阵R_SEI′修正地心距r和地心方位矢量E_SE，得到修正后的地心距r_修和修正后的地心方位矢量E_SE修；

步骤十：采用最小二乘导航算法进行精导航运算得到最终的导航结果。

本发明是根据日、地、月集成敏感器的光学脉冲信息来确定飞行器的位置、速度和姿态的方法。首先是日、地、月的方位确定，对于地心方位来说，有两种方法：基于地球球形假设的地心方位确定(用于粗导航)和基于地球扁率的地心方位确定(精导航，扁率修正)；对于上下弦期间的月心方位确定来说，考虑了光心和月球质心不重合带来的影响，给出了补偿方案。然后给出一套基于脉冲数据扁率补偿和上下弦修正的最小二乘导航实现算法，最后进行仿真精度评估。

敏感器扫描转速为ω_rot＝240r/min。圆锥地平敏感器红外地平视场圆锥的两个半锥角分别为γ₁＝38°和γ₂＝73°。扫描探头相对于狭缝敏感器对称面M₁M₂的滞后角度为B_R1＝B_R2＝0°，敏感器测量坐标系相对于体系的安装角度为γ_I＝π/6，β_I＝π/6，α_I＝π/6。扇形日月敏感器相对于与扫描转轴的倾斜角为β_s＝16°狭缝视场1在敏感器赤道平面内超前于狭缝视场2的角距υ＝4°。

轨道的初始时刻取为2012年5月6日0时0分0秒。真实轨道的初始六要素为：长半轴为6878km，轨道偏心率为0，轨道倾角为92°，升交点赤经为π/6，近地点幅角为0°，真近角为0°。模拟数据产生时，假设卫星的姿态是严格对地定向的。用到轨道测量数据的时间是从2012年5月12日0时0分0秒开始的5000秒内。

用STK产生的轨道数据来模拟脉冲，脉冲产生机制中考虑地球扁率，地球脉冲噪声的3σ＝6.5e-5s，对应角度噪声的3σ＝0.1°，地球敏感器系统误差为0.03度(对应脉冲噪声为2.3e-5s，扫描太阳(月球)脉冲噪声的3σ＝3.5e-5s，对应角度噪声的3σ＝0.05°。导航模型仍然用MATLAB中的轨道动力学模型，进行蒙特卡洛仿真，仿真次数为120次。轨道初始值的滤波估计值与真实轨道初始值的误差的位置均值为位置均方差速度均值为速度均方差位置误差和速度误差的3σ椭球如图6和图7。

附图说明

图1是日地月集成敏感器的安装结构；A为航天器，B为第一地球红外双圆锥扫描式敏感器转轴，C为第二地球红外双圆锥扫描式敏感器转轴，D为38度红外视场，E为73度红外视场，F为可见光扇形视场；图2是两个扇形狭缝视场的扫描日月敏感器的视场；L为扇形视场，M为扫描转轴，N为红外探头；图3是地球敏感器测量几何图；图4是日地月方位示意图；图5是日地月方位示意图；图6是位置误差的3σ椭球示意图；图7是速度误差的3σ椭球示意图。

具体实施方式

具体实施方式一：结合图1至图5说明本实施方式，本实施方式具体步骤如下：

步骤一：由地球红外双圆锥扫描式敏感器和两个扇形狭缝视场的扫描日月敏感器组成的导航敏感器向导航计算机提供脉冲数据；

步骤二：根据脉冲数据进行三个天体的方位确定：确定地心方向矢量在测量坐标系下的坐标E_SE、日心方向矢量在测量坐标系下的坐标S_S|SE和月心方向矢量在测量坐标系下的坐标M_S|SE，以及地心距r；

步骤三：由地心方向矢量在测量坐标系下的坐标E_SE、日心方向矢量在测量坐标系下的坐标S_S|SE和月心方向矢量在测量坐标系下的坐标M_S|SE计算出日心方向矢量和地心方向矢量的内积，以及月心方向矢量和地心方向矢量的内积：

步骤四：进行双矢量粗定姿，定义三个两两正交的矢量如下：

v₁：r_m×r_s/|r_m×r_s|

v₂：r_m/|r_m|                         (5)

v₃：v₁×v₂

其中，r_m为星月矢量，r_s为星日矢量；所述的星月矢量r_m和星日矢量r_s在惯性坐标系下的表示由地月矢量、地日矢量近似得到，而地月矢量和地日矢量可以由星历得到；设v₁、v₂、v₃在惯性坐标系下的表示为E₁，E₂，E₃，在测量坐标系下的表示为e₁，e₂，e₃，则惯性坐标系到测量坐标系的旋转矩阵：

R_SEI＝[e₁，e₂，e₃][E₁，E₂，E₃]^-1    (6)

步骤五：导航初始化，初始位置粗略估计用下式计算：

r₀＝-rR_ISEE_SE                       (7)

其中，由公式错误！未找到引用源。得到；初始速度粗略估计则有两个相邻时刻的位置做近似差分得到；

步骤六：采用最小二乘导航算法进行粗导航运算得到导航结果；

步骤七：利用步骤六中的粗导航的导航结果，修正星月矢量r_m的值，从而得到修正后的星月矢量r_m修完成了月球方位的精细化；

步骤八：进行双矢量精定姿，星月矢量r_m在惯性坐标系下的表示由上一步产生的修正后的星月矢量r_m修替代，星日矢量r_s不变，定义三个两两正交的矢量如下：

v₁′：r_m修×r_s/|r_m修×r_s|

v₂′：r_m修/|r_m修|

v₃′：v₁×v₂

设v₁′、v₂′、v₃′在惯性系下的表示为E₁′，E₂′，E₃′，在本体坐标系下的表示为e₁′，e₂′，e₃′也是可求的；则修正的双矢量定姿的惯性坐标系到测量坐标系的旋转矩阵：

R_SEI′＝[e₁′，e₂′，e₃′][E₁′，E₂′，E₃′]^-1    (8)

步骤九：考虑地球扁率基于步骤八中修正的双矢量定姿的惯性坐标系到测量坐标系的旋转矩阵R_SEI′修正地心距r和地心方位矢量E_SE，得到修正后的地心距r_修和修正后的地心方位矢量E_SE修；

步骤十：采用最小二乘导航算法进行精导航运算得到最终的导航结果。

具体实施方式二：本实施方式与具体实施方式一不同点在于步骤二中的地心方向矢量在测量坐标系下的坐标E_SE的确定过程，以及地心距r的确定过程如下：

地球红外双圆锥扫描式敏感器上的两个红外探头扫入地球时相对于测量坐标系的方位角为：

α_1-in＝-((ω_rott_ref+B_R1)-ω_rott_1-in)

(9)

α_2-in＝-((ω_rott_ref+B_R2)-ω_rott_2-in)

其中ω_rot为扫描敏感器的转速，B_R1、B_R2为初始时刻双圆锥扫描式敏感器上的两个红外探头相对于基准x轴的滞后角，t_ref为狭缝敏感器对称面穿过基准x轴对应得脉冲时刻，t_1-in、t_2-in为双圆锥扫描式敏感器上的两个红外探头扫入地平时形成的脉冲；

地球红外双圆锥扫描式敏感器上的两个红外探头扫出地球时相对于测量坐标系的方位角为：

α_1-out＝ω_rott_1-out-(ω_rott_ref+B_R1)

(10)

α_2-out＝ω_rott_2-out-(ω_rott_ref+B_R2)

其中t_1-out、t_2-out为双圆锥扫描式敏感器上的两个红外探头扫出地平时形成的脉冲；

所以地心方向矢量相对于测量坐标系的方位角为：

${\phi }_{e1}=-(\frac{1}{2}(-{\alpha }_{1-\mathrm{in}}+{\alpha }_{1-\mathrm{out}})-{\alpha }_{1-\mathrm{out}})$

(11)

${\phi }_{e2}=-(\frac{1}{2}(-{\alpha }_{2-\mathrm{in}}+{\alpha }_{2-\mathrm{out}})-{\alpha }_{2-\mathrm{out}})$

如果不考虑地球扁率，有φ_e1＝φ_e2，并用φ_e表示；

从而地球红外双圆锥扫描式敏感器得到的地球的弦宽为：

Ω_E1＝-α_1-in+α_1-out

(12)

Ω_E2＝-α_2-in+α_2-out

由球面三角公式，有

$\cos \rho =\cos {\gamma }_1\cos \eta +\sin {\gamma }_1\sin \eta \cos \frac{{\Omega }_{E1}}{2}$

$\cos \rho =\cos {\gamma }_2\cos \eta +\sin {\gamma }_2\sin \eta \cos \frac{{\Omega }_{E2}}{2}$

其中ρ为相对于卫星的地球的红外辐射圆盘的半张角，γ₁、γ₂为地球敏感器双圆锥的半锥角，η为地心方向与敏感器旋转轴之间的夹角；

所以

$\eta ={\cot }^{-1}\frac{{\sin \gamma }_2\cos 0.5{\Omega }_{E2}-{\sin \gamma }_1\cos 0.5{\Omega }_{E1}}{\cos {\gamma }_1-{\cos \gamma }_2}---\left(13\right)$

如果不考虑地球扁率，地心方向矢量在测量坐标系下的坐标求得为：

$E_{\mathrm{SE}}=\left(\begin{array}{c}\sin \eta {\cos \phi }_e\\ \sin \eta \sin {\phi }_e\\ \cos \eta \end{array}\right)---\left(14\right)$

地心距由下式计算：

r＝R_E/sinρ                  (15)

其中，R_E为地球的红外辐射球半径，ρ可以由球面三角形的余弦定理求得：

$\cos \rho =\cos \gamma \cos \eta +\sin \gamma \sin \eta \cos \frac{{\Omega }_E}{2}---\left(16\right)$

其它步骤与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一不同点在于步骤二中的日心方向矢量在测量坐标系下的坐标S_S|SE的确定过程如下：

日心方位在测量本体坐标系下的方位角和高度角为：

${\phi }_s={\omega }_{\mathrm{rot}}(\frac{t_{1\mathrm{sum}}+t_{2\mathrm{sum}}}{2}-t_{\mathrm{ref}})---\left(17\right)$

δ_s＝cot^-1(sinσ_scosβ_s)

其中：t_1sun、t_2sun为第一扇形狭缝视场、第二扇形狭缝视场敏感到太阳光时候形成的脉冲，σ_s＝(ω_rott_2sun-ω_rott_1sun-υ)/2，υ为第一扇形狭缝视场的扫描日月敏感器在敏感器赤道平面上超前于第二扇形狭缝视场的扫描日月敏感器的焦距，β_s为狭缝视场相对于扫描转轴倾斜的角度；

日心方向矢量在测量坐标系下的坐标为：

$S_{S|\mathrm{SE}}=\left(\begin{array}{c}{\cos \delta }_s{\cos \phi }_s\\ \cos {\delta }_s\sin {\phi }_s\\ \sin {\delta }_s\end{array}\right)---\left(18\right)$

其它步骤与具体实施方式一相同。

具体实施方式四：本实施方式与具体实施方式一不同点在于步骤二中的月心方向矢量在测量坐标系下的坐标M_S|SE的确定过程如下：

满月期间敏感月球时候的公式和敏感太阳光时候的类似，满月期间的月心方位在测量坐标系下的方位角和高度角为：

${\phi }_m={\omega }_{\mathrm{rot}}(\frac{t_{1\mathrm{moon}}+t_{2\mathrm{moon}}}{2}-t_{\mathrm{ref}})---\left(19\right)$

δ_m＝tan^-1(sinσ_Mcosβ_s)

其中：t_1moon、t_2moon为第一扇形狭缝视场、第二扇形狭缝视场敏感到月球时候形成的脉冲，σ_M＝(ω_rott_2moon-ω_rott_1moon-υ)/2；

从而满月期间的月心方向矢量在测量坐标系下的坐标为：

$M_{S|\mathrm{SE}}=\left(\begin{array}{c}{\cos \delta }_m{\cos \phi }_m\\ \cos {\delta }_m\sin {\phi }_m\\ \sin {\delta }_m\end{array}\right)---\left(20\right)$

上下弦期间的月心方位确定和满月期间的月心方位确定公式一样，但有个脉冲补偿模块；由粗导航得到卫星的姿态信息，可以求出敏感器扫描轴与x_m(白道面上与地月方向垂直的轴)的夹角θ_x，补偿后的脉冲应该原来的脉冲的基础上加个Δt

$\mathrm{\Delta t}=\left(\begin{array}{c}\frac{2\delta t_{\max }}{\pi }{\theta }_x({\theta }_x<\pi /2)\\ \frac{2\delta t_{\max }}{\pi }(\pi -{\theta }_x){\theta }_x\ge \pi /2\end{array}\right)$

其中，上弦月的时候Δt＞0，下弦月的时候Δt＜0；再由公式错误！未找到引用源。计算φ_m，δ_m，由公式错误！未找到引用源。确定补偿后的上下弦期间的月心方位。其它步骤与具体实施方式一相同。

具体实施方式五：本实施方式与具体实施方式一不同点在于步骤七修正星月矢量r_m，由于太阳和地球的距离很远，因此星日矢量r_s和从地心指向太阳的方向矢量R_s可以看做是平行的。而月球相对于地球的距离不是很远，星月矢量r_m和地日矢量R_m不能看做是平行的。因此星月矢量r_m在惯性坐标系下的的表示不能用地月矢量R_m来代替，需要进行有限距离修正。

从粗导航中初步确定卫星在惯性坐标系下的位置r_b，而月球惯性位置由星历求得，从而确定星月矢量r_m在惯性坐标系下的表示，修正公式如下：

r_m修＝R_m-r_b。

具体实施方式六：结合图3说明本实施方式，本实施方式与具体实施方式一不同点在于步骤九中基于扁率修正的地心方位确定要利用步骤八的姿态信息，对地心距r和地心方位矢量E_SE重新计算具体如下：

由测量几何分析，可知地心到地球表面的矢量R_E为：

$R_E={R_{\mathrm{ISE}}}^{\prime }(r\hat{r}+l\hat{\rho })---\left(21\right)$

其中：为从地心指向卫星的方向矢量(在测量坐标系下的表示)，分别和扫描视线的单位矢量(投影在测量坐标系下)，令脉冲时刻t_1in，t_1out，t_2in，t_2out对应的扫描视线的单位矢量为如果不考虑测量噪声的话，这些量是精确已知的，分别为：

${\hat{\rho }}^1=\left(\begin{array}{c}\sin \left({\gamma }_1\right)\cos ({\omega }_{\mathrm{rot}}(t_{1\mathrm{in}}-t_{\mathrm{ref}})-B_{R1})\\ \sin \left({\gamma }_1\right)\sin ({\omega }_{\mathrm{rot}}(t_{1\mathrm{in}}-t_{\mathrm{ref}})-B_{R1})\\ \cos \left({\gamma }_1\right)\end{array}\right)$

${\hat{\rho }}^2=\left(\begin{array}{c}\sin \left({\gamma }_1\right)\cos ({\omega }_{\mathrm{rot}}(t_{1\mathrm{out}}-t_{\mathrm{ref}})-B_{R1})\\ \sin \left({\gamma }_1\right)\sin ({\omega }_{\mathrm{rot}}(t_{1\mathrm{out}}-t_{\mathrm{ref}})-B_{R1})\\ \cos \left({\gamma }_1\right)\end{array}\right)$

(22)

${\hat{\rho }}^3=\left(\begin{array}{c}\sin \left({\gamma }_2\right)\cos ({\omega }_{\mathrm{rot}}(t_{2\mathrm{in}}-t_{\mathrm{ref}})-B_{R2})\\ \sin \left({\gamma }_2\right)\sin ({\omega }_{\mathrm{rot}}(t_{2\mathrm{in}}-t_{\mathrm{ref}})-B_{R2})\\ \cos \left({\gamma }_2\right)\end{array}\right)$

${\hat{\rho }}^4=\left(\begin{array}{c}\sin \left({\gamma }_2\right)\cos ({\omega }_{\mathrm{rot}}(t_{2\mathrm{out}}-t_{\mathrm{ref}})-B_{R2})\\ \sin \left({\gamma }_2\right)\sin ({\omega }_{\mathrm{rot}}(t_{2\mathrm{out}}-t_{\mathrm{ref}})-B_{R2})\\ \cos \left({\gamma }_2\right)\end{array}\right)$

地心到地球红外辐射球表面的矢量投影在惯性坐标系下。

椭球方程为：

其中p＝-1+1/(1-ee)².

由公式(24)中可得：

(25)

${z_e}^2={\left(L_3\hat{\rho }\right)}^2{l}^2+2r{L}_3\hat{r}{L}_3\hat{\rho }l+r^2{\left(L_3\hat{r}\right)}^2$

其中：L₃为R_ISE的第三行。

将错误！未找到引用源。代入错误！未找到引用源。到得到二次方程：

(a+Δa)l²+(b+Δb)l+(c+Δc)＝0      (26)

其中：

$\mathrm{\Delta a}=p{\left(L_3\hat{\rho }\right)}^2,\mathrm{\Delta b}=2\mathrm{pr}{L}_3\hat{r}{L}_3\hat{\rho },\mathrm{\Delta c}={\mathrm{pr}}^2{\left(L_3\hat{r}\right)}^2$

记脉冲对应时刻的扫描视线方向矢量则有：

(b+Δb)²-4(a+Δa)(c+Δc)＝0        (27)

也即：

(28)

将粗导航中的每个时刻的做为初值可以从上方程中解得修正后的值得注意的是，还应满足约束条件

4个未知数，5个方程，用最小二乘估计来逼近令x₀为x的迭代初值，将F在x₀点一阶泰勒展开有：

$0=F\left(x\right)=F\left(x_0\right)+\frac{\partial F}{\partial x}{|}_{x=x_0}(x-x_0)$

其中：$\frac{\partial F}{\partial x}=\left(\begin{array}{cc}\frac{{\partial f}_1}{\partial r}& \frac{\partial f_1}{\partial \hat{r}}\\ \frac{\partial f_2}{\partial r}& \frac{\partial f_2}{\partial \hat{r}}\\ \frac{\partial f_3}{\partial r}& \frac{\partial f_3}{\partial \hat{r}}\\ \frac{\partial f_4}{\partial r}& \frac{{\partial f}_4}{\partial \hat{r}}\\ \frac{\partial f_5}{\partial r}& \frac{\partial f_5}{\partial \hat{r}}\end{array}\right),$

且：

$\frac{{\partial f}_i}{\partial r}=2\frac{R_E^2}{r^3}(1+p{\left(L_3{\hat{\rho }}^i\right)}^2)i=\mathrm{1,2},\mathrm{3,4}$

由于得维数为5×4，令则有：

写成迭代的形式有：

x_k就是x的最优估计，也即修正后的地心距和地心方向矢量

具体实施方式七：本实施方式与具体实施方式一不同点在于步骤六和步骤十中的最小二乘导航算法为：假设N个观测时刻t₁，t₂，…，t_N的观测量为所述的观测量是月地之间的夹角和日地之间的夹角，由步骤三内积的反余弦得到，其真值为令残差为：

$G(X_0^{*},t_i)=\hat{H}(X_0^{*},t_i)-H(X_0^{*},t_i)$

如果没有测量噪声的话值得注意的是是未知的，因为真值是未知的。

建立最小二乘指标：

其中：

$G(X_0,t_i)=\hat{H}(X_0^{*},t_i)-H(X_0,t_i)$

显然当的时候，J有极小值，则

$\frac{\partial J}{{\partial X}_0}{|}_{X_0=X_0^{*}}=0$

设为的估计值，则将G(X₀，t_i)在附近线性化可得：

$G(X_0,t_i)=G({\tilde{X}}_0,t_i)+\frac{\partial G}{{\partial X}_0}{|}_{X_0={\tilde{X}}_0}(X_0-{\tilde{X}}_0)$

将上方程代入泛函指标中可得：

上式可以记做：

$A(X_0^{*}-{\tilde{X}}_0)+B=0$

其中：

从而可以求得：

$X_0^{*}={\tilde{X}}_0-A^{-1}B$

相应的迭代公式可以写作：

X_k+1＝X_k-A^-1B

本发明内容不仅限于上述各实施方式的内容，其中一个或几个具体实施方式的组合同样也可以实现发明的目的。