CN109084751B - High-energy-efficiency satellite attitude determination algorithm based on box particle filter - Google Patents

High-energy-efficiency satellite attitude determination algorithm based on box particle filter Download PDF

Info

Publication number
CN109084751B
CN109084751B CN201810585156.9A CN201810585156A CN109084751B CN 109084751 B CN109084751 B CN 109084751B CN 201810585156 A CN201810585156 A CN 201810585156A CN 109084751 B CN109084751 B CN 109084751B
Authority
CN
China
Prior art keywords
attitude
satellite
satellite attitude
quaternion
box
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
CN201810585156.9A
Other languages
Chinese (zh)
Other versions
CN109084751A (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201810585156.9A priority Critical patent/CN109084751B/en
Publication of CN109084751A publication Critical patent/CN109084751A/en
Application granted granted Critical
Publication of CN109084751B publication Critical patent/CN109084751B/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/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
    • G01C21/025—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means with the use of startrackers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling

Abstract

The invention discloses a box particle filter-based high-energy-efficiency satellite attitude determination algorithm, which specifically comprises the following steps: preprocessing the measurement data of the magnetometer and the solar sensitive meter through a double-vector algorithm, and resolving to obtain a satellite attitude quaternion; predicting the satellite attitude quaternion through a prediction equation to obtain a satellite attitude prediction result; comparing the satellite attitude prediction result with the satellite attitude quaternion, wherein the intersection is a residual error; obtaining a likelihood function according to the probability model and the residual error of the measured noise; contracting the satellite attitude prediction result through the residual error to obtain a contracted satellite attitude prediction result; calculating the weight of the satellite attitude at the moment by combining a likelihood function according to the weight of the satellite attitude at the previous moment; estimating according to the weight to obtain a current attitude estimation value of the satellite; the invention provides high-precision measurement data for a subsequent attitude controller, and is a necessary premise for realizing effective control of an attitude control system.

Description

High-energy-efficiency satellite attitude determination algorithm based on box particle filter
[ technical field ] A method for producing a semiconductor device
The invention belongs to the technical field of pico-nano satellite attitude determination, and particularly relates to a high-energy-efficiency satellite attitude determination algorithm based on box particle filtering.
[ background of the invention ]
The attitude sensor is a measuring device of ADCS, at present, a photoelectric sun sensor and a magnetometer are mostly adopted, and a big satellite attitude determination system such as COMPASS-1, AAUSa1 is adopted. Nowadays, there are many micro devices based on very advanced processing technologies such as MEMS, NEMS, MOEMS, etc., such as micro gyroscope and micro accelerometer made by MEMS technology, or sun sensor and high-integration three-axis micro magnetometer made by MOEMS technology. However, in the present, the development of micro sensors is limited by mass, volume and power consumption, so that they have disadvantages in performance indexes compared with the conventional components, and have poor performance in some indexes. This is also the biggest problem facing the current pico-satellite development process. On the other hand, because pico-satellites are a relatively new research project at home and abroad, the research and development experience is lacked. The important problem to be solved by the attitude determination algorithm of the pico-satellite is to provide a pico-satellite attitude determination algorithm design scheme meeting the system requirements under the constraint of the existing conditions of quality, on-satellite computing capability, power consumption, control accuracy and the like, so that the pico-satellite attitude determination algorithm can meet the requirements of the pico-satellite on quality, volume, power consumption and the like.
There are two broad categories of attitude determination algorithms. One type is a static deterministic method, in which the TRIAD (Three-axis specific Determination) method is a typical algorithm, and the attitude matrix is determined by two non-parallel vector measurement information, so that the algorithm is simple and the calculated amount is small. The other type is a state space estimation algorithm for determining the attitude, which obtains the optimal attitude estimation under the statistical significance by utilizing the historical information measured by a sensor and combining a spacecraft attitude motion model.
The deterministic method has the advantage that the attitude of the spacecraft is determined directly according to the measurement value of the attitude sensor without the prior knowledge of the attitude. The method has the disadvantages that the error of the calculation result of the attitude determination is easy to cause larger error, and the attitude determination can be carried out only by at least two vectors, so that the method has certain limitation in use.
The modern nonlinear estimation algorithm is focused on EKF when entering the field of engineering application at first, but considering that the type of noise of spacecraft attitude observation is complex and the distribution of state quantities has characteristics, not all filters can complete attitude estimation, and a Particle Filter (PF) method which has no requirement on a probability density equation and can be performed under any distribution by nonlinear approximation is gradually developed later. The PF principle is to approximate a distribution using finite randomly weighted points, which is highly adaptive to non-gaussian problems.
The attitude determination process of the pico-satellite is highly nonlinear, the calculation is complex and high based on a Particle Filter (PF) high-precision determination method, the CPU design of the pico-satellite cannot provide high-efficiency calculation capability, and the precision of the basic Extended Kalman Filter (EKF) method is not enough, so that a new filtering method needs to be searched to simultaneously meet the requirements of high energy efficiency and high precision of attitude determination.
[ summary of the invention ]
The invention aims to provide an energy-efficient satellite attitude determination algorithm based on box particle filtering, which can greatly shorten the operation time of the algorithm and reduce the energy consumption of satellite calculation while ensuring the accuracy of attitude determination.
The invention adopts the following technical scheme: an energy-efficient satellite attitude determination algorithm based on box particle filtering specifically comprises the following steps:
step 1, preprocessing the measurement data of the magnetometer and the sun sensitive meter by a double-vector algorithm, and resolving to obtain a satellite attitude quaternion q ═ η epsilon1ε2ε3]T；
Step 2, passing a prediction equation
Figure GDA0002451151330000021
Predicting the quaternion of the satellite attitude to obtain the result of satellite attitude prediction
Figure GDA0002451151330000022
Wherein [ H ]]In order to observe the matrix, the system,
Figure GDA0002451151330000023
the ith state-quantity box for the satellite at time k,
Figure GDA0002451151330000024
are respectively as
Figure GDA0002451151330000025
The lower and upper bounds of (a) and (b),
Figure GDA0002451151330000031
step 3, predicting the result through the satellite attitude
Figure GDA0002451151330000032
Comparing with the quaternion q of satellite attitude, and intersecting
Figure GDA0002451151330000033
As residual error:
Figure GDA0002451151330000034
wherein the content of the first and second substances,
Figure GDA0002451151330000035
are respectively as
Figure GDA0002451151330000036
Lower and upper bounds of vrIs white gaussian noise;
and 4, obtaining a likelihood function according to the probability model and the residual error of the measured noise:
Figure GDA0002451151330000037
wherein A isi(j) A likelihood function representing a jth quantity, j ∈ q;
step 5, contracting the satellite attitude prediction result through the residual error to obtain a contracted satellite attitude prediction result:
Figure GDA0002451151330000038
[B]is a residual error
Figure GDA0002451151330000039
A coefficient;
step 6, according to the weight of the satellite attitude at the k-1 moment
Figure GDA00024511513300000310
Combining likelihood functions AiBy the formula
Figure GDA00024511513300000311
Computing weights for satellite attitude at time k
Figure GDA00024511513300000312
Step (ii) of7. By the formula
Figure GDA00024511513300000313
Estimating to obtain the current attitude estimation value of the satellite
Figure GDA00024511513300000314
Wherein the content of the first and second substances,
Figure GDA00024511513300000315
by the formula
Figure GDA00024511513300000316
So as to obtain the compound with the characteristics of,
Figure GDA00024511513300000317
and
Figure GDA00024511513300000318
by passing
Figure GDA0002451151330000041
And (6) obtaining.
Further, the satellite attitude estimate
Figure GDA0002451151330000042
In particular by
Figure GDA0002451151330000043
And then converted into the specific state quantity of the satellite,
wherein [ q ]]＝[η]×[ε1]×[ε2]×[ε3]For the interval description of the satellite attitude quaternion q, η is the quaternion scalar section, ε1、ε2、ε3Is a part of the vector of the quaternion,
Figure GDA0002451151330000044
is a compartmentalized representation of the angular velocity of the satellite.
The invention has the beneficial effects that: the invention provides an interval pico-satellite attitude description method based on an interval theory, establishes a mathematical model of an attitude sensor, deduces a pico-satellite attitude determination algorithm based on BPF on the basis, verifies the effectiveness of the algorithm through digital simulation, simultaneously explores the influence of different parameter settings on filter parameters, provides high-precision measurement data for a subsequent attitude controller, and is a necessary premise for realizing effective control of an attitude control system.
[ description of the drawings ]
FIG. 1 is a Euler angle error comparison graph in a comparison experiment of a BPF attitude determination algorithm and a PF attitude determination algorithm of the present invention;
FIG. 2 is a comparison graph of steady state Euler angle errors in a comparison experiment of the BPF attitude determination algorithm and the PF attitude determination algorithm of the present invention;
FIG. 3 is a quaternion error comparison plot in a comparison experiment of the BPF attitude determination algorithm and the PF attitude determination algorithm of the present invention;
FIG. 4 is a steady state quaternion error comparison plot in a comparison experiment of the BPF attitude determination algorithm and the PF attitude determination algorithm of the present invention;
FIG. 5 is a plot of the attitude error RMS statistical results in a comparison experiment of the BPF attitude determination algorithm and the PF attitude determination algorithm of the present invention;
FIG. 6 is a Euler angle error comparison plot in an experiment measuring the impact of noise on BPF attitude determination accuracy in accordance with the present invention;
FIG. 7 is a comparison chart of steady state Euler angle error in an experiment for measuring the influence of noise on the determination accuracy of BPF attitude according to the present invention;
FIG. 8 is a graph of quaternion error contrast in experiments measuring the effect of noise on BPF attitude determination accuracy in accordance with the present invention;
FIG. 9 is a graph of steady state quaternion error contrast in an experiment measuring the impact of noise on BPF attitude determination accuracy in accordance with the present invention;
FIG. 10 is a comparison graph of attitude error RMS statistics in experiments measuring the impact of noise on BPF attitude determination accuracy in accordance with the present invention;
FIG. 11 is a comparison graph of dual vector attitude determination errors in an experiment measuring the impact of noise on BPF attitude determination accuracy in accordance with the present invention;
FIG. 12 is a graph of Euler angle error versus the number of particles in the box of the present invention in an experiment of the impact of the number of particles on BPF attitude determination;
FIG. 13 is a comparison chart of steady state Euler angle error in experiments of the impact of the number of cell particles on BPF attitude determination in accordance with the present invention;
FIG. 14 is a graph of quaternion error versus experiment showing the effect of the number of box particles on BPF attitude determination in accordance with the present invention;
FIG. 15 is a graph of steady state quaternion error versus the number of particles in the box of the present invention in an experiment of the effect of the number of particles on BPF attitude determination;
FIG. 16 is a plot of RMS statistics of attitude error in experiments demonstrating the effect of the number of cartridge particles on BPF attitude determination in accordance with the present invention.
[ detailed description ] embodiments
The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
An energy-efficient satellite attitude determination algorithm based on box particle filtering is characterized in that firstly, the satellite attitude is described in an interval mode, and in order to apply BPF to the attitude determination process, an attitude and motion model needs to be described by using an interval analysis theory. The satellite is required to be subjected to interval analysis firstly, then attitude interval description is carried out on the satellite, and then satellite attitude determination is carried out by utilizing a BPF (box particle filter) attitude determination algorithm.
The interval analysis specifically comprises:
in the invention, a PF (particle filter) strategy related to the interval data mobile positioning is proposed. Since the use interval data is calculated by using a group of particles which is much smaller than the normal PF algorithm, the calculation cost is reduced, and the efficiency is improved.
The interval representation is characterized by:
an interval means that an infinite number of particles are distributed throughout the interval.
One interval means that the particles are not precisely distributed over the entire interval.
Defining a space
Figure GDA0002451151330000061
A closed continuum subset within is the interval [ x ]]Then, then
Figure GDA0002451151330000062
Wherein, [ x ]1]、[x2]、
Figure GDA0002451151330000063
All of which represent the particles of the cell,
Figure GDA0002451151330000064
represents from x1To
Figure GDA0002451151330000065
The cartesian product.
In learning the interval, first, the integration of the function f must be known]Same interval [ x ]]Function of (f)]Can be expressed as [ f]([x]) The precondition for this function to have a solution is the interval x]Including the function optimal solution. The basic operation rules include +, -, and/and some standard operations in space, such as:
Figure GDA0002451151330000066
∪ and ∩ and the like likewise need to be extended into bounded error theory.
Four dimensional space
Figure GDA0002451151330000067
Element x of vector x in (1)iIf there are m f associated with itmExpressed as:
Figure GDA0002451151330000068
the simplified writing is: f (x) Om×1Wherein f is fjThe cartesian product of (a).
CSP represents a problem that aggregates variable vectors defining domain D and variable x in xi。
Defining intervals
Figure GDA0002451151330000069
(representing a set of real numbers) in an imprecise variation factor xiHas an interval model of [ x ]i]Which is usually reacted with xiA priori information defining the domain is associated. x is the number ofiPriors of a defined domainThe information is expanded to
Figure GDA0002451151330000071
In domain, defined as:
Figure GDA0002451151330000072
CSP involves seeking to satisfy the constraint f (x) Om×1X ∈ [ x ] of solution]. The CSP solution set is defined as S ═ x ∈ [ ]]|f(x)＝Om×1}. It should be noted that S is not a box. In interval theory, solving CSP is similar to finding satisfying S ∈ [ x']Of [ x']∈[x]。
CSP box [ x ]]The ith factor xiThe condition of global consistency is that all can be in [ S ]]Find at least one factor xiIs the ith coordinate, i.e.
Figure GDA0002451151330000073
CSPs (constructs Satisf infection Problem) cassette [ x]The ith factor xiThe condition of local consistency is that all [ S ] can be inj]j＝1,2,…,mAt least one vector factor xiIs the (i) th coordinate and (ii) th coordinate,
namely:
Figure GDA0002451151330000074
compartmentalized description of satellite attitude:
in the state space
Figure GDA0002451151330000075
In, define the interval [ x]，[x]Is a space
Figure GDA0002451151330000076
Inner closed continuous collection:
Figure GDA0002451151330000077
wherein, [ x ]i]Representation space
Figure GDA0002451151330000078
A closed contiguous subset of.
According to the definition of the attitude quaternion, the quaternion interval expression is as follows: [ q ] of]＝[η]×[ε1]×[ε2]×[ε3]According to the constraint condition of the quaternion,
Figure GDA0002451151330000079
let [ q ] be]＝[qlow,qhigh]Wherein q islowIs [ q ]]Lower bound, qhighIs [ q ]]And (4) an upper bound. According to the euler angle definition, the attitude angular velocity interval is expressed as: [ omega ]]＝[ωx]×[ωy]×[ωz]ω denotes angular velocity and subscripts denote the three axes x, y, z.
All possible values (particle values, namely random sample values) in a definition domain are represented by dividing the definition domain into finite intervals, particularly variables with the definition domain range of-1 to 1 such as attitude quaternion, and each interval can represent infinite points in the interval only by calculating upper and lower limits through the concept of interval analysis.
According to the attitude kinematics equation, defining f as a satellite attitude motion function:
Figure GDA00024511513300000710
then there are:
Figure GDA0002451151330000081
wherein epsilon is a quaternion vector part, η is a quaternion scalar part, I3×3An identity matrix representing 3 x 3; s (epsilon) represents a skew symmetric matrix.
According to the interval analysis theory, the interval of the definition f is expressed as [ f ], and similarly, an interval [ x ] function [ f ] can be expressed as [ f ] ([ x ]), so that the attitude kinematic equation of the attitude interval [ q ] is:
Figure GDA0002451151330000082
also, according to the attitude dynamicsThe equation, g, is defined as the satellite dynamics function,
Figure GDA0002451151330000083
then there are:
Figure GDA0002451151330000084
wherein J represents moment of inertia;
Figure GDA0002451151330000085
is an attitude rotation matrix;
Figure GDA0002451151330000086
the projection of the angular velocity of the orbital system relative to the inertial system under the orbital system; t isbFor total moment acting on stars
The compartmentalized kinetic equation is also obtained as:
Figure GDA0002451151330000087
the euler angle is solved by using a dual-vector algorithm, and then the solved euler angle is filtered, so that the estimation of sensor noise is completed, and the more accurate estimation of the satellite attitude is realized.
The method specifically comprises the following steps:
establishing an attitude estimation state equation and a measurement equation:
selecting the interval quaternion and the interval angular velocity as the state quantity includes:
Figure GDA0002451151330000091
definition of [ F]([X]) For non-linear interval of state functions, i.e.
Figure GDA0002451151330000092
In combination with the above formula to
Figure GDA0002451151330000093
Definition of lklAnd lkhRespectively representing the lower bound and the upper bound of the state quantity interval at the k-th time. And has the following components:
Figure GDA0002451151330000094
wherein, [ X ]k]A state interval at the time k; lklAnd lkhRespectively representing the lower bound and the upper bound of the state quantity interval at the k-th time.
Since the attitude kinematics curve changes smoothly, when the interval is selected to be small, the state transfer function defined in the interval is substantially linear, and thus it can be obtained:
Figure GDA0002451151330000095
wherein, [ F ]]For the interval representation of the state transition function, F (l)(k-1)l) Performing state transfer function operation on the lower boundary of the state quantity interval at the k-1 moment; f (l)(k-1)h) The state transition function operation is carried out on the upper boundary of the state quantity interval at the moment k-1.
The differential equation is solved by adopting a four-order Runge-Kutta method to obtain the state of the next moment,
Figure GDA0002451151330000101
wherein, f (x) the state solution at the next time; x is an independent variable, K1～4Is an intermediate variable.
When only the magnetometer and the sun sensor are used for attitude measurement, the amount is measured as the output of the magnetometer and the sun sensor. However, this method performs coordinate transformation on the obtained attitude rotation matrix at each filtering step, thereby increasing the amount of calculation. Therefore, a dual-vector method is adopted to preprocess the measurement, so that the measurement dimension is reduced, and the calculated amount is reduced. The three-dimensional measurement thus obtained is:
[Z]＝[ε]＝[ε1]×[ε2]×[ε3]，
likewise, define zklAnd zkhThe lower and upper bounds of the k-time measurement interval are respectively indicated. The system measurement equation thus obtained is:
Figure GDA0002451151330000102
wherein [ Z ]k]: interval representation of the observation signal at the time k; [ H ]]Is an interval representation of the observation matrix; [ V ]]For observing interval representation of noise
The attitude determination algorithmic process from PF to BPF derives:
initialization of the cartridge particles:
for a PF, this stage involves generating a set of N point particles in a defined region of the state space
Figure GDA0002451151330000103
This region is chosen to explore the state space. Typically, all particles are associated with the same weight. For the box particle filter, the initialization process is simply expanded. Therefore, unlike set-point particles, the state space region of a box particle filter can be divided into N boxes
Figure GDA0002451151330000111
And assigned to the null set and the same weight. One advantage of using the cartridge method for initialization is that the number of particles can be reduced. When k is 0, all boxes are set as empty sets and given the same weight, which can be expressed as:
Figure GDA0002451151330000112
in the formula (I), the compound is shown in the specification,
Figure GDA0002451151330000113
represents the ith state quantity box at step k,
Figure GDA0002451151330000114
to represent
Figure GDA0002451151330000115
The weight of (a) is determined,
Figure GDA0002451151330000116
represents the lower bound of the ith state quantity box particle at step k,
Figure GDA0002451151330000117
represents the upper bound of the ith state quantity box particle at step k. When k is 0, there is any box
Figure GDA0002451151330000118
And (3) state transition:
in this step, the state of each particle is updated according to the different evolution models of the different noise used. The significant difference between particle filtering and box particle filtering is the handling of noise, and the bounded error approach eliminates the need for the BPF state transition process to take into account the effects of measurement noise. For particle filtering, noise allows us to take into account errors that may occur during modeling and measurement, requiring process noise to be added during state transitions. In the BPF process, errors generated in modeling and measurement are directly processed by a bounded error model.
The state transition equation for BPF can be expressed as:
Figure GDA0002451151330000119
the state update of the BPF includes prediction, residual, likelihood function, and BPF inverse solution processes. In this step, the new measurements are used to adapt the weights of the particles in order to obtain new state quantity estimates.
Step 1, preprocessing the measurement data of the magnetometer and the sun sensitive meter by a double-vector algorithm, and resolving to obtain a satellite attitude quaternion q ═ η epsilon1ε2ε3]T。
Step 2, predictionEquation of
Figure GDA0002451151330000121
Predicting the quaternion of the satellite attitude to obtain the result of satellite attitude prediction
Figure GDA0002451151330000122
Wherein [ H ]]In order to observe the matrix, the system,
Figure GDA0002451151330000123
the ith state-quantity box for the satellite at time k,
Figure GDA0002451151330000124
are respectively as
Figure GDA0002451151330000125
The lower and upper bounds of (a) and (b),
Figure GDA0002451151330000126
Figure GDA0002451151330000127
the interval length of (A) can be expressed as
Figure GDA0002451151330000128
Step 3, predicting the result through the satellite attitude
Figure GDA0002451151330000129
Comparing with the quaternion q of satellite attitude, and intersecting
Figure GDA00024511513300001210
As residual error:
Figure GDA00024511513300001211
wherein the content of the first and second substances,
Figure GDA00024511513300001212
are respectively as
Figure GDA00024511513300001213
Lower and upper bounds of vrIs gaussian white noise.
The residual part of the particle filtering depends on the amount of difference between the actual and predicted measurements of each pose parameter. Usage prediction
Figure GDA00024511513300001214
And actual measurement yk(i.e., the attitude quaternion q of the observation), the residual is described by the direct difference of the two.
With BPF, the predictable value can be compared to a true box measurement, so the residual requirement reflects the proximity between the true measurement and the predicted value. In the framework of bounded errors, we define the residual as the intersection between two intervals. In the invention, the measured noise is similar to Gaussian distribution, namely Gaussian white noise, and conforms to normal distribution. From the confidence theory of normal distribution, the probability of random error occurrence is 99.73% within 3 times of standard deviation ± σ. And modeling the measurement by combining a boundary error theory. White gaussian noise vkAfter compartmentalization is [ v ]k]＝[-3σv,3σv]Then, the measurement interval can be obtained as follows:
[yk]＝[yk-3σ,yk+3σ]＝[ykl,ykh]，
where σ is white Gaussian noise vkStandard deviation of (d); y isklAnd ykhRespectively represent the particles [ y ] of the measuring cellk]Lower and upper bounds. Will predict the box
Figure GDA0002451151330000131
And a measuring box ykComparing, the intersection is the residual error
Figure GDA0002451151330000132
Figure GDA0002451151330000133
Because the prediction and measurement are not intersected or the intersection is a point, the judgment needs to be made, namely the satellite attitude prediction result and the satellite attitude quaternion are compared to obtain the residual error of the two:
Figure GDA0002451151330000134
wherein v isrIs white Gaussian noise, then
Figure GDA0002451151330000135
Can be written as
Figure GDA0002451151330000136
The interval length of the residual box particle is then
Figure GDA0002451151330000137
And 4, for the particle filtering, the likelihood function can be obtained according to the probability model of the measurement noise, and the measurement noise w conforms to Gaussian distribution.
For the box particle filter, in the framework of the bounded error theory, the interval with the residual error as an empty set should be discarded, and the box particle containing the actual measurement information and the prediction information should be screened out, so that the likelihood function is obtained according to the probability model of the measurement noise and the residual error:
Figure GDA0002451151330000138
wherein A isi(j) A likelihood function representing the jth quantity, j ∈ q.
Step 5, the shrinking process of the box type particle filter is the main part of the process different from the particle filter, and is the key point of the process, which is shorter time consumption compared with the particle filter under the same accuracy. In the standard particle filter algorithm process, the propagation process of each particle does not contain information of the variance of its position. Thus, the weight of each particle provides only its likelihood information.
In contrast, the box particle is used to calculate its accuracy after the propagation process under the boundary error theory. To reduce the length of each box, the shrinking algorithm must be able to estimate the measurements of non-contiguous boxes. In practice, this process is consistent with the KF correcting variance covariance based on actual measurements. The shrinking process aims at shrinking the prediction box by residual, thereby solving a new set of state quantity box particles back to the range of the state quantity box particles according to the relation between the state quantity and the prediction. The shrinker can be defined as an algorithm for shrinking the CSP base definition domain, thereby generating a new box [ x ']. e [ x ] that S ∈ [ x' ] satisfies.
According to the Waltz theory, contracting the satellite attitude prediction result through the residual error to obtain the contracted satellite attitude prediction result:
Figure GDA0002451151330000141
[B]is a residual error
Figure GDA0002451151330000142
And (4) the coefficient.
Step 6, according to the weight of the satellite attitude at the k-1 moment
Figure GDA0002451151330000143
Combining likelihood functions AiBy the formula
Figure GDA0002451151330000144
Computing weights for satellite attitude at time k
Figure GDA0002451151330000145
The BPF and the particle filter have the same unitization process, and the unitization process is to ensure that the sum of the weights of the boxes is 1, and the formula is
Figure GDA0002451151330000146
Step 7,The state estimation process in particle filtering is to estimate an estimated value of a state quantity at this time using weighted particles. And the BPF state estimation process uses the center particle of each box to represent the whole box so as to calculate the estimated value of the state quantity at the moment, and the estimated value is calculated by a formula
Figure GDA0002451151330000147
Estimating to obtain the current attitude estimation value of the satellite
Figure GDA0002451151330000148
Wherein the content of the first and second substances,
Figure GDA0002451151330000149
by the formula
Figure GDA00024511513300001410
So as to obtain the compound with the characteristics of,
Figure GDA00024511513300001411
and
Figure GDA00024511513300001412
by passing
Figure GDA00024511513300001413
And (6) obtaining.
Satellite attitude estimate
Figure GDA0002451151330000151
In particular by
Figure GDA0002451151330000152
And then converted into the specific state quantity of the satellite,
wherein [ q ]]＝[η]×[ε1]×[ε2]×[ε3]For the interval description of the satellite attitude quaternion q, η is the quaternion scalar section, ε1、ε2、ε3Is a part of the vector of the quaternion,
Figure GDA0002451151330000153
is a compartmentalized representation of the angular velocity of the satellite.
In addition, the invention can also carry out importance resampling:
there are many methods of resampling, and the importance resampling is used in the present invention. However, the box particles of the high dimensional model cannot meet the requirement due to the reduced particle diversity caused by importance resampling, which may directly result in high non-linearity. Therefore, during application, there is a need to increase the particle diversity.
According to the obtained weight of the particles, let
Figure GDA0002451151330000154
NeffRepresenting the value of the sample when Neff＜NthAnd performing an importance resampling process. N is a radical ofthThe value threshold value obtained through matlab simulation experiments is used for solving the problem of reduced particle diversity
Figure GDA0002451151330000155
To the resampled set of cartridge particles,
Figure GDA0002451151330000156
is the box particle weight after resampling. Setting a judgment number JdugeiWherein { Jduge }i|Jdugei∈(0,1),i∈[1,Nbox]}. When in use
Figure GDA0002451151330000157
Then, one can obtain:
Figure GDA0002451151330000158
after resampling is finished, continuously finishing box particle updating and weight normalization:
Figure GDA0002451151330000159
the method of the invention is carried out with the experimental analysis of the measurement noise:
since the measurement of the filter used in the present invention is the euler angle solved by the dual vector attitude determination algorithm, its noise is not gaussian. In fact, however, the noise model used in the filter design is gaussian. Therefore, modeling the error between the calculated euler angle and the real euler angle becomes the primary challenge of the whole simulation experiment, and according to the actual requirement, the maximum value and the mean value of the three-axis variance determined by the postures of different magnetometer noises and sun sensors under the noise are counted. The following results were obtained:
the mean value of the three-axis precision determined by the double-vector attitude is basically 0;
the statistics of the standard deviation of the triaxial accuracy determined by the double-vector attitude are as follows:
TABLE 1 Tri-axial Standard deviation of precision for Dual vector attitude determination
Figure GDA0002451151330000161
The whole satellite is a rigid body, the projections of the angular velocity of rotation of the satellite in the system of the satellite are all 0.05 degrees per second, the three-axis projections of the initial Euler angle in the system are all 60 degrees, and the satellite external moment is 0. The following three sets of comparative experiments were set up.
(1) Comparison experiment of BPF attitude determination algorithm and PF attitude determination algorithm.
To verify the effectiveness of the BPF pose determination algorithm, the experimental comparison of the BPF pose determination algorithm to the PF pose determination algorithm is given below. The reference scene parameters are designed as follows:
the BPF box number of particles is 100, and the PF number of particles is 1000;
the resampling threshold values of both filters are 20;
the magnetometer noise was 10-7(T) and the sun sensor noise was 0.01. From the results in table 1, the standard deviation of the estimation accuracy of the dual vector pose determination algorithm is 0.96.
As shown in fig. 1-4, the BPF versus PF attitude determination error is presented. As can be seen from the figure, in the initial stage of attitude determination, the BPF attitude determination algorithm has a faster convergence rate compared with the PF method, and the attitude error curve enters a steady state within about 500 s. Since the BPF method has a box particle shrinking process, that is, the state quantity is inversely solved through the intersection of the measurement value and the measurement prediction to shorten the interval of the state quantity, thereby speeding up the convergence, the jump in fig. 5 is the result of large deviation of the inversely solved state quantity due to the sudden change of the measurement noise; the PF posture determining method reduces the convergence speed after 700s until the method enters a steady state, and has better estimation accuracy in the steady state.
TABLE 2 comparison of BPF and PF filtering results for stable attitude
Filtering algorithm Euler angle estimation accuracy (°) Run time(s)
BPF 5.4859×10-2 10.0217
PF 4.0215×10-2 44.1465
Table 2 shows statistical results of BPF and PF attitude estimation accuracy and operating time at attitude steady state. Statistical data indicate that BPF pose determination estimates with slightly lower accuracy than the PF method (about 73.3% of the PF method), but only run time of 22.9% of the PF method. Therefore, compared with the traditional PF method, the BPF attitude determination method can effectively shorten the operation time, improve the attitude determination efficiency and reduce the energy consumption of the system under the condition that the control precision is not large.
(2) And (5) measuring influence of noise on the BPF attitude determination precision.
In order to further verify the reliability of the BPF attitude determination algorithm, the following simulation experiment gives the estimation results of BPF attitude determination under different measurement noises. In which the noise of the magnetometer for the comparative experiment was 10-6(T), the noise of the sun sensor is 0.01, the standard deviation of the estimation accuracy of the double-vector attitude determination algorithm is 3.64 according to the result in the table 1, and the rest parameters are set to be the same as the parameters of the filter in the reference scene. The simulation results are shown in fig. 6-11, which give the BPF attitude determination estimates at a metrology standard deviation of 3.96 and 3.64, respectively. It can be seen that when the measurement noise is large, the initial estimation accuracy of the attitude estimation accuracy curve is deteriorated, and the convergence rate is significantly reduced. This is because when the measurement noise is large, the measurement range in the BPF attitude determination algorithm is significantly increased, and the initial estimation accuracy is consequently deteriorated. At the same time, the new state interval obtained by the inverse solution process of the box particle becomes larger, so that the convergence speed and the final estimation precision are affected.
TABLE 3 comparison of BPF filtering results for different measured noises
Filtering algorithm Euler angle estimation accuracy (°) Run time(s)
BPF 5.4859×10-2 10.0217
PF 0.2854 11.0478
Table 3 shows the estimation accuracy and running time of the BPF pose determination algorithm under different metrology noises. The statistical result further shows that as the measurement noise increases, the difference of the operation time of the BPF attitude determination algorithm is not large, but the estimation precision is obviously reduced, namely the BPF attitude determination precision and the measurement noise are in negative correlation.
(3) Effect of number of box particles on BPF pose determination experiments.
The experiment verifies the relation between the box particle quantity and the BPF attitude determination precision mainly through simulation experiments. The number of cartridge particles for the comparative experiment was 50. The rest of the parameter settings are the same as the filter parameter settings in the reference scene. Results of simulation experiments the results of BPF pose determination estimates for the case of 50 and 100 cell counts, respectively, are shown in fig. 12-16. It can be seen that when the number of box particles is reduced, the initial estimation accuracy of the attitude estimation accuracy curve is significantly deteriorated, but the convergence speed is not changed much.
Table 4 comparison of BPF filtering results for different box particle numbers
Filtering algorithm Euler angle estimation accuracy (°) Run time(s)
BPF 5.4859×10-2 10.0217
PF 9.3723×10-2 7.4303
Table 4 gives the estimated accuracy and run time of the BPF pose determination algorithm for different box particle numbers. Statistics further show that when the number of particles is reduced from 100 to 50, the operating time of the BPF attitude determination algorithm is shortened by 25.86%, and the estimation accuracy is reduced by 41.51%. Although the running time of the algorithm is shortened along with the reduction of the number of particles, the running time of the algorithm is not in a proportional relation, because when the number of particles of the initial box is reduced, attenuation of box particle intervals in the filtering process is obvious, and therefore the algorithm consumes some time due to the fact that the number of resampling times is increased. It follows that BPF pose determination accuracy is positively correlated with the box particle number, and algorithm run time is negatively correlated with the box particle number.
The invention introduces an interval analysis theory to perform interval description on the attitude and a mathematical model of the pico-nano satellite and also to model an attitude sensor. And then starting from the model, completing the whole derivation of the algorithm. The feasibility and the performance of the BPF attitude determination algorithm of the pico-satellite are verified through digital simulation, the advantages of the BPF algorithm are summarized after the BPF attitude determination algorithm is compared with the PF algorithm, the application characteristics of the BPF algorithm on the attitude determination problem are determined, and the influence of the number of box particles and measurement noise on the invention is researched.

