CN102879011B - 一种基于星敏感器辅助的月面惯导对准方法 - Google Patents

Abstract

一种基于星敏感器辅助的月面惯导对准方法，针对的对象是装备有捷联惯导系统的月面探测器。对准的基本原理是利用加速度计确定重力方向建立水平面；利用星敏感器惯性姿态测量结合已知的起飞点位置确定方位角。在方法实现上分成了星敏+陀螺的惯性姿态估计和利用静基座导航速度误差的惯导平台修正两个步骤。本发明方法成功解决了月球表面惯导对准的问题，对准误差不大于0.05°。

Description

一种基于星敏感器辅助的月面惯导对准方法

技术领域

本发明涉及一种月球探测自主导航方法。

背景技术

对于月球采样返回这类任务来说，当完成月面采样之后，登月探测器需要携带月壤样品从月面起飞上升，完成与留轨飞行器的交会对接，并将样品交由留轨飞行器送返地球。因此月面起飞上升是月球采样返回任务成功的关键环节之一。从月球上起飞或者发射航天器采用的基本手段与地球火箭一样是惯性导航。惯性导航是一种递推式的导航方法，它需要精确的初值，因此在起飞前必须完成惯导系统的对准。地球上进行惯导对准的基本方法是自对准，即利用加速度计获得的重力方向建立水平基准，利用陀螺获得的地球自转角速度方向确定方位基准，必要时还可以通过光学瞄准设备进一步提高方位对准精度。在月球上，由于月球自转角速度很小，重力加速度也只有地球的1/6，因此惯导自对准的精度很低，特别是方位误差会达到几度甚至十几度。而且月球上没有类似地球火箭发射的光学瞄准设备，也就无法提高方位对准精度。这些因素导致在月球上进行惯导对准不能直接采用地球上的常规方法。

目前为止，国外成功完成月面起飞的只有前苏联的“月球”系列无人探测器和美国的“阿波罗”登月飞船。前苏联的“月球”探测器由于采用的是垂直上升的飞行轨迹，它只需要利用加速度计确定铅垂线方向，而无需确定方位角，因此不构成完整的惯导对准。而美国的“阿波罗”登月飞船则采用的是手动光学瞄准技术。该技术利用一种光学瞄准望远镜来实施。首先由宇航员手动瞄准某颗已知恒星，然后从望远镜上的刻度读出该恒星的方位信息，之后对另一颗恒星再次进行同样测量就可以解算出登月飞船的姿态，最后通过电路系统驱动IMU完成对准。对于无人的月球探测器来说，这种有人参与的方法显然不能使用，而且人工瞄准的方法精度也较低。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供了一种星敏感器辅助的月面惯导自对准方法，针对装备有捷联惯导系统的月面探测器，利用加速度计确定重力方向建立水平面，利用星敏感器惯性姿态测量结合已知的起飞点位置确定方位角，从而解决了月球表面惯导对准的问题。

本发明的技术解决方案是：一种基于星敏感器辅助的月面惯导对准方法，步骤如下：

(1)利用陀螺测量得到探测器的角速度扣除陀螺常值漂移得到探测器的角速度估计值其中下标k表示对应时间为t_k，上标b表示探测器本体系；

(2)利用姿态运动学方程对探测器的惯性姿态四元数进行更新，其中q是表征探测器本体系相对惯性系的姿态四元数，且有q＝[q₁ q₂ q₃ q₄]^T，下标k|k-1表示由时间t_k-1到时间t_k的预测，Δt是从时刻t_k-1到t_k的时间间隔， $E\left(q\right)=\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right];$

(3)读取星敏感器输出的光轴在惯性空间的指向和利用步骤(2)更新的惯性姿态四元数将和从惯性坐标系转换到探测器本体系中，对应得到和利用和和星敏感器的安装位置决定的星敏感器三轴指向和获取姿态预估的误差四元数上标i表示惯性系，

$\mathrm{\Δ}{\tilde{q}}_k=\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X_{\mathrm{star}}^b\×{\hat{X}}_{\mathrm{star}}^b+Y_{\mathrm{star}}^b\×{\hat{Y}}_{\mathrm{star}}^b+Z_{\mathrm{star}}^b\×{\hat{Z}}_{\mathrm{star}}^b)\\ 1\end{array}\right];$

(4)利用步骤(2)更新的惯性姿态四元数，计算得到陀螺漂移估计的残差 $\mathrm{\Δ}{\tilde{b}}_k^b={\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}_{k|k-1}^b-A\left({\hat{q}}_{k|k-1}\right){\mathrm{\ω}}_m^i,$ 其中b表示陀螺的常值漂移，为月球的自转角速度矢量，A(q)表示由姿态四元数计算的姿态矩阵，

$A\left(q\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right];$

(5)采用卡尔曼滤波的方式获得星敏感器的姿态测量误差和陀螺常值漂移误差，其中状态方程和测量方程分别为：

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\hat{q}}\\ \mathrm{\Δ}{\stackrel{.}{\hat{b}}}^b\end{array}\right]=\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ \mathrm{\Δ}{\tilde{b}}^b\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{star}}\\ 0_{3\×1}\end{array}\right]$

