CN103940433A

CN103940433A - Satellite attitude determining method based on improved self-adaptive square root UKF (Unscented Kalman Filter) algorithm

Info

Publication number: CN103940433A
Application number: CN201410198696.3A
Authority: CN
Inventors: 李敏; 王松艳; 张迎春; 耿云海; 李华义; 谢成清
Original assignee: Harbin Institute of Technology Shenzhen
Current assignee: Harbin Institute of Technology Shenzhen
Priority date: 2014-05-12
Filing date: 2014-05-12
Publication date: 2014-07-23
Anticipated expiration: 2034-05-12
Also published as: CN103940433B

Abstract

The invention relates to a satellite attitude determination method based on an improved self-adaptive square root UKF algorithm, which belongs to the technical field of satellite attitude determination. The invention solves the instability of the satellite attitude determination system caused by too large rounding errors in the numerical calculation of the existing EKF, UKF and SRUKF algorithms when the satellite attitude determination system is affected by uncertainty interference and noise and the impact on the satellite attitude determination system. The low accuracy of satellite attitude estimation and the weak ability to track the actual state of satellites. The main implementation process of the satellite attitude determination method is: use the improved adaptive square root UKF to estimate the error quaternion and gyro drift error; use the gyro measurement value and the estimated gyro drift error to substitute the attitude kinematics equation to calculate the attitude quaternion; The estimated error quaternion is used to correct the calculated attitude quaternion; the corrected attitude quaternion is used to calculate the attitude and determine the attitude of the satellite. The invention is applicable to the technical field of satellite attitude determination.

Description

A Satellite Attitude Determination Method Based on Improved Adaptive Square Root UKF Algorithm

技术领域technical field

本发明涉及星敏感器和陀螺的高精度卫星姿态确定方法，属于卫星姿态确定技术领域。The invention relates to a high-precision satellite attitude determination method of a star sensor and a gyroscope, and belongs to the technical field of satellite attitude determination.

背景技术Background technique

星敏感器和陀螺组成的姿态测量系统，由于其精度较高，因而被广泛用于卫星姿态确定系统中。针对其组成的非线性系统，采用非线性滤波技术确定姿态的方法被广泛应用。扩展卡尔曼滤波器(EKF)因为其方法简单，容易实现等优点，在处理非线性估计的工程问题中被广泛应用。但是EKF是对非线性方程做一阶近似处理，忽略其余高阶项，从而将非线性问题转化为线性问题。当系统非线性较强时，EKF违背局部线性假设，被忽略的高阶项会带来大的误差，导致EKF算法精度下降，甚至造成EKF算法的滤波发散。另外EKF在线性化处理时需要计算雅克比(Jacobian)矩阵，其计算过程繁琐复杂且容易出错。The attitude measurement system composed of star sensor and gyroscope is widely used in the satellite attitude determination system because of its high precision. For the nonlinear system composed of it, the method of determining attitude using nonlinear filtering technology is widely used. The Extended Kalman Filter (EKF) is widely used in engineering problems dealing with nonlinear estimation because of its simple method and easy implementation. However, EKF makes a first-order approximation to nonlinear equations and ignores other high-order terms, thereby converting nonlinear problems into linear problems. When the nonlinearity of the system is strong, the EKF violates the local linear assumption, and the neglected high-order terms will bring large errors, resulting in a decrease in the accuracy of the EKF algorithm, and even causing the filter divergence of the EKF algorithm. In addition, the EKF needs to calculate the Jacobian matrix during the linearization process, and the calculation process is cumbersome and error-prone.

针对上述问题，Julier等人提出了无忌卡尔曼滤波器(UKF)算法。相对于EKF，UKF采用UT变换对非线性概率密度分布进行近似，具有不需要计算Jacobian矩阵，估计精度更高，近年来在飞行器姿态估计问题当中获得广泛应用。然而UKF在数值计算中往往会存在舍入误差，可能会破坏系统估计误差协方差矩阵的非负定性和对称性，导致算法的收敛速度慢，甚至造成算法的不稳定。针对该问题平方根UKF(SRUKF)滤波算法可以较好的解决，该方法借鉴kalman滤波中平方根分解滤波思想，在滤波的过程中采用矩阵的QR分解和Cholesky分解结果直接传播并更新协方差矩阵的平方根，解决了UKF算法中计算误差可能引起的误差协方差的负定性问题，提高了滤波算法的计算效率和数值稳定性。但是，当数值累积误差太大或者权值W ₀ ^c选择不合适的时候，标准的平方根UKF仍然存在滤波发散问题。此外，EKF，UKF和SRUKF都要求系统模型精确和噪声统计特性已知。当系统存在不确定性干扰和噪声作用时，上述方法都不具备较好的估计精度、跟踪能力及鲁棒性。To solve the above problems, Julier et al. proposed the Unscrupulous Kalman Filter (UKF) algorithm. Compared with EKF, UKF uses UT transformation to approximate the nonlinear probability density distribution. It does not need to calculate the Jacobian matrix and has higher estimation accuracy. It has been widely used in aircraft attitude estimation in recent years. However, UKF often has rounding errors in numerical calculations, which may destroy the non-negative definiteness and symmetry of the system estimation error covariance matrix, resulting in slow convergence of the algorithm and even instability of the algorithm. To solve this problem, the square root UKF (SRUKF) filtering algorithm can be better solved. This method draws on the idea of square root decomposition filtering in kalman filtering. In the process of filtering, the QR decomposition and Cholesky decomposition results of the matrix are used to directly propagate and update the square root of the covariance matrix. , which solves the negative qualitative problem of error covariance that may be caused by calculation errors in the UKF algorithm, and improves the calculation efficiency and numerical stability of the filtering algorithm. However, when the numerical accumulation error is too large or the weight W ₀ ^c is not selected properly, the standard square root UKF still has the problem of filter divergence. In addition, EKF, UKF and SRUKF all require the system model to be accurate and the noise statistics to be known. When the system has uncertain interference and noise, none of the above methods have good estimation accuracy, tracking ability and robustness.

发明内容Contents of the invention