Claims (2)

1. An energy-efficient satellite attitude determination algorithm based on box particle filtering is characterized by specifically comprising the following steps:
step 1, preprocessing the measurement data of the magnetometer and the sun sensitive meter by a double-vector algorithm, and resolving to obtain a satellite attitude quaternion q ═ η epsilon1ε2ε3]T；
Step 2, passing a prediction equation
Figure FDA0002451151320000011
Predicting the quaternion of the satellite attitude to obtain the result of satellite attitude prediction
Figure FDA0002451151320000012
Wherein [ H ]]In order to observe the matrix, the system,
Figure FDA0002451151320000013
the ith state-quantity box for the satellite at time k,
Figure FDA0002451151320000014
are respectively as
Figure FDA0002451151320000015
The lower and upper bounds of (a) and (b),
Figure FDA0002451151320000016
Figure FDA0002451151320000017
represents the lower bound of the ith state quantity box particle at step k,
Figure FDA0002451151320000018
represents the upper bound of the ith state quantity box particle at the kth step;
step 3, predicting the result through the satellite attitude
Figure FDA0002451151320000019
Comparing with the quaternion q of satellite attitude, and intersecting
Figure FDA00024511513200000110
As residual error:
Figure FDA00024511513200000111
wherein the content of the first and second substances,
Figure FDA00024511513200000112
are respectively as
Figure FDA00024511513200000113
Lower and upper bounds of，vrIs white gaussian noise;
and 4, obtaining a likelihood function according to the probability model and the residual error of the measured noise:
Figure FDA00024511513200000114
wherein A isi(j) A likelihood function representing a jth quantity, j ∈ q;
step 5, contracting the satellite attitude prediction result through the residual error to obtain a contracted satellite attitude prediction result:
Figure FDA0002451151320000021
[B]is a residual error
Figure FDA0002451151320000022
A coefficient;
step 6, according to the weight of the satellite attitude at the k-1 moment
Figure FDA0002451151320000023
Combining likelihood functions AiBy the formula
Figure FDA0002451151320000024
Computing weights for satellite attitude at time k
Figure FDA0002451151320000025
Step 7, passing through a formula
Figure FDA0002451151320000026
Estimating to obtain the current attitude estimation value of the satellite
Figure FDA0002451151320000027
Wherein the content of the first and second substances,
Figure FDA0002451151320000028
by the formula
Figure FDA0002451151320000029
So as to obtain the compound with the characteristics of,
Figure FDA00024511513200000210
and
Figure FDA00024511513200000211
by passing
Figure FDA00024511513200000212
And (6) obtaining.
2. The energy-efficient satellite attitude determination algorithm based on box particle filtering as claimed in claim 1 wherein the satellite attitude estimate is
Figure FDA00024511513200000213
In particular by
Figure FDA00024511513200000214
And then converted into the specific state quantity of the satellite,
wherein [ q ]]＝[η]×[ε1]×[ε2]×[ε3]For the interval description of the satellite attitude quaternion q, η is the quaternion scalar section, ε1、ε2、ε3Is a part of the vector of the quaternion,
Figure FDA00024511513200000215
is a compartmentalized representation of the angular velocity of the satellite.
CN201810585156.9A 2018-06-08 2018-06-08 High-energy-efficiency satellite attitude determination algorithm based on box particle filter Active CN109084751B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810585156.9A CN109084751B (en) 2018-06-08 2018-06-08 High-energy-efficiency satellite attitude determination algorithm based on box particle filter

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810585156.9A CN109084751B (en) 2018-06-08 2018-06-08 High-energy-efficiency satellite attitude determination algorithm based on box particle filter