其中Δb^b表示陀螺漂移的误差，表示的斜对称阵，和是陀螺噪声，V_star是星敏感器噪声，I表示单位阵，由此得到姿态和陀螺漂移估计的修正量：K为常系数卡尔曼滤波的稳态增益；

(6)利用步骤(5)得到的修正量对探测器的姿态四元数以及陀螺的常值漂移进行反馈修正， 当姿态估计稳定后进入下一步；

(7)利用步骤(1)和步骤(2)的方法，结合陀螺测量数据，对探测器进行姿态外推，得到探测器的惯性姿态四元数；

(8)利用加速度计的测量量以及步骤(7)得到的惯性姿态四元数，外推探测器相对惯性系的速度，得到探测器相对惯性系的速度预测值以及加速度计的零偏预测值；

(9)根据探测器相对惯性系的速度预测值解算得到探测器相对月面的速度 ${\hat{v}}_{\mathrm{am},k|k-1}^g=\left[C_m^g{C}_i^m\left(t_k\right)\right]\·[{\hat{v}}_{a,k|k-1}^i-{\mathrm{\ω}}_m^i\×r_{a,k}^i],$ 其中是月心固联坐标系到月理坐标系的转换矩阵，表示由惯性系到月心固联坐标系的转换矩阵，是探测器在惯性系下的位置矢量；

(10)以步骤(9)获得的探测器相对月面的速度作为测量量，采用卡尔曼滤波的方式获得惯导速度、姿态误差的估计值，其中状态方程和测量方程分别为：

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_u\\ \mathrm{\δ}{\stackrel{\·}{v}}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n\\ {\stackrel{\·}{\mathrm{\Φ}}}_u\\ {\stackrel{\·}{\mathrm{\Φ}}}_e\\ {\stackrel{\·}{\mathrm{\Φ}}}_n\\ {\stackrel{\·}{\▿}}_u\end{array}\right]=\left[\begin{array}{ccccccc}0& 2{\mathrm{\ω}}_m\cos L& 0& 0& g_n& -g_e& 1\\ -2{\mathrm{\ω}}_m\cos L& 0& 2{\mathrm{\ω}}_m\sin L& -g_n& 0& g_u& 0\\ 0& -2{\mathrm{\ω}}_m\sin L& 0& g_e& -g_u& 0& 0\\ 0& 0& 0& 0& {\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& {\mathrm{\ω}}_m\sin L& 0\\ 0& 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]$

$\left[\begin{array}{c}{\tilde{v}}_u\\ {\tilde{v}}_e\\ {\tilde{v}}_n\end{array}\right]=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]$

其中δV_u、δV_e、δV_n分别表示惯导解算的探测器对月速度在天向、东向和北向的误差；φ_u、φ_e和φ_n分别表示惯导在天向、东向和北向的姿态误差角；g_u、g_e和g_n则表示月理坐标系下天向、东向和北向的重力加速度；是加速度零偏的天向投影；L是探测器在月表的纬度，且有由此得到惯导误差的滤波修正量，K_IMU为常系数卡尔曼滤波的稳态增益，

$\left[\begin{array}{c}\mathrm{\δ}{\hat{v}}_u\\ \mathrm{\δ}{\hat{v}}_e\\ \mathrm{\δ}{\hat{v}}_n\\ {\widehat{\mathrm{\Φ}}}_u\\ {\widehat{\mathrm{\Φ}}}_e\\ {\widehat{\mathrm{\Φ}}}_n\\ {\widehat{\▿}}_u\end{array}\right]=K_{\mathrm{IMU}}\left[\begin{array}{c}-{\tilde{v}}_u\\ -{\tilde{v}}_e\\ -{\tilde{v}}_n\end{array}\right];$

(11)利用步骤(10)获得的滤波估计值计算由解算出修正后的惯导姿态四元数 为探测器本体系相对月理坐标系的姿态矩阵， ${\hat{C}}_g^{g\′}=\left[\begin{array}{ccc}1& {\widehat{\mathrm{\Φ}}}_n& -{\widehat{\mathrm{\Φ}}}_e\\ -{\widehat{\mathrm{\Φ}}}_n& 1& {\widehat{\mathrm{\Φ}}}_u\\ {\widehat{\mathrm{\Φ}}}_e& -{\widehat{\mathrm{\Φ}}}_u& 1\end{array}\right];$

(12)利用步骤(10)得到的探测器相对月面速度的修正量更新探测器的对月速度 ${\hat{v}}_{\mathrm{am},k}^g={\hat{v}}_{\mathrm{am},k|k-1}^g+{\left[\begin{array}{ccc}\mathrm{\δ}{\hat{v}}_u& \mathrm{\δ}{\hat{v}}_e& \mathrm{\δ}{\hat{v}}_n\end{array}\right]}^T,$ 然后修正惯导外推的探测器惯性速度并完成对加速度计零偏的预估值进行修正，

