[发明专利]基于SR-UKF滤波的星敏感器和陀螺组合定姿方法

权利要求书

1.一种基于SR-UKF滤波的星敏感器和陀螺组合定姿方法，其特征在于，所述方法具体包括：

步骤1仿真出星敏感器四元数和陀螺的角速度；

步骤2以误差四元数及陀螺随机漂移误差为状态变量，利用SR-UKF算法融合处理星敏感器和陀螺的姿态信息进行滤波处理，并进行反馈，通过迭代滤波处理消除星敏感器和陀螺的误差影响，求解高精度的姿态信息；

步骤1中仿真的输入输出具体包括：

输入：

长半径；偏心率；轨道倾角；升交点赤经；近地点角距；过近地点时刻；地球引力常数；采样间隔；星敏感器噪声误差；第一星敏感器在卫星本体坐标系中的方向角；第二星敏感器在卫星本体坐标系中的方向角；

输出：

无误差姿态四元数；有误差姿态四元数；无误差陀螺角速度；有误差陀螺角速度；

步骤1具体包括：

步骤1a参数设定中设定卫星轨道参数、两个星敏感器的安装参数，采样频率；

步骤1b卫星轨道瞬时位置计算中是利用卫星的轨道参数计算卫星的位置，包括地心系与轨道坐标系之间的关系求解、卫星地心系坐标解算、卫星轨道坐标系解算；

步骤1c利用两个星敏感器的安装参数以及瞬时卫星位置信息，构建矢量信息，通过定姿计算得到无误差的卫星姿态信息；

步骤1d利用两个星敏感器的安装参数获取主光轴信息，添加随机噪声后，联合卫星的瞬时位置信息，构建矢量信息，采用QUEST定姿方法解算得到带误差的卫星姿态信息；

步骤1e利用解算得到的无误差的姿态信息，通过角速度的变换量计算，得到无误差的陀螺数据；

步骤1f利用得到的无误差陀螺数据，在三轴添加随机噪声，得到带误差的陀螺数据。

2.如权利要求1所述的基于SR-UKF滤波的星敏感器和陀螺组合定姿方法，其特征在于，所述方法中步骤2具体包括：

步骤2a参数设置

设置陀螺误差因数，将陀螺误差因数设置为从0.1到2.5，每隔0.2取一次；读入数据，包括带误差和不带误差的四元数姿态数据以及角速度姿态数据；状态变量为误差四元数和陀螺随机漂移误差，状态初值：

X₀＝[Δq_ini3×3 Δd_ini3×3]＝[0 0 0 0 0 0]

系统噪声矩阵

测量噪声协方差矩阵H＝[I_3×3 0_3×3]，其中q_rms为星敏测量中误差；

陀螺随机漂移的中误差d_rms＝10^-6[1 1 1]

初始

步骤2b设置状态方程与量测方程，状态方程如下：

x_k+1＝f(x_k)+w_k

量测方程：z_k＝h(x_k)+v_k，量测量是指由预测四元数和星敏感器得到的带误差姿态四元数二者之间的差值，q_predic为星敏感器输出的姿态四元数，q_g为陀螺数据转换的四元数；

步骤2c设置SR-UKF滤波输入的参量；

步骤2d计算权重

其中，λ＝α²(n+κ)-n,为刻度参数

步骤2e创建状态值x的sigma点

由时刻k-1的和k时刻的S_k来计算的Sigma点集

设状态向量为n维,为时刻k-1的状态向量估计值,P_k-1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:

每个sigma点可分为以下两部分；

步骤2f对状态方程进行UT变换

将步骤2e得到的sigma点代入非线性状态函数中，然后进行加权处理得到一步预测状态值再利用QR分解及Cholesky因式得到预测Sk；

χ_k|k-1＝f(χ_k-1)

其中，预测sigma点

步骤2g对量测方程进行UT变换

将sigma点代入量测函数中，然后进行加权处理得到一步预测量测值再利用QR分解及Cholesky因式得到预测Syk；

将步骤2f得到的预测sigma点带入非线性量测方程；

步骤2h计算互协方差矩阵值

步骤2i更新状态值及协方差矩阵值

步骤2j四元数修正

四元数修正是通过对预测四元数和状态更新后误差四元数做叉乘实现；姿态四元数修正方程：

为k时刻误差四元数估计值，为k时刻的四元数预测值。