Publications (2)

Publication Number Publication Date
CN109084751A CN109084751A (en) 2018-12-25
CN109084751B true CN109084751B (en) 2020-06-19

Family

ID=64839784

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810585156.9A Active CN109084751B (en) 2018-06-08 2018-06-08 High-energy-efficiency satellite attitude determination algorithm based on box particle filter

Country Status (1)

Country Link
CN (1) CN109084751B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111352138B (en) * 2020-01-21 2023-02-17 北京眸星科技有限公司 Static positioning method of satellite navigation system

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100462682C (en) * 2007-06-19 2009-02-18 北京航空航天大学 Self boundary marking method based on forecast filtering and UPF spacecraft shading device
CN101982732B (en) * 2010-09-14 2012-02-01 北京航空航天大学 Micro-satellite attitude determination method based on ESOQPF (estimar of quaternion particle filter ) and UKF (unscented kalman filter) master-slave filtering
CN103123487B (en) * 2011-11-21 2017-08-29 上海航天控制工程研究所 A kind of spacecraft attitude determination method
CN106023254A (en) * 2016-05-19 2016-10-12 西安电子科技大学 Multi-target video tracking method based on box particle PHD (Probability Hypothesis Density) filtering
CN107869993A (en) * 2017-11-07 2018-04-03 西北工业大学 Small satellite attitude method of estimation based on adaptive iteration particle filter