${\widehat{\▿}}_{k-1}^b={\widehat{\▿}}_{k|k-1}^b+{\hat{C}}_g^b\left(t_k\right){\hat{C}}_g^{g^{\′}}\left[\begin{array}{c}{\widehat{\▿}}_u\\ 0\\ 0\end{array}\right].$

本发明与现有技术相比的优点在于：

(1)本发明方法在对准过程中使用通用和自动的星敏感器取代了人工且复杂的瞄准设备，使得月面惯导对准可以在无人干预的情况下实施，简化了系统构成，具有成本低、适用面广、自主性强的特点；同时，也解决了月球自转角速度低造成惯导自对准无法达到足够的方位对准精度的问题。

(2)本发明方法改变了常规将各类数据同时引入修正的思路，将方法分成星敏感器+陀螺姿态滤波和惯导误差修正两个顺序进行的步骤。在第一步中用星敏感器和陀螺确定探测器姿态的初值，并标定陀螺漂移；在第二步中校正由星敏和IMU之间安装误差引起的惯导平台误差，并估计天向加速度计零偏。这一操作降低了滤波方程的维数，简化了计算量，便于星上实施，同时也解决了起飞前最后阶段星敏感器无法使用(为了防止发动机吹起月尘污染镜头，星敏感器在起飞十几分钟前需要关闭防尘盖，因此步骤一无法进行到探测器点火起飞前)的问题。

附图说明

图1为本发明方法的原理框图；

图2为本发明方法中月面起飞对准姿态误差角变化曲线。

具体实施方式

本发明方法在具体实施上分为两个主要的步骤，具体如图1所示。

1、惯性姿态估计

使用星敏感器+陀螺进行惯性姿态估计，即首先使用陀螺测量预估停留在月面的探测器惯性姿态，然后利用星敏感器测量对陀螺预估的姿态进行修正，同时估计出陀螺角速度测量误差。该估计方法与在轨飞行器常规的星敏感器+陀螺定姿方法的不同之处为：当探测器停留在月面时，它的角速度等于月球自转角速度，这个量已知，因此月球自转角速度也可以用作陀螺漂移估计的参考。星敏感器+陀螺获得的惯性姿态在滤波稳定后赋给惯导系统建立初值。该步骤的具体操作如下：

a)姿态外推

假设当前时刻t_k陀螺获得探测器的角速度为而前一次的陀螺漂移估计为由于陀螺常值漂移不随时间变化，则t_k时刻探测器的角速度的估计值为

${\widehat{\mathrm{\ω}}}_k^b={\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}_{k|k-1}^b---\left(1\right)$

其中，

${\hat{b}}_{k|k-1}^b={\hat{b}}_{k-1}^b---\left(2\right)$

以上各式中，上标b表示该矢量描述在探测器本体坐标系中，下标k表示时间t_k，k|k-1表示由时间t_k-1到时间t_k的预测，波浪号表示测量值，上三角表示估计值，下同。

由此，可以根据姿态运动学方程，对探测器的惯性姿态进行预估。姿态运动学方程为

$\stackrel{\·}{q}=\frac{1}{2}E\left(q\right)\mathrm{\ω}---\left(3\right)$

其中

$E\left(q\right)=\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right]---\left(4\right)$

q是表征探测器本体系相对惯性系的姿态四元数，且有q＝[q₁ q₂ q₃ q₄]^T。q上方的一点表示q对时间的导数，ω为探测器的角速度。

对(3)式进行离散化，就可以使用如下方程完成从时刻t_k-1到时刻t_k的姿态四元数的预估

${\hat{q}}_{k|k-1}\≈{\hat{q}}_{k-1}+\frac{1}{2}E\left({\hat{q}}_{k-1}\right){\widehat{\mathrm{\ω}}}_k^b\·\mathrm{\Δt}---\left(5\right)$

Δt是从时刻t_k-1到t_k的时间间隔，即Δt＝t_k-t_k-1。

b)常系数滤波1

星敏感器能够输出其敏感器光轴和横轴在惯性空间的指向，即和(上标i表示该矢量描述在惯性系下，下标star表示星敏感器)，利用预估的姿态四元数可以将它们转换到探测器的本体系中，

$\left\{\begin{array}{c}{\hat{X}}_{\mathrm{star}}^b=A\left({\hat{q}}_{k|k-1}\right){\tilde{X}}_{\mathrm{star}}^i\\ {\hat{Y}}_{\mathrm{star}}^b=A\left({\hat{q}}_{k|k-1}\right){\tilde{Y}}_{\mathrm{star}}^i\\ {\hat{Z}}_{\mathrm{star}}^b=A\left({\hat{q}}_{k|k-1}\right){\hat{Z}}_{\mathrm{star}}^i\end{array}\right.---\left(6\right)$

其中A(q)表示将四元数q转换为姿态矩阵

$A\left(q\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(7\right)$

由于星敏感器的安装已知，即确切的星敏感器三轴指向在探测器本体系是已知的，它们分别为和那么根据这三个轴的理论值与预估值的差可以获得姿态预估误差四元数的测量值

$\mathrm{\Δ}{\tilde{q}}_k\≈\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X_{\mathrm{star}}^b\×{\hat{X}}_{\mathrm{star}}^b+Y_{\mathrm{star}}^b\×{\hat{Y}}_{\mathrm{star}}^b+Z_{\mathrm{star}}^b\×{\hat{Z}}_{\mathrm{star}}^b)\\ 1\end{array}\right]---\left(8\right)$

