CN112212860B - Distributed filtering micro-nano satellite attitude determination method with fault tolerance - Google Patents

Distributed filtering micro-nano satellite attitude determination method with fault tolerance Download PDF

Info

Publication number
CN112212860B
CN112212860B CN202010882166.6A CN202010882166A CN112212860B CN 112212860 B CN112212860 B CN 112212860B CN 202010882166 A CN202010882166 A CN 202010882166A CN 112212860 B CN112212860 B CN 112212860B
Authority
CN
China
Prior art keywords
sensor
fault
attitude
coordinate system
local filter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010882166.6A
Other languages
Chinese (zh)
Other versions
CN112212860A (en
Inventor
李乐宝
王菲
张众正
刘中伟
李明翔
王磊
姜连祥
占丰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong Institute of Space Electronic Technology
Original Assignee
Shandong Institute of Space Electronic Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong Institute of Space Electronic Technology filed Critical Shandong Institute of Space Electronic Technology
Priority to CN202010882166.6A priority Critical patent/CN112212860B/en
Publication of CN112212860A publication Critical patent/CN112212860A/en
Application granted granted Critical
Publication of CN112212860B publication Critical patent/CN112212860B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Astronomy & Astrophysics (AREA)
  • Navigation (AREA)

Abstract

According to the distributed filtering micro-nano satellite attitude determination method with fault tolerance, a fault detection and processing link is added based on the traditional Federal Kalman filtering algorithm, and the integral attitude determination method is enabled to have fault tolerance performance by adding fault detection factors and deducing given fault thresholds, so that the sensor fault type is effectively discriminated, and the attitude determination precision of the system is improved. Measurement residual using attitude sensor
Figure DDA0004039713610000011
Setting a fault detection factor
Figure DDA0004039713610000012
Fault detection factor
Figure DDA0004039713610000013
The following formula is satisfied:
Figure DDA0004039713610000014
wherein trace (·) represents the trace operation of matrix calculation; l is the dimension of the measuring vector of the sensor in the local filter;
Figure DDA0004039713610000015
an observed noise variance matrix representing a local filter; (. Cndot.) T Represents a transpose of a matrix; fault threshold gamma 0 Take a value of
Figure DDA0004039713610000016

Description

Distributed filtering micro-nano satellite attitude determination method with fault tolerance
Technical Field
The invention relates to a distributed filtering micro-nano satellite attitude determination method with fault tolerance, and belongs to the technical field of satellite attitude determination and control.
Background
With the rapid development of the domestic microelectronic technology and chip process design technology, the satellite attitude determination and control level is increasingly improved. The attitude determination and control technology is a basic guarantee for normal in-orbit operation of a satellite platform, and is used for dynamically adjusting and controlling the in-orbit attitude of a satellite according to coordinate information of the satellite relative to a certain reference coordinate system (such as an orbit coordinate system or an inertia coordinate system) so as to improve data transmission precision and efficiency. The attitude determination and control system is generally composed of a brain on the satellite (an on-board computer), an attitude sensor and an attitude determination algorithm, wherein the accuracy of the result of the attitude determination algorithm directly influences the accuracy of the attitude control and adjustment of the satellite, so that the improvement of the accuracy of the satellite attitude determination algorithm is always the core topic in the field of attitude determination and control.
Due to the limitations of mass, volume and power consumption, micro-nano satellites commonly adopt miniaturized attitude sensors, such as MEMS gyroscopes, magnetometers, micro sun sensors and the like. Although the miniaturized attitude sensor is light and small and has low power consumption, the single sensor has the problems of incapability of determining the attitude, low accuracy of acquiring attitude information and the like. The conventional general improvement mode is to combine a micro attitude sensor with a multi-source information fusion algorithm to hopefully acquire attitude information with higher precision. However, a multi-source information fusion algorithm such as the Federal Kalman filtering only provides a distributed information fusion structure, and a detection and processing link of sensor faults is not considered, so that a corresponding fault-tolerant link is lacked. On the premise that the accuracy of the attitude determination of the sensor and the accuracy of the acquired attitude information are not high, targeted fault detection and isolation cannot be realized, and sufficient compensation for satellite attitude adjustment cannot be provided through fault analysis, so that the conventional attitude determination and control system needs to be further improved.
In view of this, the present patent application is specifically proposed.
Disclosure of Invention
The method for determining the attitude of the distributed filtering micro-nano satellite with fault tolerance aims to solve the problems in the prior art, and a fault detection and processing link is added based on the traditional Federal Kalman filtering algorithm, namely a fault detection factor and a fault threshold value given by derivation are added, so that the integral attitude determination method has fault tolerance, the sensor fault type is effectively discriminated, and the attitude determination precision of the system is improved.
In order to achieve the above design objective, the present application proposes the following solutions:
a distributed filtering micro-nano satellite attitude determination method with fault tolerance is based on a distributed Federal Kalman filtering algorithm and utilizes a measurement value residual error of an attitude sensor
Figure GDA0004039713600000021
Setting a fault detection factor
Figure GDA0004039713600000022
Fault detection factor
Figure GDA0004039713600000023
The following formula is satisfied:
Figure GDA0004039713600000024
wherein: trace (·) represents the trace operation of the matrix; l is the dimension of the measuring vector of the sensor in the local filter;
Figure GDA0004039713600000025
an observed noise variance matrix representing a local filter; (.) T Represents a transpose of a matrix;
fault threshold gamma 0 Take a value of
Figure GDA0004039713600000026
The following fault threshold value gamma is proposed 0 And the calculation process comprises the following steps:
assuming that the dimension of the attitude sensor measurement vector of a certain local filter Si is l,
order to
Figure GDA0004039713600000027
Then
Figure GDA0004039713600000028
There is a solution procedure as follows,
Figure GDA0004039713600000029
wherein the content of the first and second substances,
Figure GDA00040397136000000210
as vectors
Figure GDA00040397136000000211
Element (iii) σ (i) The standard deviation of the measurement noise of a sensor in a local filter Si is obtained;
due to the order of l matrix
Figure GDA00040397136000000212
The sum of each element on the main diagonal is the trace of the matrix, then have
Figure GDA00040397136000000213
Make the single-dimensional measurement residual error of local filter Si
Figure GDA00040397136000000214
Then
Figure GDA0004039713600000031
Wherein the content of the first and second substances,
Figure GDA0004039713600000032
measuring residual error delta in one dimension when the sensor is normal (i) D is the one-dimensional measurement residual error delta when the sensor fails (i) The information of (a);
further, the fault detection factor is derived from the above equations
Figure GDA0004039713600000033
Standard deviation sigma of sensor measuring noise in local filter Si (i) Making a close correlation;
as described above, when the attitude sensor is not faulty, the measurement residual
Figure GDA0004039713600000034
Similar to the sensor measurement noise, the sensor measurement noise satisfies the characteristics of zero-mean Gaussian white noise, and generally, the upper bound of the attitude sensor measurement noise is taken as 3 sigma (i) Therefore, the residual error of one-dimensional measurement value when the attitude sensor in the local filter Si is not in fault
Figure GDA0004039713600000035
The upper limit of (2) may be 3 σ (i) I.e. by
Figure GDA0004039713600000036
When the attitude sensor has no fault, | d | =0,
Figure GDA0004039713600000037
so the failure threshold gamma 0 Is taken as
Figure GDA0004039713600000038
Further, the method for determining the attitude of the distributed filtering micro-nano satellite with fault tolerance comprises the following steps:
1) Acquiring the angular velocity omega of the micro-nano satellite body coordinate system b relative to the inertial coordinate system i by using the measurement value of the MEMS gyroscope m For estimation of angular velocity;
2) The mounting shaft of the magnetometer is consistent with the coordinate system of the micro-nano satellite body, and the geomagnetic field vector measurement value B under the coordinate system of the satellite body is obtained b ；
3) The aiming axis of the sun sensor is consistent with the coordinate system-Y axis of the micro-nano satellite body to obtain the sun vector measurement value S under the satellite body coordinate system b ；
4) Selecting error quaternion Δ q bo The vector component Δ q and the gyro angular rate drift estimation error Δ b of (1) are state quantities
Figure GDA0004039713600000039
Giving a state equation of a satellite attitude determination system;
5) Utilizing the geomagnetic field vector measurement value B under the satellite body coordinate system obtained in the step 2) b And the sun vector measurement value S under the satellite body coordinate system obtained in the step 3) b Combined with the reference value B of the earth magnetic field vector in the orbital coordinate system o And the sun vector reference value S under the orbital coordinate system o Providing observation information of two local filters in a distributed Federal Kalman filtering algorithm;
6) And calculating to obtain the attitude of the micro-nano satellite by utilizing a distributed Federal Kalman filtering algorithm, and realizing satellite attitude calculation based on a plurality of low-cost, small-volume and low-power consumption micro sensors.
Further, the fault detection and processing and state fusion process is implemented according to the following steps:
(1) Fault detection and handling
Setting a fault detection factor
Figure GDA00040397136000000310
Fault threshold gamma of 0 Namely:
Figure GDA00040397136000000311
when the sensor is normal;
Figure GDA00040397136000000312
when the sensor fails;
based on the fault detection factor and the fault threshold, when the sensor is detected to have a fault, if the local filter S1 fails, the local filter S1 is used for judging whether the sensor fails or not
Figure GDA0004039713600000041
Is incorrect and thus not input to the main filter, at which point the overall estimate of the error state of the system may be modified to
Figure GDA0004039713600000042
Similarly, if the local filter S2 fails, the overall estimation of the system error state is
Figure GDA0004039713600000043
When the sensor is not detected to have faults, directly executing the following step (2);
(2) State fusion
State estimation for local filters S1 and S2
Figure GDA0004039713600000044
Sum estimation error covariance matrix
Figure GDA0004039713600000045
Performing data fusion to obtain final state estimation
Figure GDA0004039713600000046
Sum estimation error covariance matrix P g,k ；
Figure GDA0004039713600000047
Figure GDA0004039713600000048
In conclusion, the distributed filtering micro-nano satellite attitude determination method with fault tolerance has the following advantages:
1. through the fault tolerance link provided by the application, the precision of the sensor for acquiring the attitude information of the micro-nano satellite can be improved, and the integral fault tolerance performance of the attitude determination system can be improved by detecting and processing the fault of the sensor.
2. A fault detection factor is innovatively provided, a value basis and a strict mathematical analysis process are provided for a fault threshold, and a large amount of data debugging work in engineering application is remarkably reduced, so that the efficient design of the fault-tolerant attitude determination method is realized.
3. When the satellite is at the end of the service life, if the attitude sensor fails (such as failure of a magnetometer), the existing attitude determination system can still normally acquire and adjust attitude information based on the application, and accordingly the service life of the satellite is prolonged.
Drawings
The following drawings are illustrative of specific embodiments of the present application.
FIG. 1 is a schematic diagram of a prior art multi-source information distributed fusion pose determination method;
FIG. 2 is a schematic diagram of a distributed Federal Kalman filtering algorithm based on fault tolerance described in the present application;
FIG. 3 is a flow chart of a fault detection and handling link according to the present application;
FIG. 4 is a waveform of satellite attitude information determined based on the fault tolerant distributed filtering algorithm described herein when the local filter sensor is fault-free;
FIG. 5 is a waveform of satellite angular velocity information determined based on the fault tolerant distributed filtering algorithm described herein when the local filter sensor is fault-free;
FIG. 6 is a waveform diagram of satellite roll angle information determined based on the fault tolerant distributed filtering algorithm described herein when a local filter magnetometer or sun sensor fails;
FIG. 7 is a waveform diagram of satellite pitch angle information determined based on the fault-tolerant distributed filtering algorithm described in the present application when a local filter magnetometer or sun sensor fails;
fig. 8 is a waveform diagram of satellite yaw angle information determined based on the fault-tolerant distributed filtering algorithm described in the present application when a local filter magnetometer or a sun sensor fails.
Detailed Description
The following further describes embodiments of the present invention with reference to the drawings.
Embodiment 1, the application provides a novel method for determining a micro/nano satellite attitude with a fault tolerance link on the basis of the existing distributed federal kalman filtering algorithm.
As shown in fig. 1 and 2, the existing and improved distributed fusion micro-nano satellite attitude determination methods are based on the existing multi-source information distributed fusion structure, an MEMS gyroscope, a magnetometer and a sun sensor are used as attitude sensors, and the distributed federal kalman filtering algorithm is composed of two local filters and a main filter.
The local filter S1 and the local filter S2 respectively complete the measurement updating of the estimated error covariance and the state of the magnetometer and the sun sensor according to the feedback attitude and the error covariance of the adaptive distribution; the state variables of the two local filters are the same, and the two local filters perform parallel operation.
The main filter realizes the prediction of attitude quaternion, the one-step prediction of state and covariance and information distribution, detects sensor faults according to fault detection factors or performs state fusion according to output information of a local filter, and corrects and finally outputs attitude and gyro drift.
As shown in fig. 2, the method for determining the attitude of the distributed filtering micro/nano satellite with fault tolerance according to the present application can complete satellite attitude determination and detect and process sensor faults at the same time, and includes the following implementation steps:
1) And the measured value of the MEMS gyroscope is utilized,obtaining the angular velocity omega of the coordinate system b of the micro-nano satellite body relative to the inertial coordinate system i m For estimation of angular velocity;
2) When the mounting shaft of the magnetometer is consistent with the coordinate system of the micro-nano satellite body, the geomagnetic field vector measurement value B under the coordinate system of the satellite body is obtained b ；
3) The aiming axis of the sun sensor is consistent with the coordinate system-Y axis of the micro/nano satellite body to obtain the sun vector measurement value S under the satellite body coordinate system b ；
4) Selection error quaternion Δ q bo The vector component Δ q and the gyro angular rate drift estimation error Δ b of (1) are state quantities
Figure GDA0004039713600000051
The discrete state equation for the satellite attitude determination system is given as:
X k ＝Φ k,k-1 X k-1 +Γ k,k-1 W k-1
wherein the content of the first and second substances,
Figure GDA0004039713600000061
t is the calculation step size and is the calculation step size,
Figure GDA0004039713600000062
ω bi is the angular velocity vector, I, of the satellite body coordinate system relative to the Earth's center inertial coordinate system 3×3 The random walk noise is a unit matrix of 3 x 3, upsilon is the gyro angle random walk noise, ζ is the gyro angular velocity random walk noise, k represents the kth time, and k-1 represents the kth time.
5) Utilizing the geomagnetic field vector measurement value B under the satellite body coordinate system obtained in the step 2) b And the sun vector measurement value S under the satellite body coordinate system obtained in the step 3) b Combined with the reference value B of the earth magnetic field vector in the orbital coordinate system o And the sun vector reference value S under the orbital coordinate system o And providing observation information of two local filters in the distributed Federal Kalman filtering algorithm:
the observation information of the local filter S1 is
Z 1k ＝(B b(k) -B b(k/k-1) )/2
Wherein, B b(k) The measured value of the geomagnetic field vector under a satellite body coordinate system at the moment k is obtained; b is b(k/k-1) ＝T bo (q bo(k/k-1) )B o(k) The earth magnetic field vector under the body coordinate system is calculated according to the attitude estimation value; b is o(k) A ground magnetic field vector of a satellite orbit coordinate system at the moment k; q. q.s bo(k/k-1) Is the predicted value of the attitude quaternion from the moment k-1 to the moment k.
The local filter S2 observes the information as
Z 2k ＝(S b(k) -S b(k/k-1) )/2
Wherein S is b(k) The measured value of the sun vector is the measured value of the satellite body coordinate system at the moment k; s b(k/k-1) ＝T bo (q bo(k/k-1) )S o(k) The sun vector under the body coordinate system is calculated according to the attitude estimation value; s o(k) The solar vector is the solar vector under the satellite orbit coordinate system at the moment k.
6) And calculating to obtain the attitude of the micro-nano satellite by utilizing a distributed Federal Kalman filtering algorithm, and realizing satellite attitude calculation based on a plurality of low-cost, small-volume and low-power consumption micro sensors.
As shown in fig. 3, the present application provides an improvement of a distributed fusion micro-nano satellite attitude determination method, namely, a fault detection and processing link is added. The method for determining the attitude of the distributed Federal Kalman filtering micro-nano satellite with fault tolerance is implemented according to the following steps:
(1) Prediction of attitude quaternion
Predicting attitude quaternion by using RungeKutta equation according to attitude kinematics equation and estimated angular velocity
Figure GDA0004039713600000071
Quaternion estimation from attitude at time k-1
Figure GDA0004039713600000072
And estimating angular velocity
Figure GDA0004039713600000073
Predicting the angular speed of the body coordinate system relative to the orbit coordinate system at the k time as follows:
Figure GDA0004039713600000074
according to the kinematic equation of satellite attitude, adopting RungeKutta equation to predict attitude quaternion at k moment
Figure GDA0004039713600000075
Is composed of
Figure GDA0004039713600000076
Wherein k is 1 、k 2 、k 3 And k 4 Is an intermediate variable of the Runge-Kutta method.
(2) One step prediction of state and covariance
According to the discrete state information, the state estimation value is estimated from the k-1 time
Figure GDA0004039713600000077
Computing one-step state prediction at time k
Figure GDA0004039713600000078
Sum estimation error covariance matrix P g,k/k-1 ；
Figure GDA0004039713600000079
Figure GDA00040397136000000710
Wherein, P g,k-1 An estimation error covariance matrix at the time of k-1; q is the process noise variance matrix of the main filter.
(3) Information distribution
Information distribution factor alpha of main filter i And resetting the estimation error covariance matrix to the local filter S1 and the local filter S2
Figure GDA00040397136000000711
Calculating an information distribution factor alpha i The following were used:
Figure GDA00040397136000000712
wherein the content of the first and second substances,
Figure GDA00040397136000000713
Figure GDA00040397136000000714
is the error covariance matrix of the local filter i,
Figure GDA00040397136000000715
is a matrix
Figure GDA00040397136000000716
The trace of (c). Resetting the estimation error covariance matrix to the local filter S1 and the local filter S2
Figure GDA00040397136000000717
Therefore, the temperature of the molten metal is controlled,
Figure GDA00040397136000000718
(4) And updating the measurement
Each local filter carries out measurement updating according to the observation information of the magnetometer and the sun sensor respectively, and local filter Kalman gain at the moment k is calculated
Figure GDA00040397136000000719
Local filter state estimation
Figure GDA00040397136000000720
And an error covariance matrix
Figure GDA00040397136000000721
Figure GDA00040397136000000722
Wherein the content of the first and second substances,
Figure GDA0004039713600000081
an observation matrix representing the local filter at time k;
Figure GDA0004039713600000082
and Z ik Respectively representing an observation noise variance matrix and observation information of a local filter;
Figure GDA0004039713600000083
(5) Fault detection and handling and state fusion
And detecting the sensor fault according to the fault detection factor or carrying out state fusion according to the output information of the local filter.
(5.1) Fault detection and handling
Based on distributed Federal Kalman filtering algorithm, utilizing measurement value residual error of attitude sensor
Figure GDA0004039713600000084
Setting a fault detection factor
Figure GDA0004039713600000085
To detect attitude to determine if there is an abrupt fault or a slowly accumulating fault in the system.
Fault detection factor
Figure GDA0004039713600000086
The following formula is satisfied:
Figure GDA0004039713600000087
wherein: trace (·) represents a trace operation of the matrix; l is the dimension of the measuring vector of the sensor in the local filter;
Figure GDA0004039713600000088
an observed noise variance matrix representing a local filter; (.) T Representing the transpose of the matrix.
In conjunction with FIG. 2, then
Figure GDA0004039713600000089
And
Figure GDA00040397136000000818
the fault detection factors of the local filter S1 and the local filter S2.
When the attitude sensor has no fault, the corresponding measurement value residual error
Figure GDA00040397136000000810
Should always be small, theoretically satisfying the characteristics of zero-mean white gaussian noise. When the attitude sensor has sudden change fault or slowly changing fault accumulated to a certain degree, the residual error of the measured value
Figure GDA00040397136000000811
Will become significantly larger, applying the above described fault detection factor
Figure GDA00040397136000000812
The calculation result can directly judge whether the sensor in the attitude determination system has a fault.
Therefore, the following failure detection factor needs to be set
Figure GDA00040397136000000813
Fault threshold gamma of 0 Namely:
Figure GDA00040397136000000814
when the sensor is normal;
Figure GDA00040397136000000815
the sensor fails.
For fault threshold gamma 0 When γ is analyzed 0 When a smaller value is set, fault detection is easier to be carried out on the attitude determination system, but error judgment is also easier to be caused; when gamma is 0 With a larger value set, it is relatively difficult to detect a fault.
The present application proposes the following fault threshold γ 0 And the calculation process comprises the following steps:
assuming that the dimension of the attitude sensor measurement vector of a certain local filter Si is l,
order to
Figure GDA00040397136000000816
Then
Figure GDA00040397136000000817
There is a solution procedure as follows,
Figure GDA0004039713600000091
wherein the content of the first and second substances,
Figure GDA0004039713600000092
as vectors
Figure GDA0004039713600000093
Element (iii) σ (i) The standard deviation of the measurement noise of a sensor in a local filter Si is obtained;
due to the order of l matrix
Figure GDA0004039713600000094
The sum of each element on the main diagonal is the trace of the matrix, then have
Figure GDA0004039713600000095
Figure GDA0004039713600000096
Make the single-dimensional measurement residual error of local filter Si
Figure GDA0004039713600000097
Then
Figure GDA0004039713600000098
Wherein the content of the first and second substances,
Figure GDA0004039713600000099
measuring residual error delta in one dimension when the sensor is normal (i) D is the one-dimensional measurement residual error delta when the sensor fails (i) The information of (a);
further, the fault detection factor is derived from the above equations
Figure GDA00040397136000000910
Standard deviation sigma of sensor measuring noise in local filter Si (i) Making a close correlation;
as described above, when the attitude sensor is not faulty, the measurement residual
Figure GDA00040397136000000911
Similar to the sensor measurement noise, the sensor measurement noise satisfies the characteristics of zero-mean Gaussian white noise, and generally, the upper bound of the attitude sensor measurement noise is taken as 3 sigma (i) Therefore, the residual error of one-dimensional measurement value when the attitude sensor in the local filter Si is not in fault
Figure GDA00040397136000000912
The upper limit of (3 a) may be taken to be 3 a (i) I.e. by
Figure GDA00040397136000000913
When the attitude sensor has no fault, | d | =0,
Figure GDA00040397136000000914
so the failure threshold gamma 0 Is taken as
Figure GDA00040397136000000915
Based on the fault detection factor and the fault threshold, when the sensor is detected to be in fault, if the local filter S1 is invalid, the sensor is detected to be in fault
Figure GDA0004039713600000101
Is incorrect and thus not input to the main filter, at which point the overall estimate of the system error state may be modified to
Figure GDA0004039713600000102
Similarly, if local filter S2 fails, the overall estimate of the system error state is
Figure GDA0004039713600000103
When the sensor is not detected to be in fault, the following step (5.2) is directly executed.
(5.2) State fusion
State estimation for local filters S1 and S2
Figure GDA0004039713600000104
Sum estimation error covariance matrix
Figure GDA0004039713600000105
Performing data fusion to obtain final state estimation
Figure GDA0004039713600000106
Sum estimation error covariance matrix P g,k ；
Figure GDA0004039713600000107
Figure GDA0004039713600000108
(6) Attitude and gyro drift correction
Using state estimation
Figure GDA0004039713600000109
Modifying estimated attitude quaternion
Figure GDA00040397136000001010
And random drift estimate
Figure GDA00040397136000001011
And combined with the measured value omega of the MEMS gyroscope m Obtaining the final estimation attitude quaternion
Figure GDA00040397136000001012
And estimating angular velocity
Figure GDA00040397136000001013
Figure GDA00040397136000001014
Figure GDA00040397136000001015
Figure GDA00040397136000001016
Figure GDA00040397136000001017
The following simulation experiment results are given in fig. 4 to 8 to verify the accuracy and validity of the fault detection and state fusion.
Taking a micro-nano satellite as an example, the micro-nano satellite runs on the sun synchronization with the orbit height of 520kmOn the orbit, the inertia moment of the satellite is diag [0.08845 0.1422.07518 ]]kg·m 2 The descending intersection point is 7. When the satellite is stable on the three earth axes, the expected attitude is [0,0,0]Angle random walk sigma of MEMS gyroscope υ ＝0.03°/s 1/2 Angular velocity random walk σ ζ ＝0.0001°/s 3/2 The measuring noise of the magnetometer is 100nT, and the measuring noise of the sun sensor is 0.1 degree. The distributed filtering micro-nano satellite attitude determination method for fault tolerance is utilized to carry out simulation experiments. When the local filter sensor has no fault, the attitude information of the micro/nano satellite determined based on the improved distributed filtering algorithm is shown in fig. 4; when the local filter sensor has no fault, the angular velocity information of the micro-nano satellite determined based on the improved distributed filtering algorithm provided by the application is shown in fig. 5. When the magnetometer of the local filter fails or the sun sensor fails, the roll angle, the pitch angle and the yaw angle information of the micro-nano satellite determined by the distributed filtering algorithm with fault tolerance based on the application are respectively shown in fig. 6, 7 and 8.
As can be seen from fig. 4 to 5, when the local filter sensor has no fault, the attitude angle and the angular velocity of the micro-nano satellite estimated based on the distributed filtering algorithm of the present application have higher estimation accuracy. As can be seen from fig. 6 to 8, when the sun sensor of the local filter fails, the attitude angle of the micro/nano satellite determined based on the fault-tolerant distributed filtering algorithm of the present application is within a range of ± 0.5 °; when the local filter magnetometer has a fault, the rolling angle and the yaw angle of the micro-nano satellite determined based on the fault-tolerant distributed filtering algorithm are within the range of +/-0.4 degrees, and the estimated pitch angle is within the range of +/-1.25 degrees. According to comparison of all simulation results, the distributed filtering micro-nano satellite attitude determination method with fault tolerance has high practicability and good fault tolerance.
In summary, the embodiments presented in connection with the figures are only preferred solutions. Those skilled in the art can derive other alternative structures according to the design concept of the present invention, and the alternative structures should also fall within the scope of the solution of the present invention.