本发明的目的是提出一种基于改进的自适应平方根UKF算法的卫星姿态确定方法，以解决在卫星姿态确定系统受到不确定性干扰和噪声的影响时，由于现有的EKF,UKF和SRUKF算法数值计算的舍入误差太大所引起的卫星姿态确定系统的不稳定以及对卫星姿态估计的精度低和卫星实际状态跟踪能力弱等问题。The purpose of this invention is to propose a satellite attitude determination method based on the improved self-adaptive square root UKF algorithm, to solve when the satellite attitude determination system is affected by uncertainty interference and noise, due to the existing EKF, UKF and SRUKF algorithm The instability of the satellite attitude determination system caused by the large rounding error of the numerical calculation, the low precision of the satellite attitude estimation and the weak tracking ability of the actual state of the satellite and so on.

本发明为解决上述技术问题所采用的技术方案是：The technical scheme that the present invention adopts for solving the problems of the technologies described above is:

本发明所述的一种基于改进的自适应平方根UKF算法的卫星姿态确定方法，是按照以下步骤实现的：步骤一、建立陀螺测量模型；步骤二、建立卫星姿态运动学方程；步骤三、建立基于误差四元数和陀螺漂移误差组成的状态变量的系统状态方程；步骤四、建立误差系统观测方程；步骤五、利用改进的自适应平方根UKF估计误差四元数和陀螺漂移误差；步骤六、利用陀螺测量值和估计出来的陀螺漂移误差代入姿态运动学方程计算姿态四元数；步骤七、利用估计出的误差四元数对解算出的姿态四元数进行修正；步骤八、利用修正的姿态四元数进行姿态解算，确定卫星的姿态。A kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm of the present invention is realized according to the following steps: Step 1, establishes the gyroscope measurement model; Step 2, establishes satellite attitude kinematics equation; Step 3, establishes The system state equation of the state variable based on the error quaternion and the gyro drift error; Step 4, establish the error system observation equation; Step 5, use the improved adaptive square root UKF to estimate the error quaternion and the gyro drift error; Step 6, Use the gyro measurement value and the estimated gyro drift error to substitute the attitude kinematics equation to calculate the attitude quaternion; step seven, use the estimated error quaternion to correct the attitude quaternion calculated by the solution; step eight, use the corrected The attitude quaternion calculates the attitude and determines the attitude of the satellite.

本发明的有益效果是：The beneficial effects of the present invention are:

一、本发明改善了由于现有算法的不稳定所导致的卫星姿态确定系统的稳定性，使卫星姿态确定系统的稳定性更好。1. The present invention improves the stability of the satellite attitude determination system caused by the instability of the existing algorithm, so that the stability of the satellite attitude determination system is better.

二、在改进的平方根UKF基础上引入自适应因子μ_k+1，使得平方根UKF具有自适应性，从而在系统具有未知干扰的情况下，提高了卫星姿态估计精度。2. On the basis of the improved square root UKF, an adaptive factor μ _k+1 is introduced to make the square root UKF self-adaptive, thereby improving the accuracy of satellite attitude estimation when the system has unknown interference.

三、本发明改善了UKF和平方根UKF对未知干扰及突变状态的跟踪能力，使卫星实际状态跟踪能力增强，从而在模型不确定性和噪声统计特性不确定时也能够较好的估计出卫星的姿态，因此在复杂多变的环境当中更适用。3. The present invention improves the tracking ability of UKF and square root UKF to unknown interference and mutation state, and enhances the tracking ability of the actual state of the satellite, so that the satellite's tracking ability can be better estimated when the model uncertainty and noise statistical characteristics are uncertain. attitude, so it is more applicable in complex and changeable environments.

四、相对于STF方法，本发明(IASRUKF)在滚转角的估计精度方面提高了66％～67％，在俯仰角的估计精度方面提高了68％～69％，在偏航角的估计精度方面提高了58％～％59；相对于SRUKF方法，本发明在滚转角的估计精度方面提高了97％～98％，在俯仰角的估计精度方面提高了97％～98％，在偏航角的估计精度方面提高了97％～98％。Four, with respect to STF method, the present invention (IASRUKF) has improved 66%～67% aspect the estimation precision of roll angle, has improved 68%～69% aspect the estimation precision of pitch angle, aspect the estimation precision of yaw angle Improve 58%～%59; Compared with the SRUKF method, the present invention has improved 97%～98% aspect the estimation precision of roll angle, has improved 97%～98% aspect the estimation precision of pitch angle, and has improved 97%～98% in the estimation precision of yaw angle. Estimated accuracy has increased by 97% to 98%.

附图说明Description of drawings

图1为本发明的流程图；图2为卫星姿态误差系统的改进的自适应平方根UKF算法流程图；图3为STF(强跟踪滤波器)、SRUKF(平方根UKF)和本发明的IASRUKF的滚转角误差曲线对比图；图4为STF(强跟踪滤波器)、SRUKF(平方根UKF)和本发明的IASRUKF的俯仰角误差曲线对比图；图5为STF(强跟踪滤波器)、SRUKF(平方根UKF)和本发明的IASRUKF的偏航角误差曲线对比图。Fig. 1 is a flow chart of the present invention; Fig. 2 is the improved adaptive square root UKF algorithm flow chart of satellite attitude error system; Fig. 3 is the rolling of STF (strong tracking filter), SRUKF (square root UKF) and IASRUKF of the present invention Corner error curve contrast figure; Fig. 4 is the pitch angle error curve contrast figure of STF (strong tracking filter), SRUKF (square root UKF) and IASRUKF of the present invention; Fig. 5 is STF (strong tracking filter), SRUKF (square root UKF ) and the yaw angle error curve comparison chart of IASRUKF of the present invention.

具体实施方式Detailed ways

具体实施方式一：本实施方式所述的一种基于改进的自适应平方根UKF算法的卫星姿态确定方法，其特征在于所述方法包括以下步骤：Specific embodiment one: a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm described in the present embodiment is characterized in that described method comprises the following steps:

步骤一、建立陀螺测量模型；Step 1. Establish a gyroscope measurement model;