由于月球自转角速度是已知的，记为那么也可以根据预估的姿态四元数计算出陀螺漂移估计的残差

$\mathrm{\Δ}{\tilde{b}}_k^b={\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}_{k|k-1}^b-A\left({\hat{q}}_{k|k-1}\right){\mathrm{\ω}}_m^i---\left(9\right)$

由于姿态预估误差四元数为对其两边求导并将式(3)代入其中，则可以推导出姿态四元数外推过程中标量部分的误差传播方程为

$\mathrm{\δ}\stackrel{\·}{q}=-{\widehat{\mathrm{\ω}}}^b\×\mathrm{\δq}-\frac{1}{2}\mathrm{\Δ}{b}^b-\frac{1}{2}{\mathrm{\η}}_1^b---\left(10\right)$

式中 是陀螺测量的随机白噪声。而陀螺的常值漂移估计误差可以建模为

$\mathrm{\Δ}{\stackrel{\·}{b}}^b={\mathrm{\η}}_2^b---\left(11\right)$

也是随机白噪声。联立式(10)、(11)可以构造滤波器的状态方程为

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\hat{q}}\\ \mathrm{\Δ}{\stackrel{.}{\hat{b}}}^b\end{array}\right]=\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]---\left(12\right)$

其中表示的斜对称阵，即

$[{\widehat{\mathrm{\ω}}}^b\×]=\left[\begin{array}{ccc}0& -{\widehat{\mathrm{\ω}}}_z^b& {\widehat{\mathrm{\ω}}}_y^b\\ {\widehat{\mathrm{\ω}}}_z^b& 0& -{\widehat{\mathrm{\ω}}}_x^b\\ -{\widehat{\mathrm{\ω}}}_y^b& {\widehat{\mathrm{\ω}}}_x^b& 0\end{array}\right]$ (下标xyz分别表示在探测器本体系三个坐标轴的分量，探测器本体系固联在探测器上，原点在探测器质心，三个坐标轴指向探测器上固定的方向。)

滤波器的测量方程为

$\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ \mathrm{\Δ}{\tilde{b}}^b\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{star}}\\ 0_{3\×1}\end{array}\right]---\left(13\right)$

其中，V_star表示星敏感器的测量噪声，I_3×3是3×3的单位矩阵，0_3×3是3×3的零矩阵。式(13)中的测量量由式(8)和(9)得到。由此，可以使用卡尔曼滤波估计出姿态误差和陀螺常值漂移误差。

令系统状态

$x=\left[\begin{array}{c}\mathrm{\δq}\\ \mathrm{\Δ}{b}^b\end{array}\right]---\left(14\right)$

根据式(12)状态转移矩阵为

${\mathrm{\Φ}}_{k|k-1}\≈I+\mathrm{\Δt}\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}_{k-1}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(15\right)$

根据式(13)观测矩阵为

$H=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(16\right)$

系统噪声

$w=\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]---\left(17\right)$

其方差为Q。并且令

$G=\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(18\right)$

令测量量为

$z=\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ \mathrm{\Δ}{\tilde{b}}^b\end{array}\right]$

测量噪声为

$v=\left[\begin{array}{c}{v}_{\mathrm{star}}\\ 0_{3\×1}\end{array}\right]---\left(19\right)$

它的方差为R，则可以使用卡尔曼滤波进行状态估计，方法如下

x_k|k-1＝Φ_k|k-1x_k-1          (20)

$P_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{P}_{k-1}{\mathrm{\Φ}}_{k|k-1}^T+G_k{Q}_k{G}_k^T---\left(21\right)$

$K_k={\left(P_{k|k-1}{H}_k^T\right)}^{-1}(H_k{P}_{k|k-1}{H}_k^T+R_k)---\left(22\right)$

x_k＝x_k|k-1+K_k(z_k-H_kx_k|k-1)   (23)

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(24\right)$

从式(20)～(24)看到矩阵K_k的稳态值(k→∞)只与Φ、G、Q、H和R矩阵有关，除了Φ以外，都是常数矩阵；而且由于探测器停留在月面时自转角速度不变，因此用月球自转角速度矢量在标称姿态下的投影取代后，Φ也是常数矩阵。则该滤波可以转换为常系数滤波，即

首先，用式(8)和(9)式获得观测残差和

然后，由观测残差获得姿态和陀螺漂移估计的修正量