Claims (3)

1. A distributed filtering micro-nano satellite attitude determination method with fault tolerance is characterized by comprising the following steps: based on distributed Federal Kalman filtering algorithm, utilizing measurement value residual error of attitude sensor
Figure FDA0004039713590000011
Setting a fault detection factor
Figure FDA0004039713590000012
Fault detection factor
Figure FDA0004039713590000013
The following formula is satisfied:
Figure FDA0004039713590000014
wherein: trace (·) represents the trace operation of the matrix; l is the dimension of the measuring vector of the sensor in the local filter;
Figure FDA0004039713590000015
an observed noise variance matrix representing a local filter; (.) T Represents a transpose of a matrix;
fault threshold gamma 0 Take a value of
Figure FDA0004039713590000016
The following fault threshold value gamma is proposed 0 And the calculation process comprises the following steps:
assuming that the dimension of the attitude sensor measurement vector of a certain local filter Si is l,
order to
Figure FDA0004039713590000017
Then
Figure FDA0004039713590000018
There is a solution process as follows for the solution,
Figure FDA0004039713590000019
wherein the content of the first and second substances,
Figure FDA00040397135900000110
as vectors
Figure FDA00040397135900000111
Element (iii) σ (i) The standard deviation of the measurement noise of a sensor in a local filter Si is obtained;
due to the order of l matrix
Figure FDA00040397135900000112
The sum of each element on the main diagonal is the trace of the matrix, then have
Figure FDA00040397135900000113
Figure FDA00040397135900000114
Make the single-dimensional measurement residual error of local filter Si
Figure FDA0004039713590000021
Then
Figure FDA0004039713590000022
Wherein the content of the first and second substances,
Figure FDA0004039713590000023
measuring residual error delta in one dimension when the sensor is normal (i) D is the one-dimensional measurement residual error delta when the sensor fails (i) The information of (a);
further, the fault detection factor is derived from the above equations
Figure FDA0004039713590000024
Standard deviation sigma of sensor measuring noise in local filter Si (i) Making a close correlation;
as described above, when the attitude sensor is not faulty, the measurement residual
Figure FDA0004039713590000025
Similar to the sensor measurement noise, the sensor measurement noise satisfies the characteristics of zero-mean Gaussian white noise, and generally, the upper bound of the attitude sensor measurement noise is taken as 3 sigma (i) Therefore, the residual error of one-dimensional measurement value when the attitude sensor in the local filter Si is not in fault
Figure FDA0004039713590000026
The upper limit of (3 a) may be taken to be 3 a (i) I.e. by
Figure FDA00040397135900000210
When the attitude sensor has no fault, | d | =0,
Figure FDA0004039713590000027
so the failure threshold gamma 0 Is taken as
Figure FDA0004039713590000028
2. The method for determining the attitude of the distributed filtering micro-nano satellite with fault tolerance according to claim 1, wherein the method comprises the following steps: comprises the following implementation steps of the following steps of,
1) Acquiring the angular velocity omega of the micro-nano satellite body coordinate system b relative to the inertial coordinate system i by using the measurement value of the MEMS gyroscope m By usingAn estimate of angular velocity;
2) The mounting shaft of the magnetometer is consistent with the coordinate system of the micro-nano satellite body, and the geomagnetic field vector measurement value B under the coordinate system of the satellite body is obtained b ；
3) The aiming axis of the sun sensor is consistent with the coordinate system-Y axis of the micro/nano satellite body to obtain the sun vector measurement value S under the satellite body coordinate system b ；
4) Selecting an error quaternion Δ q bo The vector component Δ q and the gyro angular rate drift estimation error Δ b of (1) are state quantities
Figure FDA0004039713590000029
Giving a state equation of a satellite attitude determination system;
5) Utilizing the geomagnetic field vector measurement value B in the satellite body coordinate system obtained in the step 2) b And the sun vector measurement value S under the satellite body coordinate system obtained in the step 3) b Combined with the reference value B of the earth magnetic field vector in the orbital coordinate system o And the sun vector reference value S under the orbital coordinate system o Providing observation information of two local filters in a distributed Federal Kalman filtering algorithm;
6) And calculating to obtain the attitude of the micro-nano satellite by utilizing a distributed Federal Kalman filtering algorithm, and realizing satellite attitude calculation based on a plurality of low-cost, small-volume and low-power consumption micro sensors.
3. The method for determining the attitude of the distributed filtering micro-nano satellite with fault tolerance according to claim 2, wherein the method comprises the following steps: the fault detection and processing and state fusion process is implemented as follows,
(1) Fault detection and handling
Setting a fault detection factor
Figure FDA0004039713590000031
Fault threshold gamma of 0 Namely:
Figure FDA0004039713590000032
when the sensor is normal;
Figure FDA0004039713590000033
when the sensor fails, the sensor fails;
based on the fault detection factor and the fault threshold, when the sensor is detected to have a fault, if the local filter S1 fails, the local filter S1 is used for judging whether the sensor fails or not
Figure FDA0004039713590000034
Is incorrect and thus not input to the main filter, at which point the overall estimate of the system error state may be modified to
Figure FDA0004039713590000035
Similarly, if the local filter S2 fails, the overall estimation of the system error state is
Figure FDA0004039713590000036
When the sensor is not detected to have faults, directly executing the following step (2);
(2) State fusion
State estimation for local filters S1 and S2
Figure FDA0004039713590000037
Sum estimation error covariance matrix
Figure FDA0004039713590000038
Performing data fusion to obtain final state estimation
Figure FDA0004039713590000039
Sum estimation error covariance matrix P g,k ；
Figure FDA00040397135900000310
CN202010882166.6A 2020-08-28 2020-08-28 Distributed filtering micro-nano satellite attitude determination method with fault tolerance Active CN112212860B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010882166.6A CN112212860B (en) 2020-08-28 2020-08-28 Distributed filtering micro-nano satellite attitude determination method with fault tolerance

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010882166.6A CN112212860B (en) 2020-08-28 2020-08-28 Distributed filtering micro-nano satellite attitude determination method with fault tolerance