步骤二、建立卫星姿态运动学方程；Step 2, establishing satellite attitude kinematic equations;

步骤三、建立基于误差四元数和陀螺漂移误差组成的状态变量的系统状态方程；Step 3, establishing the system state equation based on the state variable composed of the error quaternion and the gyro drift error;

步骤四、建立误差系统观测方程；Step 4, establishing the error system observation equation;

步骤五、利用改进的自适应平方根UKF估计误差四元数和陀螺漂移误差；Step 5, using the improved adaptive square root UKF to estimate the error quaternion and gyro drift error;

步骤六、利用陀螺测量值和估计出来的陀螺漂移误差代入姿态运动学方程计算姿态四元数；Step 6. Substituting the gyro measurement value and the estimated gyro drift error into the attitude kinematics equation to calculate the attitude quaternion;

步骤七、利用估计出的误差四元数对解算出的姿态四元数进行修正；Step 7, using the estimated error quaternion to correct the attitude quaternion calculated by the solution;

步骤八、利用修正的姿态四元数进行姿态解算，确定卫星的姿态。Step 8, using the corrected attitude quaternion to calculate the attitude and determine the attitude of the satellite.

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤一所述的建立陀螺测量模型的具体过程为：在陀螺测量坐标系与星体的坐标系为同一坐标系的情况下，陀螺测量模型为Embodiment 2: The difference between this embodiment and Embodiment 1 is that the specific process of establishing the gyro measurement model described in step 1 is: when the gyro measurement coordinate system and the coordinate system of the star are the same coordinate system, the gyro The measurement model is

$\{\begin{matrix} g g ((t t)) = = ω ω ((t t)) + + β β ((t t)) + + {η η}_{u u} ((t t)) \\ \overset{\cdot &Center Dot;}{β β ((t t))} = = {η η}_{v v} ((t t)) \end{matrix} - - - - - - ((11))$

式中，g(t)为陀螺的测量输出值,ω(t)为陀螺真实角速度，β(t)为陀螺漂移，η_u(t)和η_v(t)为互不相关的高斯噪声，满足：where g(t) is the measured output value of the gyro, ω(t) is the true angular velocity of the gyro, β(t) is the drift of the gyro, _ηu (t) and _ηv (t) are uncorrelated Gaussian noises, satisfy:

$\{\begin{matrix} E E. {{{η η}_{u u} ((t t)) {η η}_{u u}^{T T} ((τ τ))}} = = {σ σ}_{u u}^{22} δ δ ((t t - - τ τ)) {I I}_{33 \times \times 33} \\ E E. {{{η η}_{v v} ((t t)) {η η}_{v v}^{T T} ((τ τ))}} = = {σ σ}_{v v}^{22} δ δ ((t t - - τ τ)) {I I}_{33 \times \times 33} \end{matrix} - - - - - - ((22))$

式中，和分别为白噪声，η_u(t)和η_v(t)为均方差，δ(t-τ)为狄拉克函数。其它步骤及参数与具体实施方式一相同。In the formula, and are white noise, η _u (t) and η _v (t) are mean square errors, and δ(t-τ) is a Dirac function. Other steps and parameters are the same as those in Embodiment 1.

具体实施方式三：本实施方式与具体实施方式一或二不同的是：步骤二所述的建立卫星姿态运动学方程的具体过程为：卫星姿态四元数被定义为Specific embodiment three: the difference between this embodiment and specific embodiment one or two is: the specific process of establishing the satellite attitude kinematics equation described in step two is: the satellite attitude quaternion is defined as

q＝[q₁ q₂ q₃ q₄]^T (3)q＝[q ₁ q ₂ q ₃ q ₄ ] ^T (3)

式中，q₄＝cos(θ/2)，和θ分别为单位旋转向量和旋转角；In the formula, q ₄ =cos(θ/2), and θ are unit rotation vector and rotation angle respectively;

四元数满足如下约束：Quaternions satisfy the following constraints:

${q q}_{11}^{22} + + {q q}_{22}^{22} + + {q q}_{33}^{22} + + {q q}_{44}^{22} = = 11 - - - - - - ((44))$

用四元数表示卫星姿态运动学方程为：The satellite attitude kinematics equation expressed by quaternion is:

$\frac{dq dq ((t t))}{dt dt} = = \frac{11}{22} Ω Ω ((ω ω ((t t)))) q q ((t t)) - - - - - - ((55))$

式中，ω(t)为卫星的真实角速度，且where ω(t) is the true angular velocity of the satellite, and

$Ω (ω (t)) = {[\begin{matrix} - {[ω (t) \times]}_{3 \times 3} & ω (t) \\ - ω^{T} (t) & 0 \end{matrix}]}_{4 \times 4}, [ω (t) \times] = [\begin{matrix} 0 & - ω_{z} (t) & ω_{y} (t) \\ ω_{z} (t) & 0 & - ω_{x} (t) \\ - ω_{y} (t) & ω_{x} (t) & 0 \end{matrix}] .$ 其它步骤及参数与具体实施方式一或二相同。 $Ω (ω (t)) = {[\begin{matrix} - {[ω (t) \times]}_{3 \times 3} & ω (t) \\ - ω^{T} (t) & 0 \end{matrix}]}_{4 \times 4}, [ω (t) \times] = [\begin{matrix} 0 & - ω_{z} (t) & ω_{the y} (t) \\ ω_{z} (t) & 0 & - ω_{x} (t) \\ - ω_{the y} (t) & ω_{x} (t) & 0 \end{matrix}] .$ Other steps and parameters are the same as those in Embodiment 1 or Embodiment 2.

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：步骤三所述的建立基于误差四元数和陀螺漂移误差组成的状态变量的系统状态方程的具体过程为：四元数存在范数约束，如果选择将四个四元数分量作为状态变量，则方差矩阵是奇异的。因此为了避免方差矩阵奇异的问题，采用乘性四元数定义真实四元数与四元数计算值之间的误差四元数为状态变量：Specific embodiment four: the difference between this embodiment and one of the specific embodiments one to three is: the specific process of establishing the system state equation based on the state variable formed by the error quaternion and the gyro drift error described in step 3 is: quaternion There is a norm constraint on the number, and if the four quaternion components are chosen as state variables, the variance matrix is singular. Therefore, in order to avoid the problem of the singularity of the variance matrix, the multiplicative quaternion is used to define the real quaternion and the calculated value of the quaternion Error between quaternions for state variables:

$δq δq = = {\overset{^^}{q q}}^{- - 11} &CircleTimes; &CircleTimes; q q = = [\begin{matrix} δe δe & δq δq \end{matrix}] - - - - - - ((66))$

取误差四元数的矢量部分δe和陀螺漂移误差△b作为状态变量，系统的状态变量为△x＝[δe △b]，由卫星姿态运动学方程有：Taking the vector part δe of the error quaternion and the gyro drift error △b as the state variables, the state variable of the system is △x=[δe △b], and the satellite attitude kinematic equation is:

$\overset{\cdot &Center Dot;}{q q} = = \frac{11}{22} q q &CircleTimes; &CircleTimes; ω ω - - - - - - ((77))$

$\overset{\overset{\cdot \cdot}{^^}}{q q} = = \frac{11}{22} \overset{^^}{q q} &CircleTimes; &CircleTimes; \overset{^^}{ω ω} - - - - - - ((88))$

式中，是卫星角速度的计算值；In the formula, is the calculated value of satellite angular velocity;

对式(6)求导，并将式(7)和式(8)代入式(6)，得：Deriving formula (6), and substituting formula (7) and formula (8) into formula (6), we get:

$δ δ \overset{\cdot \cdot}{q q} = = \frac{11}{22} [[δq δq &CircleTimes; &CircleTimes; ω ω - - \overset{^^}{ω ω} &CircleTimes; &CircleTimes; δq δq]] - - - - - - ((99))$

定义：definition:

$δω δω = = [[ω ω - - \overset{^^}{ω ω}]] = = [[((b b - - \overset{^^}{b b})) + + {η η}_{u u}]] - - - - - - ((1010))$

将式(10)代入式(9)得：Substitute formula (10) into formula (9):

$δ δ \overset{\cdot &Center Dot;}{q q} = = \frac{11}{22} [[δq δq &CircleTimes; &CircleTimes; \overset{^^}{ω ω} - - \overset{^^}{ω ω} &CircleTimes; &CircleTimes; δq δq]] + + \frac{11}{22} δq δq &CircleTimes; &CircleTimes; δω δω - - - - - - ((1111))$

将式(11)线性化，有：Linearize formula (11), we have:

$δ δ \overset{\cdot &Center Dot;}{q q} = = - - \overset{^^}{ω ω} \times \times δe δe - - \frac{11}{22} ((Δb Δb + + {η η}_{u u})) - - - - - - ((1212))$

则可得误差状态变量的系统状态方程为:Then the system state equation of the error state variable can be obtained as:

$Δ Δ \overset{\cdot \cdot}{X x} ((t t)) = = F f ((t t)) ΔX ΔX ((t t)) + + G G ((t t)) ϵ ϵ ((t t)) - - - - - - ((1313))$

式中，In the formula,

$F (t) = [\begin{matrix} - [ω (t) \times] & \frac{1}{2} I_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}], G (t) = [\begin{matrix} - \frac{1}{2} I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{matrix}], ϵ (t) = \begin{matrix} [\begin{matrix} η_{u} \\ 0 \end{matrix}] \end{matrix} .$ 其它步骤及参数与具体实施方式一至三之一相同。 $f (t) = [\begin{matrix} - [ω (t) \times] & \frac{1}{2} I_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}], G (t) = [\begin{matrix} - \frac{1}{2} I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{matrix}], ϵ (t) = \begin{matrix} [\begin{matrix} η_{u} \\ 0 \end{matrix}] \end{matrix} .$ Other steps and parameters are the same as those in Embodiments 1 to 3.

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤四所述的建立误差系统观测方程的具体过程为：由于采用的是将四元数q作为输出量，将星敏感器得到的四元数误差作为测量输出，取其矢量部分作为观测量，有观测方程：Specific embodiment five: the difference between this embodiment and one of the specific embodiments one to four is that the specific process of establishing the error system observation equation described in step four is: because the quaternion q is used as the output, the star The quaternion error obtained by the sensor is used as the measurement output, and its vector part is taken as the observation quantity, and there is an observation equation:

z＝Hx+v (14)z=Hx+v (14)

式中，H为观测矩阵，v为观测噪声；In the formula, H is the observation matrix, and v is the observation noise;

$H h = = [\begin{matrix} 11 & 00 & 00 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 & 00 \end{matrix}]$

采用四阶龙格库塔法对式(13)和式(14)组成的姿态误差系统方程进行离散化。其它步骤及参数与具体实施方式一至四之一相同。The fourth-order Runge-Kutta method is used to discretize the attitude error system equation composed of formula (13) and formula (14). Other steps and parameters are the same as one of the specific embodiments 1 to 4.

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤五所述的利用改进的自适应平方根UKF估计误差四元数和陀螺漂移误差的具体过程为：Specific embodiment six: the difference between this embodiment and one of specific embodiments one to five is: the specific process of using the improved adaptive square root UKF estimation error quaternion and gyro drift error described in step five is:

(1)初始化状态状态误差协方差矩阵平方根S₀为(1) Initialization state The square root S ₀ of the state error covariance matrix is

${\overset{^^}{X x}}_{00} = = E E. [[{X x}_{00}]] - - - - - - ((1515))$

${S S}_{00} = = chol chol ((E E. [[(({X x}_{00} - - {\overset{^^}{X x}}_{00} {(({X x}_{00} - - {\overset{^^}{X x}}_{00}))}^{T T}))]])) - - - - - - ((1616))$

(2)对于k＝1,2,3,…,n实现步骤如下：(2) For k=1,2,3,...,n, the implementation steps are as follows:

①计算sigma点① Calculate the sigma point