$\left[\begin{array}{c}\mathrm{\δ}{\hat{q}}_k\\ \mathrm{\Δ}{\hat{b}}_k^b\end{array}\right]=K\left[\begin{array}{c}\mathrm{\δ}{\tilde{q}}_k\\ \mathrm{\Δ}{\tilde{b}}_k^b\end{array}\right]---\left(25\right)$

其中滤波修正系数矩阵K取值为由式(21)、(22)和(24)递推得到的卡尔曼滤波稳态增益，即k足够大时的K_k。

最后，进行闭环修正

${\hat{q}}_k={\hat{q}}_{k|k-1}+E\left(\hat{q}\right)\mathrm{\δ}{\hat{q}}_k---\left(26\right)$

${\hat{b}}_k^b={\hat{b}}_{k|k-1}^b+\mathrm{\Δ}{\hat{b}}_k^b---\left(27\right)$

2、惯性平台误差的估计和校正

在惯性姿态估计完成后，启动惯性导航，利用加速度计和陀螺测量进行探测器速度、姿态外推。由于存在IMU测量误差、星敏感器测量误差以及星敏感器和IMU之间的安装误差，惯导递推获得的探测器速度和姿态也存在误差并且误差逐渐发散。但由于探测器相对月表并没有运动，因此可以将惯导解算出的对月速度作为观测量，估计惯导平台的误差角以及陀螺和加速度计的测量误差，即自对准。但在这一过程中，滚动误差角与东向陀螺漂移、俯仰误差角与北向加速度计零偏、偏航误差角与东向加速度计零偏这三对组合中的两个状态量不同时可观，因此滤波状态中需要去掉这六个状态中的三个。另外，由于陀螺漂移已经进行了估计，不需在在此进行重复估计。因此，最终滤波状态中去掉了三个陀螺漂移和北向及东向加速度计零偏。

由滤波器估计出惯导平台的误差角后，将该误差角反馈给惯导系统，即可校正惯性导航外推的探测器速度和姿态。

a)姿态外推

步骤二的姿态外推与步骤一的姿态外推基本相同，区别是陀螺漂移估计不再随时间更新。即

${\widehat{\mathrm{\ω}}}_k^b={\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}^b---\left(28\right)$

${\hat{q}}_{k|k-1}\≈{\hat{q}}_{k-1}+\frac{1}{2}E\left({\hat{q}}_{k-1}\right){\widehat{\mathrm{\ω}}}_k^b\·\mathrm{\Δt}---\left(29\right)$

其中，是步骤一最终获得的陀螺常值漂移估计。

b)速度外推

取参考坐标系为月心惯性坐标系，探测器的速度矢量表示为假设加速度计的测量为加速度计零偏的估计值为引力加速度为g，则从时间t_k-1到t_k的速度外推可如下计算：

${\hat{v}}_{a,k|k-1}^i={\hat{v}}_{a,k-1}^i+\mathrm{\Δt}[A\left({\hat{q}}_{k-1}\right)\·({\hat{f}}_{k-1}^b-{\widehat{\▿}}_{k-1}^b)-g_{k-1}^i]---\left(30\right)$

同时对加速度计零偏进行预测

${\widehat{\▿}}_{k|k-1}^b={\widehat{\▿}}_{k-1}^b---\left(31\right)$

c)预估探测器相对月面的速度

首先将每一时刻陀螺预估的姿态四元数转换为惯性姿态矩阵

${\hat{C}}_i^b\left(t_k\right)=A\left({\hat{q}}_k\right)---\left(32\right)$

由于月球的星历已知，因此可以得到从惯性坐标系到月心固联坐标系的变换阵月心固联坐标系的原点在月心，X轴在赤道面内指向零度经线，Z轴平行于月球自转轴指向北极，Y轴在赤道面内与X轴、Y轴构成右手坐标系。由于探测器在月心固联系下的位置已知，设经度、纬度和高度分别为L、λ和h，由此可以计算出月心固联系到当地月理坐标系(原点在当地参考椭球面上，X轴垂直于当地参考椭球面指向天，Y轴指向东，Z轴指向北)的转换阵。

$C_m^g=\left[\begin{array}{ccc}\cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\\ -\sin & \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\end{array}\right]---\left(33\right)$

这样，就可以算出探测器本体相对当地月理坐标系的姿态矩阵

${\hat{C}}_g^b\left(t_k\right)={\hat{C}}_i^b\left(t_k\right){\left[C_m^g{C}_i^m\left(t_k\right)\right]}^T---\left(34\right)$

然后，根据惯性速度解算探测器相对月面的速度

${\hat{v}}_{\mathrm{am},k|k-1}^g=\left[C_m^g{C}_i^m\left(t_k\right)\right]\·[{\hat{v}}_{a,k|k-1}^i-{\mathrm{\ω}}_m^i\×r_{a,k}^i]---\left(35\right)$

其中，是月球自转角速度矢量，是探测器在惯性系下的位置，它可以如下计算