Also Published As

Publication number Publication date
CN109084751A (en) 2018-12-25

Similar Documents

Publication Publication Date Title
CN110118560B (en) Indoor positioning method based on LSTM and multi-sensor fusion
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN105606096B (en) A kind of posture of carrier movement status information auxiliary and course calculate method and system
CN108519090B (en) Method for realizing double-channel combined attitude determination algorithm based on optimized UKF algorithm
US20120065947A1 (en) Collision Effect And Particle Information Update In Particulate Fluid Flow Simulations
Qin et al. Accuracy improvement of GPS/MEMS-INS integrated navigation system during GPS signal outage for land vehicle navigation
Sushchenko et al. Processing of redundant information in airborne electronic systems by means of neural networks
CN109343550A (en) A kind of estimation method of the spacecraft angular speed based on moving horizon estimation
CN104613966B (en) A kind of cadastration off-line data processing method
CN108061887A (en) A kind of near space method for tracking target based on fuzzy interacting multiple model algorithm
CN107807527A (en) The adaptive super-twisting sliding mode control method of gyroscope adjustable gain
CN104048676A (en) MEMS (Micro Electro Mechanical System) gyroscope random error compensating method based on improved particle filter
CN109084751B (en) High-energy-efficiency satellite attitude determination algorithm based on box particle filter
CN103218482A (en) Estimation method for uncertain parameters in dynamic system
CN115046545A (en) Positioning method combining deep network and filtering
CN109067381A (en) A kind of Real-Time Filtering system and method for MEMS gyroscope random noise
CN105021199A (en) LS (Least square)-based multi- model adaptive state estimation method and system
CN110262237A (en) Gyroscope super-twisting sliding mode control method based on double feedback fuzzy neural networks
CN113204909A (en) Satellite geometric feature and attitude estimation method based on ground observation photometric signal
CN110186483B (en) Method for improving drop point precision of inertia guidance spacecraft
Yan et al. An intelligent adaptive Kalman filter for integrated navigation systems
Yafei et al. Particle filtering for gyroless attitude/angular rate estimation algorithm
Sang et al. Invariant cubature Kalman filtering-based visual-inertial odometry for robot pose estimation
CN104715133B (en) A kind of kinematics parameters in-orbit identification method and apparatus of object to be identified
CN110672127A (en) Real-time calibration method for array type MEMS magnetic sensor

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