${ξ ξ}_{k k} = = [\begin{matrix} {\overset{^^}{X x}}_{k k} & {\overset{^^}{X x}}_{k k} + + γ γ {S S}_{k k} & {\overset{^^}{X x}}_{k k} - - γ γ {S S}_{k k} \end{matrix}] - - - - - - ((1717))$

式中，λ＝α²(n+κ)-n，α的取值在0.001～1之间，n为系统状态向量的维数，κ为第三刻度因数，取值为0；In the formula, λ=α ² (n+κ)-n, the value of α is between 0.001 and 1, n is the dimension of the system state vector, κ is the third scale factor, and the value is 0;

②时间更新②Time update

Sigma采样点的非线性传播：Nonlinear propagation of Sigma sampling points:

ξ_i,k+1|k＝F_k+1|kξ_i,k (18)ξ _i,k+1|k = F _k+1|k ξ _i,k (18)

状态值的一步预测值计算：Computation of one-step predictors of state values:

${\overset{^^}{X x}}_{k k + + 11 | | k k} = = {Σ Σ}_{i i = = 00}^{22 n no} {W W}_{i i}^{m m} {ξ ξ}_{k k + + 11 | | k k} - - - - - - ((1919))$

观测值的一步预测计算：One-step forecast calculation for observations:

${\overset{^^}{Z Z}}_{k k + + 11 | | k k} = = H h {\overset{^^}{X x}}_{k k + + 11 | | k k} - - - - - - ((2020))$

预测值与真实值的残差计算：The residual calculation of the predicted value and the actual value:

${e e}_{k k + + 11} = = {Z Z}_{k k + + 11} - - {\overset{^^}{Z Z}}_{k k + + 11 | | k k} - - - - - - ((21 twenty one))$

③计算自适应渐消因子μ_k+1 ③ Calculate the adaptive fading factor μ _k+1