$r_{a,k}^i={\left[C_m^g{C}_i^m\left(t_k\right)\right]}^T\left[\begin{array}{c}{R}_m+h\\ 0\\ 0\end{array}\right]---\left(36\right)$

其中，R_m为月球参考半径。

d)常系数滤波2

由于探测器停留在月面不动，真实的相对月面的速度为零，因此惯导系统外推得到的探测器相对月面的速度就是速度误差。这样可建立如下滤波系统方程和测量方程

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_u\\ \mathrm{\δ}{\stackrel{\·}{v}}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n\\ {\stackrel{\·}{\mathrm{\Φ}}}_u\\ {\stackrel{\·}{\mathrm{\Φ}}}_e\\ {\stackrel{\·}{\mathrm{\Φ}}}_n\\ {\stackrel{\·}{\▿}}_u\end{array}\right]=\left[\begin{array}{ccccccc}0& 2{\mathrm{\ω}}_m\cos L& 0& 0& g_n& -g_e& 1\\ -2{\mathrm{\ω}}_m\cos L& 0& 2{\mathrm{\ω}}_m\sin L& -g_n& 0& g_u& 0\\ 0& -2{\mathrm{\ω}}_m\sin L& 0& g_e& -g_u& 0& 0\\ 0& 0& 0& 0& {\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& {\mathrm{\ω}}_m\sin L& 0\\ 0& 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]---\left(37\right)$

$\left[\begin{array}{c}{\tilde{v}}_u\\ {\tilde{v}}_e\\ {\tilde{v}}_n\end{array}\right]=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]$

其中，δV_u、δV_e、δV_n分别表示惯导解算的探测器对月速度在天向、东向和北向的误差；φ_u、φ_e和φ_n分别表示惯导在天向、东向和北向的姿态误差角；g_u、g_e和g_n则表示月理坐标系下天向、东向和北向的重力加速度；是加速度零偏的天向投影。由于探测器相对月表没有运动，因此惯导系统给出的对月速度的误差就是惯导给出的对月速度本身，因此对于t_k时刻的测量有

${\left[\begin{array}{ccc}{\tilde{v}}_{u,k}& {\tilde{v}}_{e,k}& {\tilde{v}}_{n,k}\end{array}\right]}^T={\hat{v}}_{\mathrm{am},k|k-1}^g---\left(38\right)$

式(37)表示的是一个定常系统，因此可以使用常系数滤波进行状态估计。

令

$\mathrm{\δX}=\left[\begin{array}{cc}{\mathrm{\δv}}^g& \mathrm{\Φ}\end{array}\right]={\left[\begin{array}{ccccccc}{\mathrm{\δv}}_u& {\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\Φ}}_u& {\mathrm{\Φ}}_e& {\mathrm{\Φ}}_n& {\▿}_u\end{array}\right]}^T---\left(39\right)$

则滤波方程可以转换为

$\mathrm{\δ}\hat{X}=K_{\mathrm{IMU}}\left[\begin{array}{c}-{\tilde{v}}_u\\ -{\tilde{v}}_e\\ -{\tilde{v}}_n\end{array}\right]---\left(40\right)$

其中，K_IMU是式(37)构成的卡尔曼滤波的稳态增益阵，它可以用与式(21)、(22)和(24)同样的方法获得。

e)惯性姿态修正

获得月理系下的姿态误差后，需要将它转换为惯性系下的姿态误差，并反馈修正惯导姿态四元数。修正后的惯性姿态矩阵为

${\hat{C}}_i^b\left(t_k\right)={\hat{C}}_g^b\left(t_k\right){\hat{C}}_g^{g\′}{\hat{C}}_m^g{C}_i^m\left(t_k\right)---\left(41\right)$

其中

${\hat{C}}_g^{g\′}=\left[\begin{array}{ccc}1& {\widehat{\mathrm{\Φ}}}_n& -{\widehat{\mathrm{\Φ}}}_e\\ -{\widehat{\mathrm{\Φ}}}_n& 1& {\widehat{\mathrm{\Φ}}}_u\\ {\widehat{\mathrm{\Φ}}}_e& -{\widehat{\mathrm{\Φ}}}_u& 1\end{array}\right]---\left(42\right)$

由可以解算出修正后的姿态四元数

f)惯性速度修正

首先获得相对月面速度的修正量

${\hat{v}}_{\mathrm{am},k}^g={\hat{v}}_{\mathrm{am},k|k-1}^g+{\left[\begin{array}{ccc}\mathrm{\δ}{\hat{v}}_u& \mathrm{\δ}{\hat{v}}_e& \mathrm{\δ}{\hat{v}}_w\end{array}\right]}^T---\left(43\right)$

然后，修正探测器惯性速度

${\hat{v}}_{a,k}^i={\left[C_m^g{C}_i^m\left(t_k\right)\right]}^T{\hat{v}}_{\mathrm{am},k}^g+{\mathrm{\ω}}_m^i\×r_{a,k}^i---\left(44\right)$

之后，还要对加速度计零偏的预估值进行修正

