[发明专利]一种改进的基于EKF的天文自主定轨算法

权利要求书

1.一种改进的基于EKF的天文自主定轨算法，其特征在于所述算法步骤如下：

一、参考坐标系由地心惯性坐标系转为滤波坐标系；

二、在滤波坐标系中线性化和离散化滤波器状态方程；

三、在滤波坐标系中进行EKF滤波；

四、参考坐标系由滤波坐标系转回为地心惯性坐标系。

2.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤一中，将参考坐标系由地心惯性坐标系转为滤波坐标系之前，需要对EKF滤波器参数初始化，具体步骤如下：

1）给滤波迭代次数                                                赋初值为；

2）设滤波器的状态变量，其中为地心到航天器的位置矢量，为航天器的速度矢量；给初始时刻的状态变量估计值赋初值，其中上角标说明该变量为地心惯性坐标系；

3）滤波时间赋初值；

4）给状态模型噪声的协方差矩阵赋值；

5）给观测模型噪声的协方差矩阵赋值；

6）给预估状态协方差赋初值。

3.根据权利要求1或2所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤一的具体步骤如下：

1）获取上一时刻航天器位置矢量的估计值以及这一时刻的观测信息矢量，其中表示上一时刻滤波器状态变量估计向量的前三个元素组成的列向量；

2）通过下式计算与其期望的参考坐标表示之间的夹角：

其中表示矩阵或向量的转置；

 3）通过下式计算和两个矢量所在平面的单位法方向量：

 4）通过下式计算地心惯性坐标系到滤波坐标系的转动四元数：

5）通过下式计算地心惯性坐标系到滤波坐标系的余弦转换矩阵：

其中为转动四元数的第个分量；

6）通过下式计算在滤波坐标系中的坐标表示和地心惯性坐标系中的速度矢量坐标表示在滤波坐标系中的坐标表示：

组合滤波坐标系中的位置矢量和速度矢量，得到滤波器状态变量在滤波坐标系中的坐标表示为：，这时的前三个分量相等，即满足以下等式：

 7）通过下式计算观测信息矢量在滤波坐标系中的坐标表示：

。

4.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤二的具体步骤如下：

1）计算滤波器状态方程的雅克比矩阵，其中为对时间的导数；

2）计算时刻的状态转移矩阵，其中为单位矩阵，为滤波时间步长。

5.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤三的具体步骤如下：

1）在滤波坐标系中进行状态预估：

其中表示滤波器的状态方程，表示的是时刻滤波器状态变量的初步计算值，为时刻的预估状态协方差，为时刻滤波器的预估状态协方差的初步计算值；

2）在滤波坐标系中进行状态更新：

 

其中表示滤波器的观测方程，为时刻的观测方程的雅克比矩阵，为增益矩阵，为单位矩阵，为时刻的预估状态协方差，为时刻滤波器状态变量的估计值在滤波坐标系下的表示。

6.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤四的具体步骤如下：

通过下式对步骤三的结果进行坐标反变换：

通过可得到航天器的位置矢量的估计值和速度矢量的估计值。

7.根据权利要求6所述的改进的基于EKF的天文自主定轨算法，其特征在于递增滤波迭代次数，然后跳回至步骤一，通过以上步骤的周期运行，可从滤波器状态变量的估计值中获得航天器当前时刻位置矢量的估计值和速度矢量的估计值。