${μ μ}_{k k + + 11,, i i} = = \{\begin{matrix} {μ μ}_{k k + + 11,, i i},, {μ μ}_{k k + + 11,, i i} &GreaterEqual; &Greater Equal; 11 \\ 11,, {μ μ}_{k k + + 11,, i i} < < 11 \end{matrix},, i i = = 1,2 1,2,, . . . . . .,, n no - - - - - - ((22 twenty two))$

μ_k+1,i＝trace(N_k+1)/trace(M_ii,k+！) (23)μ _k+1,i =trace(N _k+1 )/trace(M _ii,k+! ) (23)

N_k+1＝V_k+1-HQ_k+1H^T-lR_k+1 (24)N _k+1 = V _k+1 -HQ _k+1 H ^T -lR _k+1 (24)

M_k+1＝J_k+1HH^T (25)M _k+1 = J _k+1 HH ^T (25)

${J J}_{k k + + 11} = = {Σ Σ}_{i i = = 11}^{22 n no} {W W}_{i i}^{c c} (({ξ ξ}_{k k + + 11 | | k k,, i i} - - {ξ ξ}_{00,, k k + + 11 | | k k})) {(({ξ ξ}_{k k + + 11 | | k k,, i i} - - {ξ ξ}_{00,, k k + + 11 | | k k}))}^{T T} - - - - - - ((2626))$

${V V}_{k k + + 11} = = \{\begin{matrix} {e e}_{k k + + 11} {e e}_{k k + + 11}^{T T},, k k = = 00 \\ \frac{ρ ρ {V V}_{k k} + + {e e}_{k k + + 11} {e e}_{k k + + 11}^{T T}}{11 + + ρ ρ},, k k &GreaterEqual; &Greater Equal; 11 \end{matrix} - - - - - - ((2727))$

④测量更新④Measurement update

状态一步预测协方差矩阵的平方根计算：Computation of the square root of the state one-step forecast covariance matrix:

${S S}_{k k + + 11 | | k k} = = qr qr {{[[diag diag (({μ μ}_{k k + + 11})) \sqrt{{W W}_{11}^{c c}} (({ξ ξ}_{11 : : 22 n no,, k k + + 11 | | k k} - - {ξ ξ}_{00,, k k + + 11 | | k k})),, {G G}_{k k} \sqrt{{Q Q}_{k k}}]]}} - - - - - - ((2828))$

互协方差矩阵计算：Cross-covariance matrix calculation:

${P P}_{XZ XZ,, k k + + 11} = = (({S S}_{k k + + 11 | | k k} {S S}_{k k + + 11 | | k k}^{T T})) {H h}^{T T} - - - - - - ((2929))$

输出协方差矩阵的平方根计算：Calculate the square root of the output covariance matrix:

U_k+1＝HS_k+1|k (30)U _k+1 ＝HS _k+1|k (30)

${S S}_{Z Z,, k k + + 11} = = qr qr {{[[{U u}_{k k + + 11},, \sqrt{{R R}_{k k + + 11}}]]}} - - - - - - ((3131))$

状态增益矩阵计算：State gain matrix calculation:

${K K}_{k k + + 11} = = (({P P}_{XZ XZ,, k k + + 11} / / {S S}_{Z Z,, k k + + 11}^{T T})) / / {S S}_{Z Z,, k k + + 11} - - - - - - ((3232))$

状态估计值计算：State estimate calculation:

${\overset{^^}{X x}}_{k k + + 11 | | k k + + 11} = = {\overset{^^}{X x}}_{k k + + 11 | | k k} + + {K K}_{k k + + 11} {e e}_{k k + + 11} - - - - - - ((3333))$

状态误差协方差矩阵的平方根计算：The square root calculation of the state error covariance matrix:

${S S}_{k k + + 11} = = {S S}_{k k + + 11 | | k k} - - {S S}_{k k + + 11 | | k k} {U u}_{k k + + 11}^{T T} {S S}_{z z,, k k + + 11}^{- - T T} {(({S S}_{z z,, k k + + 11} + + \sqrt{{R R}_{k k + + 11}}))}^{- - 11} {U u}_{k k + + 11} - - - - - - ((3434))$

上述过程中用到的参数计算如下：The parameters used in the above process are calculated as follows:

${W W}_{00}^{m m} = = \frac{λ λ}{n no + + λ λ} - - - - - - ((3535))$

${W W}_{00}^{c c} = = {W W}_{00}^{m m} + + ((11 - - {α α}^{22} + + β β)) - - - - - - ((3636))$

其它步骤及参数与具体实施方式一至五之一相同。 Other steps and parameters are the same as one of the specific embodiments 1 to 5.

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：步骤八所述的姿态解算方法为：Specific embodiment seven: the difference between this embodiment and one of the specific embodiments one to six is that the attitude calculation method described in step eight is:

θ＝arcsin(-2(q₁q₃-q₂q₄)) (39)θ=arcsin(-2(q ₁ q ₃ -q ₂ q ₄ )) (39)

$ψ ψ = = arctan arctan ((22 (({q q}_{22} {q q}_{11} + + {q q}_{33} {q q}_{44})) / / ((11 - - 22 (({q q}_{33}^{22} + + {q q}_{22}^{22})))) - - - - - - ((4040))$

式中，θ，ψ分别代表滚转角、俯仰角和偏航角。其它步骤及参数与具体实施方式一至六之一相同。In the formula, θ, ψ represent roll angle, pitch angle and yaw angle respectively. Other steps and parameters are the same as one of the specific embodiments 1 to 6.

为了说明本发明的优点，将本发明和强跟踪滤波器(STF)方法及平方根UKF(SRUKF)方法相比较,仿真参数设置为：卫星的初始姿态角速度为：ω₀＝10^-2×[2 2 2]^Tdeg/s，陀螺漂移为：β₀＝[0.05 0.05 0.05]^Tdeg/s，陀螺的测量噪声和漂移噪声的均方差分别为：σ_u＝0.5(°)/h和σ_v＝0.04(°)/h，测量噪声的均方差为：σ_q＝10″，滤波采样时间为△t＝1s，在仿真时间为200s的时候，加入突变干扰：△x＝[0,0,0,0.02,0.02,0.02]^T，在仿真时间为500s的时候，将系统的噪声统计特性扩大20倍。得到仿真实验结果如图3～图5所示。通过对仿真实验结果的分析，本发明的估计精度是最好的，相对于STF方法，本发明(IASRUKF)在滚转角的估计精度方面提高了66％～67％，在俯仰角的估计精度方面提高了68％～69％，在偏航角的估计精度方面提高了58％～％59；相对于SRUKF方法，本发明在滚转角的估计精度方面提高了97％～98％，在俯仰角的估计精度方面提高了97％～98％，在偏航角的估计精度方面提高了97％～98％。In order to illustrate the advantages of the present invention, the present invention is compared with the strong tracking filter (STF) method and the square root UKF (SRUKF) method, and the simulation parameters are set to: the initial attitude angular velocity of the satellite is: ω ₀ = ^10-2 × [2 2 2] ^T deg/s, the gyro drift is: β ₀ =[0.05 0.05 0.05] ^T deg/s, the mean square error of the measurement noise and drift noise of the gyro are: σ _u ＝0.5(°)/h and σ _v =0.04(°)/h, the mean square error of measurement noise is: σ _q =10″, the filter sampling time is △t=1s, when the simulation time is 200s, add mutation interference: △x=[0,0, 0,0.02,0.02,0.02] ^T , when the simulation time is 500s, the noise statistical characteristics of the system will be expanded by 20 times. The simulation results are shown in Figure 3-5. Through the analysis of the simulation results, the The estimation accuracy of the invention is the best. Compared with the STF method, the invention (IASRUKF) has improved the estimation accuracy of the roll angle by 66% to 67%, and the estimation accuracy of the pitch angle by 68% to 69%. The estimating accuracy of the yaw angle has been improved by 58% to 59%; compared with the SRUKF method, the present invention has improved the estimating accuracy of the roll angle by 97% to 98%, and has improved the estimating accuracy of the pitch angle by 97% to 98%. %, the estimation accuracy of the yaw angle has increased by 97% to 98%.

Claims

1. a satellite attitude determining method based on the improved self-adaptive square root UKF algorithm, is characterized in that described method comprises the following steps:

Step 1. Establish a gyroscope measurement model;

Step 2, establishing satellite attitude kinematic equations;

Step 3, establishing the system state equation based on the state variable composed of the error quaternion and the gyro drift error;

Step 4, establishing the error system observation equation;

Step 5, using the improved adaptive square root UKF to estimate the error quaternion and gyro drift error;

Step 6. Substituting the gyro measurement value and the estimated gyro drift error into the attitude kinematics equation to calculate the attitude quaternion;

Step 7, using the estimated error quaternion to correct the attitude quaternion calculated by the solution;

Step 8, using the corrected attitude quaternion to calculate the attitude and determine the attitude of the satellite.

2. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 1, it is characterized in that the concrete process of setting up the gyro measurement model described in step 1 is: in the gyro measurement coordinate system and the star body When the coordinate system is the same coordinate system, the gyro measurement model is

\{\begin{matrix} g g ((t t)) = = ω ω ((t t)) + + β β ((t t)) + + {η η}_{u u} ((t t)) \\ \overset{\cdot \cdot}{β β ((t t))} = = {η η}_{v v} ((t t)) \end{matrix} - - - - - - ((11))

where g(t) is the measured output value of the gyro, ω(t) is the true angular velocity of the gyro, β(t) is the drift of the gyro, _ηu (t) and _ηv (t) are uncorrelated Gaussian noises, satisfy:

\{\begin{matrix} E E. {{{η η}_{u u} ((t t)) {η η}_{u u}^{T T} ((τ τ))}} = = {σ σ}_{u u}^{22} δ δ ((t t - - τ τ)) {I I}_{33 \times \times 33} \\ E E. {{{η η}_{v v} ((t t)) {η η}_{v v}^{T T} ((τ τ))}} = = {σ σ}_{v v}^{22} δ δ ((t t - - τ τ)) {I I}_{33 \times \times 33} \end{matrix} - - - - - - ((22))

In the formula, and are white noise, η _u (t) and η _v (t) are mean square errors, and δ(t-τ) is a Dirac function.

3. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 2, it is characterized in that the concrete process of setting up the satellite attitude kinematics equation described in step 2 is: the satellite attitude quaternion is defined as

q＝[q ₁ q ₂ q ₃ q ₄ ] ^T (3)

In the formula, q ₄ =cos(θ/2), and θ are unit rotation vector and rotation angle respectively;

Quaternions satisfy the following constraints:

{q q}_{11}^{22} + + {q q}_{22}^{22} + + {q q}_{33}^{22} + + {q q}_{44}^{22} = = 11 - - - - - - ((44))

The satellite attitude kinematics equation expressed by quaternion is:

\frac{dq dq ((t t))}{dt dt} = = \frac{11}{22} Ω Ω ((ω ω ((t t)))) q q ((t t)) - - - - - - ((55))

where ω(t) is the true angular velocity of the satellite, and

Ω Ω ((ω ω ((t t)))) = = {[\begin{matrix} - - {[[ω ω ((t t)) \times \times]]}_{33 \times \times 33} & ω ω ((t t)) \\ - - {ω ω}^{T T} ((t t)) & 00 \end{matrix}]}_{44 \times \times 44},, [[ω ω ((t t)) \times \times]] = = [\begin{matrix} 00 & - - {ω ω}_{z z} ((t t)) & {ω ω}_{y the y} ((t t)) \\ {ω ω}_{z z} ((t t)) & 00 & - - {ω ω}_{x x} ((t t)) \\ - - {ω ω}_{y the y} ((t t)) & {ω ω}_{x x} ((t t)) & 00 \end{matrix}] . .

4. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 3, is characterized in that the described establishment of step 3 is based on the system state of the state variable that error quaternion and gyro drift error form The specific process of the equation is: use the multiplicative quaternion to define the real quaternion and the calculated value of the quaternion Error between quaternions for state variables:

δq δq = = {\overset{^^}{q q}}^{- - 11} &CircleTimes; &CircleTimes; q q = = [\begin{matrix} δe δe & δq δq \end{matrix}] - - - - - - ((66))

Taking the vector part δe of the error quaternion and the gyro drift error △b as the state variables, the state variable of the system is △x=[δe △b], and the satellite attitude kinematic equation is:

\overset{\cdot &Center Dot;}{q q} = = \frac{11}{22} q q &CircleTimes; &CircleTimes; ω ω - - - - - - ((77))

\overset{\overset{\cdot &Center Dot;}{^^}}{q q} = = \frac{11}{22} \overset{^^}{q q} &CircleTimes; &CircleTimes; \overset{^^}{ω ω} - - - - - - ((88))

In the formula, is the calculated value of satellite angular velocity;

Deriving formula (6), and substituting formula (7) and formula (8) into formula (6), we get:

δ δ \overset{\cdot \cdot}{q q} = = \frac{11}{22} [[δq δq &CircleTimes; &CircleTimes; ω ω - - \overset{^^}{ω ω} &CircleTimes; &CircleTimes; δq δq]] - - - - - - ((99))

definition:

δω δω = = [[ω ω - - \overset{^^}{ω ω}]] = = [[((b b - - \overset{^^}{b b})) + + {η η}_{u u}]] - - - - - - ((1010))

Substitute formula (10) into formula (9):

δ δ \overset{\cdot &Center Dot;}{q q} = = \frac{11}{22} [[δq δq &CircleTimes; &CircleTimes; \overset{^^}{ω ω} - - \overset{^^}{ω ω} &CircleTimes; &CircleTimes; δq δq]] + + \frac{11}{22} δq δq &CircleTimes; &CircleTimes; δω δω - - - - - - ((1111))

Linearize formula (11), we have:

δ δ \overset{\cdot \cdot}{q q} = = - - \overset{^^}{ω ω} \times \times δe δ e - - \frac{11}{22} ((Δb Δb + + {η η}_{u u})) - - - - - - ((1212))

Then the system state equation of the error state variable can be obtained as:

Δ Δ \overset{\cdot \cdot}{X x} ((t t)) = = F f ((t t)) ΔX ΔX ((t t)) + + G G ((t t)) ϵ ϵ ((t t)) - - - - - - ((1313))

In the formula,

F f ((t t)) = = [\begin{matrix} - - [[ω ω ((t t)) \times \times]] & \frac{11}{22} {I I}_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} \end{matrix}],, G G ((t t)) = = [\begin{matrix} - - \frac{11}{22} {I I}_{33 \times \times 33} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & {I I}_{33 \times \times 33} \end{matrix}],, ϵ ϵ ((t t)) = = \begin{matrix} [\begin{matrix} {η η}_{u u} \\ 00 \end{matrix}] \end{matrix} . .

5. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 4, it is characterized in that the concrete process of setting up the error system observation equation described in step 4 is: due to adopting the quaternion The number q is taken as the output quantity, the quaternion error obtained by the star sensor is taken as the measurement output, and its vector part is taken as the observation quantity, and there is an observation equation:

z=Hx+v (14)

In the formula, H is the observation matrix, and v is the observation noise;

H h = = [\begin{matrix} 11 & 00 & 00 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 & 00 & 00 \\ 00 & 00 & 11 & 00 & 00 & 00 \end{matrix}]

The fourth-order Runge-Kutta method is used to discretize the attitude error system equation composed of equations (13) and (14).

6. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 5, it is characterized in that the self-adaptive square root UKF estimation error quaternion and gyro drift error described in step 5 are utilized The specific process is:

(1) Initialization state The square root S ₀ of the state error covariance matrix is

{\overset{^^}{X x}}_{00} = = E E. [[{X x}_{00}]] - - - - - - ((1515))

{S S}_{00} = = chol chol ((E E. [[(({X x}_{00} - - {\overset{^^}{X x}}_{00} {(({X x}_{00} - - {\overset{^^}{X x}}_{00}))}^{T T}))]])) - - - - - - ((1616))

(2) For k=1,2,3,...,n, the implementation steps are as follows:

① Calculate the sigma point

{ξ ξ}_{k k} = = [\begin{matrix} {\overset{^^}{X x}}_{k k} & {\overset{^^}{X x}}_{k k} + + γ γ {S S}_{k k} & {\overset{^^}{X x}}_{k k} - - γ γ {S S}_{k k} \end{matrix}] - - - - - - ((1717))

In the formula, λ=α ² (n+κ)-n, the value of α is between 0.001 and 1, n is the dimension of the system state vector, κ is the third scale factor, and the value is 0;

②Time update

Nonlinear propagation of Sigma sampling points:

ξ _i,k+1|k = F _k+1|k ξ _i,k (18)

Computation of one-step predictors of state values:

{\overset{^^}{X x}}_{k k + + 11 | | k k} = = {Σ Σ}_{i i = = 00}^{22 n no} {W W}_{i i}^{m m} {ξ ξ}_{k k + + 11 | | k k} - - - - - - ((1919))

One-step forecast calculation for observations:

{\overset{^^}{Z Z}}_{k k + + 11 | | k k} = = H h {\overset{^^}{X x}}_{k k + + 11 | | k k} - - - - - - ((2020))

The residual calculation of the predicted value and the actual value:

{e e}_{k k + + 11} = = {Z Z}_{k k + + 11} - - {\overset{^^}{Z Z}}_{k k + + 11 | | k k} - - - - - - ((21 twenty one))

③ Calculate the adaptive fading factor μ _k+1

{μ μ}_{k k + + 11,, i i} = = \{\begin{matrix} {μ μ}_{k k + + 11,, i i},, {μ μ}_{k k + + 11,, i i} &GreaterEqual; &Greater Equal; 11 \\ 11,, {μ μ}_{k k + + 11,, i i} < < 11 \end{matrix},, i i = = 1,2 1,2,, . . . . . .,, n no - - - - - - ((22 twenty two))

μ _k+1,i =trace(N _k+1 )/trace(M _ii,k+! ) (23)

N _k+1 = V _k+1 -HQ _k+1 H ^T -lR _k+1 (24)

M _k+1 = J _k+1 HH ^T (25)

{J J}_{k k + + 11} = = {Σ Σ}_{i i = = 11}^{22 n no} {W W}_{i i}^{c c} (({ξ ξ}_{k k + + 11 | | k k,, i i} - - {ξ ξ}_{00,, k k + + 11 | | k k})) {(({ξ ξ}_{k k + + 11 | | k k,, i i} - - {ξ ξ}_{00,, k k + + 11 | | k k}))}^{T T} - - - - - - ((2626))

{V V}_{k k + + 11} = = \{\begin{matrix} {e e}_{k k + + 11} {e e}_{k k + + 11}^{T T},, k k = = 00 \\ \frac{ρ ρ {V V}_{k k} + + {e e}_{k k + + 11} {e e}_{k k + + 11}^{T T}}{11 + + ρ ρ},, k k &GreaterEqual; &Greater Equal; 11 \end{matrix} - - - - - - ((2727))

④Measurement update

Computation of the square root of the state one-step forecast covariance matrix:

{S S}_{k k + + 11 | | k k} = = qr qr {{[[diag diag (({μ μ}_{k k + + 11})) \sqrt{{W W}_{11}^{c c}} (({ξ ξ}_{11 : : 22 n no,, k k + + 11 | | k k} - - {ξ ξ}_{00,, k k + + 11 | | k k})),, {G G}_{k k} \sqrt{{Q Q}_{k k}}]]}} - - - - - - ((2828))

Cross-covariance matrix calculation:

{P P}_{XZ XZ,, k k + + 11} = = (({S S}_{k k + + 11 | | k k} {S S}_{k k + + 11 | | k k}^{T T})) {H h}^{T T} - - - - - - ((2929))

Calculate the square root of the output covariance matrix:

U _k+1 ＝HS _k+1|k (30)

{S S}_{Z Z,, k k + + 11} = = qr qr {{[[{U u}_{k k + + 11},, \sqrt{{R R}_{k k + + 11}}]]}} - - - - - - ((3131))

State gain matrix calculation:

{K K}_{k k + + 11} = = (({P P}_{XZ XZ,, k k + + 11} / / {S S}_{Z Z,, k k + + 11}^{T T})) / / {S S}_{Z Z,, k k + + 11} - - - - - - ((3232))

State estimate calculation:

{\overset{^^}{X x}}_{k k + + 11 | | k k + + 11} = = {\overset{^^}{X x}}_{k k + + 11 | | k k} + + {K K}_{k k + + 11} {e e}_{k k + + 11} - - - - - - ((3333))

The square root calculation of the state error covariance matrix:

{S S}_{k k + + 11} = = {S S}_{k k + + 11 | | k k} - - {S S}_{k k + + 11 | | k k} {U u}_{k k + + 11}^{T T} {S S}_{z z,, k k + + 11}^{- - T T} {(({S S}_{z z,, k k + + 11} + + \sqrt{{R R}_{k k + + 11}}))}^{- - 11} {U u}_{k k + + 11} - - - - - - ((3434))

The parameters used in the above process are calculated as follows:

{W W}_{00}^{m m} = = \frac{λ λ}{n no + + λ λ} - - - - - - ((3535))

{W W}_{00}^{c c} = = {W W}_{00}^{m m} + + ((11 - - {α α}^{22} + + β β)) - - - - - - ((3636))

{W W}_{i i}^{c c} = = {W W}_{i i}^{m m} = = \frac{11}{22 ((n no + + λ λ))} - - - - - - ((3737)) . .

7. a kind of satellite attitude determination method based on the improved self-adaptive square root UKF algorithm according to claim 6, it is characterized in that the attitude solution method described in step 8 is:

θ=arcsin(-2(q ₁ q ₃ -q ₂ q ₄ )) (39)

ψ ψ = = arctan arctan ((22 (({q q}_{22} {q q}_{11} + + {q q}_{33} {q q}_{44})) / / ((11 - - 22 (({q q}_{33}^{22} + + {q q}_{22}^{22})))) - - - - - - ((4040))

In the formula, θ, ψ represent roll angle, pitch angle and yaw angle respectively.