${\widehat{\▿}}_{k-1}^b={\widehat{\▿}}_{k|k-1}^b+{\hat{C}}_g^b\left(t_k\right){\hat{C}}_g^{g^{\′}}\left[\begin{array}{c}{\widehat{\▿}}_u\\ 0\\ 0\end{array}\right]---\left(45\right)$

实施例

此处通过一个仿真例子对本发明提出的月面起飞对准方法进行验证。设探测器在月面的位置为北纬45°、西经60°、高度0m，且探测器纵轴朝上，但相对月面有15°的倾斜。地面给定的探测器位置在经度、纬度和高度上均有500m(3σ)的误差。

探测器上装备有星敏感器和IMU。其中，星敏感器光轴的标称指向为[cos(38°)，cos(54．7°)，cos(102．2°)]^T，光轴误差为3"、横轴误差为24"(3σ)；IMU包含三个陀螺和三个加速度计，它们的输入轴指向相同。分别为

P₁[cos(54°44′8")，sin(54°44′8")cos(90°)，sin(54°44′8")sin(90°)]^T

P₂=[cos(54°44′8")，sin(54°44′8")cos(210°)，sin(54°44′8")sin(210°)]^T

P₃=[cos(54°44′8")，sin(54°44′8″)cos(330°)，sin(54°44′8")sin(330°)]^T陀螺的常值漂移为3°/h(3σ)，加速度计的零偏为1×10^-4g(3σ)。

考虑到探测器经过火箭发射、月球软着陆等飞行阶段后，内部结构存在变形，假设月面起飞前星敏相对IMU在光轴和横轴方向分别有20"和2′的结构变形。

仿真中设定对准总共用时1200秒，其中前300秒进行步骤一，后900秒进行步骤二。对准的姿态误差变化曲线如图2所示。可以看到，在步骤一中，通过星敏感器和陀螺滤波估计，惯导姿态能够很快的稳定。但由于星敏感器和IMU之间存在的形变误差，使得第一步的对准结果存在常值偏差。到步骤二后，由于引入了加速度计测量作为重力方向的参考，因此能够校正星敏感器和IMU安装偏差在水平方向带来的对准误差，即俯仰θ和偏航ψ误差角明显减小。仿真表明，该月面对准方法简单、实用且有效。

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

Claims (1)

1.一种基于星敏感器辅助的月面惯导对准方法，其特征在于步骤如下：

(1)利用陀螺测量得到探测器的角速度扣除陀螺常值漂移得到探测器的角速度估计值其中下标k表示对应时间为t_k，上标b表示探测器本体系；

(2)利用姿态运动学方程对探测器的惯性姿态四元数进行更新，其中q是表征探测器本体系相对惯性系的姿态四元数，且有q＝[q₁ q₂ q₃ q₄]^T，下标k|k-1表示由时间t_k-1到时间t_k的预测，Δt是从时刻t_k-1到t_k的时间间隔， $E\left(q\right)=\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right];$

(3)读取星敏感器输出的光轴在惯性空间的指向和利用步骤(2)更新的惯性姿态四元数将和从惯性坐标系转换到探测器本体系中，对应得到和利用和和星敏感器的安装位置决定的星敏感器三轴指向和获取姿态预估的误差四元数上标i表示惯性系，

$\mathrm{\Δ}{\tilde{q}}_k=\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X_{\mathrm{star}}^b\×{\hat{X}}_{\mathrm{star}}^b+Y_{\mathrm{star}}^b\×{\hat{Y}}_{\mathrm{star}}^b+Z_{\mathrm{star}}^b\×{\hat{Z}}_{\mathrm{star}}^b)\\ 1\end{array}\right];$

(4)利用步骤(2)更新的惯性姿态四元数，计算得到陀螺漂移估计的残差其中b表示陀螺的常值漂移，为月球的自转角速度矢量，A(q)表示由姿态四元数计算的姿态矩阵，

$A\left(q\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right];$

(5)采用卡尔曼滤波的方式获得星敏感器的姿态测量误差和陀螺常值漂移误差，其中状态方程和测量方程分别为：

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\hat{q}}\\ \mathrm{\Δ}{\stackrel{.}{\hat{b}}}^b\end{array}\right]=\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ \mathrm{\Δ}{\tilde{b}}^b\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\hat{q}\\ \mathrm{\Δ}{\hat{b}}^b\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{star}}\\ 0_{3\×1}\end{array}\right]$

其中Δb^b表示陀螺漂移的误差，表示的斜对称阵，和是陀螺噪声，V_star是星敏感器噪声，I表示单位阵，由此得到姿态和陀螺漂移估计的修正量：K为常系数卡尔曼滤波的稳态增益；

(6)利用步骤(5)得到的修正量对探测器的姿态四元数以及陀螺的常值漂移进行反馈修正， 当姿态估计稳定后进入下一步；

(7)利用步骤(1)和步骤(2)的方法，结合陀螺测量数据，对探测器进行姿态外推，得到探测器的惯性姿态四元数；