Publications (2)

Publication Number Publication Date
CN112212860A CN112212860A (en) 2021-01-12
CN112212860B true CN112212860B (en) 2023-03-03

Family

ID=74058977

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010882166.6A Active CN112212860B (en) 2020-08-28 2020-08-28 Distributed filtering micro-nano satellite attitude determination method with fault tolerance

Country Status (1)

Country Link
CN (1) CN112212860B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116149186B (en) * 2023-02-16 2023-08-11 大连交通大学 Kalman filtering method for fault estimation of satellite attitude control system

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216319A (en) * 2008-01-11 2008-07-09 南京航空航天大学 Low orbit satellite multi-sensor fault tolerance autonomous navigation method based on federal UKF algorithm
CN102735259A (en) * 2012-06-18 2012-10-17 北京控制工程研究所 Satellite control system fault diagnosis method based on multiple layer state estimators
CN102778888A (en) * 2012-07-11 2012-11-14 江苏科技大学 Fault detecting device and method of cable-free bathyscaph track control system
CN103323007A (en) * 2013-06-17 2013-09-25 南京航空航天大学 Robust federated filtering method based on time-variable measurement noise
CN103676941A (en) * 2013-12-24 2014-03-26 北京控制工程研究所 Satellite control system fault diagnosis method based on kinematics and dynamics model
CN106679693A (en) * 2016-12-14 2017-05-17 南京航空航天大学 Fault detection-based vector information distribution adaptive federated filtering method
CN108279010A (en) * 2017-12-18 2018-07-13 北京时代民芯科技有限公司 A kind of microsatellite attitude based on multisensor determines method
CN109373999A (en) * 2018-10-23 2019-02-22 哈尔滨工程大学 Combinated navigation method based on failure tolerant Kalman filtering
CN109813342A (en) * 2019-02-28 2019-05-28 北京讯腾智慧科技股份有限公司 A kind of fault detection method and system of inertial navigation-satellite combined guidance system
CN110196443A (en) * 2019-06-06 2019-09-03 中国人民解放军战略支援部队信息工程大学 A kind of fault-tolerance combined navigation method and system of aircraft
CN110285815A (en) * 2019-05-28 2019-09-27 山东航天电子技术研究所 It is a kind of can in-orbit whole-process application micro-nano satellite multi-source information attitude determination method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109829938B (en) * 2019-01-28 2020-12-08 杭州电子科技大学 Adaptive fault-tolerant volume Kalman filtering method applied to target tracking

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216319A (en) * 2008-01-11 2008-07-09 南京航空航天大学 Low orbit satellite multi-sensor fault tolerance autonomous navigation method based on federal UKF algorithm
CN102735259A (en) * 2012-06-18 2012-10-17 北京控制工程研究所 Satellite control system fault diagnosis method based on multiple layer state estimators
CN102778888A (en) * 2012-07-11 2012-11-14 江苏科技大学 Fault detecting device and method of cable-free bathyscaph track control system
CN103323007A (en) * 2013-06-17 2013-09-25 南京航空航天大学 Robust federated filtering method based on time-variable measurement noise
CN103676941A (en) * 2013-12-24 2014-03-26 北京控制工程研究所 Satellite control system fault diagnosis method based on kinematics and dynamics model
CN106679693A (en) * 2016-12-14 2017-05-17 南京航空航天大学 Fault detection-based vector information distribution adaptive federated filtering method
CN108279010A (en) * 2017-12-18 2018-07-13 北京时代民芯科技有限公司 A kind of microsatellite attitude based on multisensor determines method
CN109373999A (en) * 2018-10-23 2019-02-22 哈尔滨工程大学 Combinated navigation method based on failure tolerant Kalman filtering
CN109813342A (en) * 2019-02-28 2019-05-28 北京讯腾智慧科技股份有限公司 A kind of fault detection method and system of inertial navigation-satellite combined guidance system
CN110285815A (en) * 2019-05-28 2019-09-27 山东航天电子技术研究所 It is a kind of can in-orbit whole-process application micro-nano satellite multi-source information attitude determination method
CN110196443A (en) * 2019-06-06 2019-09-03 中国人民解放军战略支援部队信息工程大学 A kind of fault-tolerance combined navigation method and system of aircraft

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Fault detection and diagnosis in a spacecraft attitude determination system;F.N. Pirmoradi, et al;《Acta Astronautica》;20091231;第710–729页 *
组合导航系统的故障检测与重构;田晓春;《组合导航系统的故障检测与重构》;20200315;第33-44页 *

