CN102435763A - 一种基于星敏感器的航天器姿态角速度测量方法 - Google Patents

Abstract

本发明公开了一种基于星敏感器的航天器姿态角速度测量方法，目的是解决现有方法精度受星敏感器测量误差影响较大的问题。技术方案是读入初始时刻t₀的星图，获得t₀的星敏感器测量矢量及导航星信息；令t＝t₀+δt，读入t时刻星图，经星点提取、序列星图识别，对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t的导航星总数n以及对应的星敏感器测量矢量对的集合Ω1；然后对卡尔曼滤波器进行初始化，估计航天器姿态角速度，重置滤波器初始值，令t＝t+δt，重复读入t时刻星图，经星点提取、序列星图识别，对导航星进行编号，获得n以及Ω1的步骤。本发明能消除星敏感器测量随机噪声对姿态角速度估计的影响，提高测量的精度。

Description

一种基于星敏感器的航天器姿态角速度测量方法

技术领域

本发明涉及一种航天测量领域中航天器姿态角速度的测量方法，具体地说是一种基于星敏感器的航天器姿态角速度的测量方法。

背景技术

航天器姿态控制和确定，需要已知姿态角速度信息。通常，多采用陀螺作为航天器角速度的测量器件。然而，近年来小卫星技术发展迅速，其重量、功率等限制，往往不装载陀螺组件，仅利用姿态角敏感器进行卫星姿态确定的技术受到了广泛关注。此外，当陀螺失效时，也需要采用其它手段获得姿态角速度信息。

通常，无陀螺条件下的姿态角速度可以根据姿态矩阵或姿态四元素，由差分法直接获得；此外，也可根据卫星动力学方程，采用状态估计的方法获得。但是，前者依赖于姿态矩阵或姿态四元素的确定精度，后者由于引入了如星体惯量、外干扰力矩等不确定因素，精度受限制。2002年，John L.Crassidis在期刊《Journal of Guidance，Control，and Dynamics》发表文献“Angular velocity determination directly from star tracker measurements”，提出一种根据星敏感器测量矢量在卫星本体坐标系的投影矢量，直接获得航天器姿态角速度的方法。该方法的优点是姿态角速度的测量，不依赖于姿态矩阵，缺点是存在延时，且精度受星敏感器测量矢量误差影响较大。

发明内容

本发明要解决的技术问题是：针对文献“Angular velocity determination directly from startracker measurements”描述的航天器姿态角速度的测量方法存在延时、精度受星敏感器测量的误差影响较大的问题，提出一种基于星敏感器航天器姿态角速度的测量方法。以实现在简化计算复杂度的同时，消除星敏感器测量随机噪声对姿态角速度估计的影响，提高测量的精度。

本发明的技术方案包括以下步骤：

第一步，读入初始时刻t₀的星图，进行星点提取后，利用全天球识别模式的星图识别算法(见《光学精密工程》2009年第17卷第1期刊载的“改进的基于主星的星图识别算法”)，获得t₀时刻的星敏感器测量矢量及相应的导航星信息。

第二步，令t＝t₀+δt，δt为星敏感器采样时间间隔，读入t时刻星图，进行星点提取、序列星图识别，对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的导航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1，具体方法如下：

2.1读入t时刻星图，进行星点提取(见《光学技术》2009年第35卷第3期刊载的“基于背景自适应预测的星点提取算法”)，获得t时刻星图中星点的质心坐标，作为星点在星敏感器像面坐标系中的投影位置；

2.2进行序列星图识别(见《Journal of IEEE Transactions on Aerospace and ElectronicSystems》2005年第41卷第40期刊载的“Recursive Mode Star IdentificationAlgorithms”)，获得t时刻星图中识别的导航星i的星敏感器测量矢量v(t)_i，

$v{\left(t\right)}_i=\frac{1}{\sqrt{x^2{\left(t\right)}_i+y^2{\left(t\right)}_i+f^2}}\left[\begin{array}{c}x{\left(t\right)}_i\\ y{\left(t\right)}_i\\ f\end{array}\right]---\left(1\right)$

其中，x(t)_i和y(t)_i分别表示t时刻星图中识别的导航星i在星敏感器像面坐标系中的投影位置的x坐标和y坐标；f表示星敏感器镜头焦距；i为正整数，，表示识别的导航星编号；

2.3对同时出现在前一帧星图和当前帧星图的导航星进行编号，具体方法为，统计同时出现在前一帧t-δt时刻和当前帧t时刻的星图中的导航星，将总数目记为n，并按赤经由小到大编号为1，2，…k，…n。将导航星k在t-δt星图中的星敏感器测量矢量v(t-δt)_k和t时刻的星图中的星敏感器测量矢量v(t)_k，作为数据对记入集合Ω1，其中k为导航星序号，1≤k≤n；

第三步，对卡尔曼滤波器进行初始化

3.1设置估计噪声协方差矩阵初值P₀为3×3的单位矩阵I_3×3；

3.2取Ω1中的数据，利用公式(2)，获得t₀+δt时刻的航天器姿态角速度初值 $\left[\begin{array}{c}\mathrm{\ω}{(t_0+\mathrm{\δt})}_x\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_y\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_z\end{array}\right],$ ω(t₀+δt)_x、ω(t₀+δt)_y、ω(t₀+δt)_z分别表示t₀+δt时刻的航天器在x、y、x方向上的姿态角速度分量；令 $X_0=\left[\begin{array}{c}\mathrm{\ω}{(t_0+\mathrm{\δt})}_x\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_y\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_z\end{array}\right],$ 将X₀作为卡尔曼滤波器的状态矢量的初值；

$\left[\begin{array}{c}\mathrm{\ω}{(t_0+\mathrm{\δt})}_x\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_y\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_z\end{array}\right]=-\frac{1}{\mathrm{\δt}}{\left\{\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{[v{(t_0+\mathrm{\δk})}_k\×]}^T\right[v{(t_0+\mathrm{\δt})}_k\×\left]\right\}}^{-1}\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{[v{(t_0+\mathrm{\δt})}_k\×]}^{T}v{\left(t_0\right)}_k---\left(2\right)$

式2中，[]^T表示矩阵转置，{}^-1表示矩阵求逆，[v(t₀+δt)_k×]表示t₀+δt时刻导航星k的星敏感器测量矢量 $v{(t_0+\mathrm{\δt})}_k=\frac{1}{\sqrt{x^2{(t_0+\mathrm{\δt})}_k+y^2{(t_0+\mathrm{\δt})}_k+f^2}}\left[\begin{array}{c}x{(t_0+\mathrm{\δt})}_k\\ y{(t_0+\mathrm{\δt})}_k\\ f\end{array}\right]$ 的叉乘矩阵，具体形式为：

$[v{(t_0+\mathrm{\δt})}_k\×]=\frac{1}{\sqrt{x^2{(t_0+\mathrm{\δt})}_k+y^2{(t_0+\mathrm{\δt})}_k+f^2}}\left[\begin{array}{ccc}0& -f& y{(t_0+\mathrm{\δt})}_k\\ f& 0& -x{(t_0+\mathrm{\δt})}_k\\ -y{(t_0+\mathrm{\δt})}_k& x{(t_0+\mathrm{\δt})}_k& 0\end{array}\right]---\left(3\right)$

 第四步，估计航天器姿态角速度

4.1取集合Ω1中的数据，以导航星序号k的增加作为时间序列，构建卡尔曼滤波器的状态方程和测量方程如下：

$\left\{\begin{array}{c}X{\left(t\right)}_k=I_{3\×3}X{\left(t\right)}_{k-1}+w_{k-1}\\ Z{\left(t\right)}_k=H{\left(t\right)}_{k}X{\left(t\right)}_k+u_k\end{array}\right.---\left(4\right)$

其中， $X\left(t\right)=\left[\begin{array}{c}\mathrm{\ω}{\left(t\right)}_x\\ \mathrm{\ω}{\left(t\right)}_y\\ \mathrm{\ω}{\left(t\right)}_z\end{array}\right]$ 表示t时刻的卡尔曼滤波器的状态矢量，ω(t)_x、ω(t)_y、ω(t)_z分别表示t时刻的航天器在x、y、x方向上的姿态角速度分量；X(t)_k和X(t)_k-1分别表示t时刻导航星k和导航星k-1对应的卡尔曼滤波器的状态矢量；

I_3×3为3×3单位矩阵；w_k-1表示导航星k-1对应的过程噪声，u_k表示导航星k对应的测量噪声；

表示t时刻星敏感器测量矢量k的一阶导；

观测矩阵H(t)_k＝[v(t)_k×]，[v(t)_k×]表示t时刻导航星k的星敏感器测量矢量 $v{\left(t\right)}_k=\frac{1}{\sqrt{x^2{\left(t\right)}_k+y^2{\left(t\right)}_k+f^2}}\left[\begin{array}{c}x{\left(t\right)}_x\\ {y\left(t\right)}_y\\ f\end{array}\right]$ 的叉乘矩阵，具体形式为：

$[v{\left(t\right)}_k\×]=\frac{1}{\sqrt{x^2{\left(t\right)}_k+y^2{\left(t\right)}_k+f^2}}\left[\begin{array}{ccc}0& -f& y{\left(t\right)}_k\\ f& 0& -x{\left(t\right)}_k\\ -y{\left(t\right)}_k& x{\left(t\right)}_k& 0\end{array}\right]---\left(5\right)$

w_k-1和u_k分别表示过程噪声和测量噪声；

4.2输入状态矢量的初值X₀、估计噪声协方差矩阵初值P₀、过程噪声协方差矩阵Q、测量噪声的协方差矩阵R，估计当前时刻的航天器姿态角速度估计值X(t)和估计噪声协方差矩阵P_n，具体步骤如下：

4.2.1令k＝1，输入状态矢量的初值X₀、估计噪声协方差矩阵初值P₀；

4.2.2采用公式(6)预估导航星k对应的卡尔曼滤波器的状态矢量，记为

$X{\left(t\right)}_k^{-}=I_{3\×3}X{\left(t\right)}_{k-1}---\left(6\right);$

4.2.3采用公式(7)预估导航星k对应的噪声协方差矩阵，记为

$P_k^{-}=P_{k-1}+Q---\left(7\right);$

4.2.4采用公式(8)计算导航星k对应的卡尔曼增益矩阵，记为K_k

$K_k=P_k^{-}{[P_k^{-}+R]}^{-1}---\left(8\right);$

4.2.5采用公式(9)更新估计，获得导航星k对应的的卡尔曼滤波器的状态矢量X(t)_k

$X{\left(t\right)}_k=X{\left(t\right)}_k^{-}+K_k[Z{\left(t\right)}_k-H{\left(t\right)}_{k}X{\left(t\right)}_k^{-}]---\left(9\right);$

4.2.6采用公式(10)计算更新后的导航星k对应的估计噪声协方差矩阵P_k

$P_k=[I_{3\×3}-K_{k}H{\left(t\right)}_k]P_k^{-}---\left(10\right);$

4.2.7如果k＝n，清空集合Ω1，输出X(t)_n和P_n，作为当前时刻的航天器姿态角速度估计值X(t)和估计噪声协方差矩阵，转第五步；否则，令k＝k+1，转步骤4.2.2；

第五步，重置滤波器初始值

5.1设置估计噪声协方差矩阵初值P₀＝P_n，其中P_n为第四步最后得到的估计噪声协方差矩阵；

5.2利用线性插值的方法求下一帧星图对应的航天器姿态角速度预测值，将航天器姿态角速度预测值作为下一帧星图的卡尔曼滤波器的状态矢量的初值X₀，计算公式为：

X₀＝2X(t)-X₀    (11)

第六步，令t＝t+δt，读入t时刻星图，进行2.1所述的星点提取、2.2所述的序列星图识别，2.3所述的对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的导航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1，转第四步。

在航天器在轨运行期间，星敏感器不断采集星图，在无故障情况下，程序不断循环(重复执行第四步～第六步)，在第四步的4.2.7输出航天器姿态角速度值，实现航天器姿态角速度测量。若出现故障，重新从第一步开始执行。

采用本发明可以达到以下技术效果：

(1)第四步直接利用星敏感器测量矢量，估计航天器姿态角速度值，无需陀螺或姿态矩阵；

(2)第四步以星敏感器有效测量矢量数目的增加作为时间序列，构建卡尔曼滤波器，以滤波器最后输出的X(t)_n作为当前时刻的航天器姿态角速度估计值X(t)，可以减小随机噪声，同时，卡尔曼滤波属于序列处理方法，具有计算简单的特点；

(3)第五步以前一帧星图的估计噪声协方差矩阵P_n和线性插值预测的航天器姿态角速度X(t)作为滤波器初值，可以有效改善滤波效果，提高估计精度。

附图说明

图1是本发明基于星敏感器的航天器姿态角速度测量方法整体流程图；

图2是仿真的某卫星三轴姿态角速度真值；

图3是采用背景技术John L.Crassidis报道的根据星敏感器测量矢量直接获得航天器姿态角速度的方法，估计的卫星姿态角速度的误差；

图4是采用本发明的基于星敏感器的航天器姿态角速度测量方法，估计的卫星姿态角速度的误差。

具体实施方式

采用本发明对航天器姿态角速度进行估计，具体步骤如下：

第一步，读入初始时刻t₀的星图，进行星点提取后，利用全天球识别模式的星图识别算法，获得初始时刻t₀的星敏感器测量矢量及相应的导航星信息。

第二步，令t＝t₀+δt，读入t时刻星图，进行星点提取、序列星图识别，对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1。

第三步，对卡尔曼滤波器进行初始化。

第四步，估计航天器姿态角速度。

第五步，重置滤波器初始值。

第六步，令t＝t+δt，读入t时刻星图，进行星点提取、序列星图识别，对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的导航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1，转第四步。

为了验证本发明的有效性，对卫星在轨运行0～6500s时间的状态进行了仿真。仿真星敏感器的基本参数设计为：

焦距：76.078mm

视场：10°×10°

图像传感器像元阵列：1024×1024

像元尺寸：13μm×13μm

星敏感器数据输出频率：10Hz

星敏感器相对于卫星本体坐标系的安装方位角为(0，0，π/4)

初始时刻(t₀＝0)，卫星轨道参数为：

位置：(463480.909599m，-6258628.275922m，165616.012775m)

速度：(277.207697m/s，324.329998m/s，7687.234174m/s)

假设星点质心噪声为0均值，标准差为0.05像素的高斯噪声。图2中，图a、b、c分别给出了卫星姿态角速度在x、y、z三个方向上的分量随时间变化的曲线；图3中，图a、b、c分别给出了采用John L.Crassidis报道的根据星敏感器测量矢量在卫星本体坐标系的投影矢量，直接获得航天器姿态角速度的方法，估计的卫星姿态角速度在x、y、z三个方向上的误差随时间变化；图4中，图a、b、c分别给出了采用本发明进行卡尔曼滤波后，估计的卫星姿态角速度在x、y、z三个方向上的误差随时间变化。根据图3中的数据，John L.Crassidis报道的技术姿态角速度估计误差为(1.8×10^-5，1.8×10^-5，2.6×10^-3)rad/s(1σ)；而根据图4中的数据，采用本发明进行卡尔曼滤波后，卫星姿态角速度的估计误差为(3.0×10^-8，2.0×10^-6，2.1×10^-6)rad/s(1σ)。

本发明直接利用星敏感器测量矢量构建卡尔曼滤波器，估计估计航天器姿态角速度值，无需陀螺或姿态矩阵，具有精度高、计算简单的特点。

Claims (1)

1.一种基于星敏感器的航天器姿态角速度测量方法，其特征在于包括以下步骤：

第一步，读入初始时刻t₀的星图，进行星点提取后，利用全天球识别模式的星图识别算法获得t₀时刻的星敏感器测量矢量及相应的导航星信息；

第二步，令t＝t₀+δt，δt为星敏感器采样时间间隔，读入t时刻星图，进行星点提取、序列星图识别，对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的导航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1，具体方法如下：

2.1读入t时刻星图，进行星点提取，获得t时刻星图中星点的质心坐标，作为星点在星敏感器像面坐标系中的投影位置；

2.2进行序列星图识别，获得t时刻星图中识别的导航星i的星敏感器测量矢量v(t)_i，

$v{\left(t\right)}_i=\frac{1}{\sqrt{x^2{\left(t\right)}_i+y^2{\left(t\right)}_i+f^2}}\left[\begin{array}{c}x{\left(t\right)}_i\\ y{\left(t\right)}_i\\ f\end{array}\right]---\left(1\right)$

其中，x(t)_i和y(t)_i分别表示t时刻星图中识别的导航星i在星敏感器像面坐标系中的投影位置的x坐标和y坐标；f表示星敏感器镜头焦距；i为正整数，表示识别的导航星编号；

2.3对同时出现在前一帧星图和当前帧星图的导航星进行编号，具体方法为，统计同时出现在前一帧t-δt时刻和当前帧t时刻的星图中的导航星，将总数目记为n，

并按赤经由小到大编号为1，2，…k，…n；将导航星k在t-δt星图中的星敏感器测量矢量v(t-δt)_k和t时刻的星图中的星敏感器测量矢量v(t)_k，作为数据对记入集合Ω1，其中k为导航星序号，1≤k≤n；

第三步，对卡尔曼滤波器进行初始化：

3.1设置估计噪声协方差矩阵初值P₀为3×3的单位矩阵I_3×3；

3.2取Ω1中的数据，利用公式(2)，获得t₀+δt时刻的航天器姿态角速度初值

ω(t₀+δt)_x、ω(t₀+δt)_y、ω(t₀+δt)_z分别表示t₀+δt时刻的航天器在x、y、x方向上的姿态角速度分量；令 $X_0=\left[\begin{array}{c}\mathrm{\ω}{(t_0+\mathrm{\δt})}_x\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_y\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_z\end{array}\right],$ 将X₀作为卡尔曼滤波器的状态矢量的初值；

$\left[\begin{array}{c}\mathrm{\ω}{(t_0+\mathrm{\δt})}_x\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_y\\ \mathrm{\ω}{(t_0+\mathrm{\δt})}_z\end{array}\right]=-\frac{1}{\mathrm{\δt}}{\left\{\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{[v{(t_0+\mathrm{\δk})}_k\×]}^T\right[v{(t_0+\mathrm{\δt})}_k\×\left]\right\}}^{-1}\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{[v{(t_0+\mathrm{\δt})}_k\×]}^{T}v{\left(t_0\right)}_k---\left(2\right)$

式2中，[]^T表示矩阵转置，{}^-1表示矩阵求逆，[v(t₀+δt)_k×]表示t₀+δt时刻导航星k的星敏感器测量矢量 $v{(t_0+\mathrm{\δt})}_k=\frac{1}{\sqrt{x^2{(t_0+\mathrm{\δt})}_k+y^2{(t_0+\mathrm{\δt})}_k+f^2}}\left[\begin{array}{c}x{(t_0+\mathrm{\δt})}_k\\ y{(t_0+\mathrm{\δt})}_k\\ f\end{array}\right]$ 的叉乘矩阵，具体形式为：

$[v{(t_0+\mathrm{\δt})}_k\×]=\frac{1}{\sqrt{x^2{(t_0+\mathrm{\δt})}_k+y^2{(t_0+\mathrm{\δt})}_k+f^2}}\left[\begin{array}{ccc}0& -f& y{(t_0+\mathrm{\δt})}_k\\ f& 0& -x{(t_0+\mathrm{\δt})}_k\\ -y{(t_0+\mathrm{\δt})}_k& x{(t_0+\mathrm{\δt})}_k& 0\end{array}\right]---\left(3\right)$

第四步，估计航天器姿态角速度：

4.1取集合Ω1中的数据，以导航星序号k的增加作为时间序列，构建卡尔曼滤波器的状态方程和测量方程如下：

$\left\{\begin{array}{c}X{\left(t\right)}_k=I_{3\×3}X{\left(t\right)}_{k-1}+w\\ Z{\left(t\right)}_k=H{\left(t\right)}_{k}X{\left(t\right)}_k+u\end{array}\right.---\left(4\right)$

其中， $X\left(t\right)=\left[\begin{array}{c}\mathrm{\ω}{\left(t\right)}_x\\ \mathrm{\ω}{\left(t\right)}_y\\ \mathrm{\ω}{\left(t\right)}_z\end{array}\right]$ 表示t时刻的卡尔曼滤波器的状态矢量，ω(t)_x、ω(t)_y、ω(t)_z分别表示t时刻的航天器在x、y、x方向上的姿态角速度分量；X(t)_k和X(t)_k-1分别表示t时刻导航星k和导航星k-1对应的卡尔曼滤波器的状态矢量；

I_3×3为3×3单位矩阵；w_k-1表示导航星k-1对应的过程噪声，u_k表示导航星k对应的测量噪声；

表示t时刻星敏感器测量矢量k的一阶导；

观测矩阵H(t)_k＝[v(t)_k×]，[v(t)_k×]表示t时刻导航星k的星敏感器测量矢量 $v{\left(t\right)}_k=\frac{1}{\sqrt{x^2{\left(t\right)}_k+y^2{\left(t\right)}_k+f^2}}\left[\begin{array}{c}x{\left(t\right)}_x\\ {y\left(t\right)}_y\\ f\end{array}\right]$ 的叉乘矩阵，具体形式为：

$[v{\left(t\right)}_k\×]=\frac{1}{\sqrt{x^2{\left(t\right)}_k+y^2{\left(t\right)}_k+f^2}}\left[\begin{array}{ccc}0& -f& y{\left(t\right)}_k\\ f& 0& -x{\left(t\right)}_k\\ -y{\left(t\right)}_k& x{\left(t\right)}_k& 0\end{array}\right]---\left(5\right)$

4.2输入状态矢量的初值X₀、估计噪声协方差矩阵初值P₀、过程噪声协方差矩阵Q、测量噪声的协方差矩阵R，估计当前时刻的航天器姿态角速度估计值X(t)和估计噪声协方差矩阵P_n，具体步骤如下：

4.2.1令k＝1，输入状态矢量的初值X₀、估计噪声协方差矩阵初值P₀；

4.2.2采用公式(6)预估导航星k对应的卡尔曼滤波器的状态矢量，记为

$X{\left(t\right)}_k^{-}=I_{3\×3}X{\left(t\right)}_{k-1}---\left(6\right);$

4.2.3采用公式(7)预估导航星k对应的噪声协方差矩阵，记为

$P_k^{-}=P_{k-1}+Q---\left(7\right);$

4.2.4采用公式(8)计算导航星k对应的卡尔曼增益矩阵，记为K_k

$K_k=P_k^{-}{[P_k^{-}+R]}^{-1}---\left(8\right);$

4.2.5采用公式(9)更新估计，获得导航星k对应的的卡尔曼滤波器的状态矢量X(t)_k

$X{\left(t\right)}_k=X{\left(t\right)}_k^{-}+K_k[Z{\left(t\right)}_k-H{\left(t\right)}_{k}X{\left(t\right)}_k^{-}]---\left(9\right);$

4.2.6采用公式(10)计算更新后的导航星k对应的估计噪声协方差矩阵P_k

$P_k=[I_{3\×3}-K_{k}H{\left(t\right)}_k]P_k^{-}---\left(10\right);$

4.2.7如果k＝n，清空集合Ω1，输出X(t)_n和P_n，作为当前时刻的航天器姿态角速度估计值X(t)和估计噪声协方差矩阵，转第五步；否则，令k＝k+1，转步骤4.2.2；

第五步，重置滤波器初始值：

5.1设置估计噪声协方差矩阵初值P₀＝P_n，其中P_n为第四步最后得到的估计噪声协方差矩阵；

5.2利用线性插值的方法求下一帧星图对应的航天器姿态角速度预测值，将航天器姿态角速度预测值作为下一帧星图的卡尔曼滤波器的状态矢量的初值X₀，计算公式为：

X₀＝2X(t)-X₀    (11)

第六步，令t＝t+δt，读入t时刻星图，进行2.1所述的星点提取、2.2所述的序列星图识别，2.3所述的对同时出现在前一帧星图和当前帧星图的导航星进行编号，获得同时出现在t-δt和t时刻的星图中的导航星总数目n，以及对应的星敏感器测量矢量对的集合Ω1，转第四步。

Images