(8)利用加速度计的测量量以及步骤(7)得到的惯性姿态四元数，外推探测器相对惯性系的速度，得到探测器相对惯性系的速度预测值以及加速度计的零偏预测值；

(9)根据探测器相对惯性系的速度预测值解算得到探测器相对月面的速度 ${\hat{v}}_{\mathrm{am},k|k-1}^g=\left[C_m^g{C}_i^m\left(t_k\right)\right]\·[{\hat{v}}_{a,k|k-1}^i-{\mathrm{\ω}}_m^i\×r_{a,k}^i],$ 其中是月心固联坐标系到月理坐标系的转换矩阵，表示由惯性系到月心固联坐标系的转换矩阵，是探测器在惯性系下的位置矢量；

(10)以步骤(9)获得的探测器相对月面的速度作为测量量，采用卡尔曼滤波的方式获得惯导速度、姿态误差的估计值，其中状态方程和测量方程分别为：

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_u\\ \mathrm{\δ}{\stackrel{\·}{v}}_e\\ \mathrm{\δ}{\stackrel{\·}{v}}_n\\ {\stackrel{\·}{\mathrm{\Φ}}}_u\\ {\stackrel{\·}{\mathrm{\Φ}}}_e\\ {\stackrel{\·}{\mathrm{\Φ}}}_n\\ {\stackrel{\·}{\▿}}_u\end{array}\right]=\left[\begin{array}{ccccccc}0& 2{\mathrm{\ω}}_m\cos L& 0& 0& g_n& -g_e& 1\\ -2{\mathrm{\ω}}_m\cos L& 0& 2{\mathrm{\ω}}_m\sin L& -g_n& 0& g_u& 0\\ 0& -2{\mathrm{\ω}}_m\sin L& 0& g_e& -g_u& 0& 0\\ 0& 0& 0& 0& {\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& {\mathrm{\ω}}_m\sin L& 0\\ 0& 0& 0& 0& -{\mathrm{\ω}}_m\cos L& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]$

$\left[\begin{array}{c}{\tilde{v}}_u\\ {\tilde{v}}_e\\ {\tilde{v}}_n\end{array}\right]=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{v}_u\\ \mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ {\mathrm{\Φ}}_u\\ {\mathrm{\Φ}}_e\\ {\mathrm{\Φ}}_n\\ {\▿}_u\end{array}\right]$

其中δV_u、δV_e、δV_n分别表示惯导解算的探测器对月速度在天向、东向和北向的误差；φ_u、φ_e和φ_n分别表示惯导在天向、东向和北向的姿态误差角；g_u、g_e和g_n则表示月理坐标系下天向、东向和北向的重力加速度；是加速度零偏的天向投影；L是探测器在月表的纬度，且有由此得到惯导误差的滤波修正量，K_IMU为常系数卡尔曼滤波的稳态增益，

$\left[\begin{array}{c}\mathrm{\δ}{\hat{v}}_u\\ \mathrm{\δ}{\hat{v}}_e\\ \mathrm{\δ}{\hat{v}}_n\\ {\widehat{\mathrm{\Φ}}}_u\\ {\widehat{\mathrm{\Φ}}}_e\\ {\widehat{\mathrm{\Φ}}}_n\\ {\widehat{\▿}}_u\end{array}\right]=K_{\mathrm{IMU}}\left[\begin{array}{c}-{\tilde{v}}_u\\ -{\tilde{v}}_e\\ -{\tilde{v}}_n\end{array}\right];$

(11)利用步骤(10)获得的滤波估计值计算由解算出修正后的惯导姿态四元数 为探测器本体系相对月理坐标系的姿态矩阵， ${\hat{C}}_g^{g\′}=\left[\begin{array}{ccc}1& {\widehat{\mathrm{\Φ}}}_n& -{\widehat{\mathrm{\Φ}}}_e\\ -{\widehat{\mathrm{\Φ}}}_n& 1& {\widehat{\mathrm{\Φ}}}_u\\ {\widehat{\mathrm{\Φ}}}_e& -{\widehat{\mathrm{\Φ}}}_u& 1\end{array}\right];$

(12)利用步骤(10)得到的探测器相对月面速度的修正量更新探测器的对月速度 ${\hat{v}}_{\mathrm{am},k}^g={\hat{v}}_{\mathrm{am},k|k-1}^g+{\left[\begin{array}{ccc}\mathrm{\δ}{\hat{v}}_u& \mathrm{\δ}{\hat{v}}_e& \mathrm{\δ}{\hat{v}}_n\end{array}\right]}^T,$ 然后修正惯导外推的探测器惯性速度

并完成对加速度计零偏的预估值进行修正，

${\widehat{\▿}}_{k-1}^b={\widehat{\▿}}_{k|k-1}^b+{\hat{C}}_g^b\left(t_k\right){\hat{C}}_g^{g^{\′}}\left[\begin{array}{c}{\widehat{\▿}}_u\\ 0\\ 0\end{array}\right].$