Also Published As

Publication number Publication date
CN112212860A (en) 2021-01-12

Similar Documents

Publication Publication Date Title
CN107525503B (en) Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN100356139C (en) Miniature assembled gesture measuring system for mini-satellite
CN112505737B (en) GNSS/INS integrated navigation method
CN109916395B (en) Gesture autonomous redundant combined navigation algorithm
CN103676941B (en) Satellite control system method for diagnosing faults based on kinematics and dynamics modeling
Youn et al. Combined quaternion-based error state Kalman filtering and smooth variable structure filtering for robust attitude estimation
CN107830858B (en) Gravity-assisted mobile phone heading estimation method
CN110231029B (en) Underwater robot multi-sensor fusion data processing method
Qin et al. Accuracy improvement of GPS/MEMS-INS integrated navigation system during GPS signal outage for land vehicle navigation
CN104698485A (en) BD, GPS and MEMS based integrated navigation system and method
CN110017837A (en) A kind of Combinated navigation method of the diamagnetic interference of posture
CN108168548A (en) A kind of pedestrian's inertial navigation system and method by machine learning algorithm and model-aided
CN115143954B (en) Unmanned vehicle navigation method based on multi-source information fusion
CN112212860B (en) Distributed filtering micro-nano satellite attitude determination method with fault tolerance
WO2022222938A1 (en) Motion state monitoring-based adaptive horizontal attitude measurement method
Liu et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system
Zhu et al. Orientation estimation by partial-state updating Kalman filter and vectorial magnetic interference detection
CN115265532A (en) Auxiliary filtering method for marine integrated navigation
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN112762925A (en) Low-orbit satellite attitude determination method based on geomagnetism meter and gyroscope
CN110375740B (en) Vehicle navigation method, device, equipment and storage medium
CN109916399B (en) Carrier attitude estimation method under shadow
Lajimi et al. A comprehensive filter to reduce drift from Euler angles, velocity, and position using an IMU
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant