CN111578931B - High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation - Google Patents

High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation Download PDF

Info

Publication number
CN111578931B
CN111578931B CN202010437604.8A CN202010437604A CN111578931B CN 111578931 B CN111578931 B CN 111578931B CN 202010437604 A CN202010437604 A CN 202010437604A CN 111578931 B CN111578931 B CN 111578931B
Authority
CN
China
Prior art keywords
time domain
estimation
attitude
rolling time
matrix
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
CN202010437604.8A
Other languages
Chinese (zh)
Other versions
CN111578931A (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.)
Naval Aeronautical University
Original Assignee
Naval Aeronautical 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 Naval Aeronautical University filed Critical Naval Aeronautical University
Priority to CN202010437604.8A priority Critical patent/CN111578931B/en
Publication of CN111578931A publication Critical patent/CN111578931A/en
Application granted granted Critical
Publication of CN111578931B publication Critical patent/CN111578931B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

The invention belongs to the technical field of aircraft attitude estimation filtering, and provides an online rolling time domain autonomous attitude estimation method of a gyroscope/triaxial magnetometer, which is suitable for a high-dynamic aircraft, in order to solve the problems of poor real-time performance and difficult online application when the rolling time domain estimation method estimates the attitude. Firstly, establishing a rolling time domain autonomous attitude estimation model based on a gyro/triaxial magnetometer; then, the nominal model predicts a measurement value, and a solution of an ideal rolling time domain estimation problem is calculated according to the predicted measurement value; and finally, after the actual triaxial magnetometer measurement value is obtained, a correction solution of the rolling time domain estimation problem is rapidly calculated by a nonlinear programming sensitivity formula.

Description

High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation
Technical Field
The invention belongs to the technical field of aircraft attitude estimation filtering, and particularly relates to a high-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation.
Background
The global satellite positioning system is an all-weather, global coverage, three-dimensional fixed-speed and timing high-precision navigation positioning system, great achievements are obtained from application, and the development of a reliable autonomous navigation mode without data link communication is a problem to be solved urgently.
The gyroscope is used as an autonomous navigation mode, does not need to transmit and receive signals, has good concealment and high short-term precision, can provide all-weather continuous real-time navigation information, and has become a necessary option of a navigation system. But has the defects of accumulated errors and need of timely correction of other auxiliary information. Other auxiliary autonomous navigation modes also have respective bottlenecks, such as the defects that the sea surface topographic characteristics are not obvious when the visual navigation and the topographic navigation fly across the sea, and the combined filtering is easy to disperse; the gravity field changes extremely slightly, the sensor is difficult to measure and expensive in cost, and gravity navigation is difficult to realize; the starlight navigation is greatly influenced by weather and has lower reliability.
The geomagnetic field is a vector field, is the inherent resource of the earth, has rich variation, and provides a natural coordinate system for navigation, aviation and aerospace. And measuring each signal component of the geomagnetic field under the carrier coordinate system by using a magnetic detection technology, comparing the signal component with a geomagnetic model, and calculating to obtain the space attitude of the aircraft. Among them, a Three-Axis Magnetometer (TAM) is a commonly used vector magnetic measurement sensor. They have attracted much attention and have been widely studied because of their low cost, low power consumption, high reliability, strong resistance to electronic warfare interference, and their resistance to high overload and error accumulation over time.
In consideration of the complementary characteristics of the gyroscope and the magnetometer, the gyroscope providing the angular velocity measurement value and the TAM providing the magnetic field vector observation value can be simultaneously installed on the aircraft to estimate the attitude of the aircraft. The combination of the two sensors has the advantages that: the all-weather and continuous navigation characteristics of pure inertial navigation equipment are maintained; passive measurement is adopted, good concealment is achieved, and exposure and deception and interference of an enemy during communication are avoided; the system has high precision, can provide multi-dimensional high-precision navigation information such as attitude, course and position for the carrier, and has great application potential in the fields of military, security and the like.
A nonlinear attitude estimation system expressed by quaternions can be established through a gyroscope and a model of the TAM. The state estimation Kalman filtering of a linear system can be well solved, but in a nonlinear system, the traditional filtering method based on Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) has the problems that quaternion normalization constraint cannot be considered, undisplayed solution can be obtained, suboptimal results can only be obtained, precision is not high, computation is complex and even filtering divergence is caused when the nonlinear problem is solved. The commonly used method of Moving Horizon Estimation (MHE) solves the Estimation problem by transforming it into an optimization problem described by a cost function, which can explicitly represent the constraints and systematically solve them. These constraints are also generally more reflective of the actual situation and therefore provide more information, reduce the search space of the estimator, improve the search efficiency, and reduce the sensitivity to prior probability density functions and false initializations by including more data in the time domain.
But at the same time it has the drawback that an online solution of the dynamic optimization problem has to be obtained. Up to now, on-line operation of MHE remains a challenge due to the large amount of computation, and this problem also limits its practical application to a large extent, especially for highly dynamic aircraft, whose real-time requirements are difficult to meet. For the problem, the processing method in the model predictive control is to divide the calculation task into a preparation phase and a feedback phase. According to the idea, the invention utilizes the characteristic that the gyro update rate is far higher than that of the TAM to expand the gyro update rate into the MHE problem. Firstly, a measurement value is predicted by utilizing an attitude estimation system model and gyro data, an MHE problem is solved on the basis, and then a solution before the MHE is quickly corrected by using a concept of Nonlinear Programming (NLP) sensitivity after a real TAM measurement value is obtained. By the method, the computing units are divided according to different sensor updating rates, and online waiting time is reduced so as to meet the requirement of a high-dynamic real-time online application scene.
Disclosure of Invention
The invention aims to solve the problems of poor real-time performance and difficult online application when the traditional rolling time domain estimation method estimates the attitude, and provides a high-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation. The main problems solved therein include:
(1) the satellite positioning system has poor concealment, is easy to be interfered by electronic warfare of enemies and cannot realize autonomous operation;
(2) the classical kalman filtering algorithm cannot explicitly solve quaternion normalization constraints;
(3) the traditional rolling time domain estimation method has large calculation amount, is difficult to operate on line and meets the real-time requirement;
(4) the KKT matrix is of a non-diagonal structure, and is large in dimension, large in occupied storage space and large in calculation amount during inversion operation.
The invention relates to a high dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation, which is characterized by comprising the following technical measures:
step one, substituting gyro data at the current moment into a discretized attitude dynamics model, obtaining an attitude quaternion at the next moment, substituting a disturbance-free measurement equation established by a three-axis magnetometer to predict a measurement value of the three-axis magnetometer at the next moment, and defining the predicted measurement value, the known measurement in the past period, the state of a first point of a window and the state covariance of the first point as an extended data vector;
step two, substituting the expansion data vector into a rolling time domain estimation model, solving the model by using a Gauss Newton iteration method so as to obtain a KKT system, and multiplying two sides of an equation of the KKT system by the inverse of a KKT matrix so as to obtain an approximate attitude estimation value;
converting the attitude estimation problem into a nonlinear programming problem by using a rolling time domain estimation model, expressing the nonlinear programming problem into a hidden function about an unknown state according to Lagrange's theorem, and calculating by using the hidden function theorem to obtain a sensitivity matrix;
and step four, bringing the real measurement value, the approximate attitude estimation value and the sensitivity matrix of the triaxial magnetometer into a first-order estimation model of a solution at the next moment, and obtaining the corrected attitude estimation value.
Compared with the prior art, the high dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation has the advantages that:
(1) a rolling time domain autonomous attitude calculation model based on a gyro/triaxial magnetometer is constructed, autonomous operation of the system is realized, and the problem that a communication link is interfered is avoided;
(2) the problem that quaternion normalization constraint cannot be considered in a filtering algorithm under a classic Kalman frame is solved;
(3) the problems of large calculation amount, time delay and difficulty in online operation of the traditional rolling time domain estimation method are solved, and the real-time performance of the method is improved;
(4) after the KKT is converted into a diagonal structure, the data are recorded in a sparse matrix mode, the occupied storage space is small, and the calculation is simple and convenient during inversion.
Drawings
FIG. 1 is a flow chart of an implementation of a high dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation; FIG. 2 is a basic schematic diagram of a rolling time domain estimation method;
FIG. 3 is a KKT matrix structure diagram with a non-diagonal structure;
fig. 4 is a diagram of a KKT matrix having a diagonal configuration.
Detailed Description
In order to solve the problems of poor real-time performance and difficult online application when the rolling time domain estimation method estimates the attitude, the gyro/triaxial magnetometer online rolling time domain autonomous attitude estimation method suitable for the high-dynamic aircraft is provided. Firstly, establishing a rolling time domain autonomous attitude estimation model based on a gyro/triaxial magnetometer; then, the nominal model predicts a measurement value, and the solution of the ideal MHE problem is calculated according to the predicted measurement value; and finally, after obtaining the actual triaxial magnetometer measurement value, quickly calculating the correction solution of the MHE problem by an NLP sensitivity formula.
The invention is described in further detail below with reference to the accompanying figure 1. Referring to the attached figure 1 of the specification, the processing flow of the invention comprises the following steps:
(1) construction of extended data vectors
Establishing state equation
Assume an attitude quaternion vector q of
Figure GDA0003463531530000031
Wherein e ═ ex ey ez]TIs the main axis of rotation and alpha represents the angle of rotation of the reference coordinate system to the carrier system. By this method, quaternions are made to fully and uniquely describe the orientation of the rigid body in the reference coordinate system.
At this time, the attitude matrix a (sometimes referred to as an attitude cosine matrix) can be expressed as a quaternion
Figure GDA0003463531530000041
Wherein the matrix [ q ]1:3×]Multiplying the difference by an antisymmetric matrix; i is3×3Is a 3 x 3 identity matrix.
The quaternion kinetic equation can be expressed as
Figure GDA0003463531530000042
Wherein ω is [ ω ═ ω [ [ ω ]x,ωy,ωz]TThe angular velocity is in the carrier coordinate system. The quaternion kinetic equation does not exhibit singularities at any rotation.
The angular velocity is measured on the carrier by means of a gyroscope. The gyroscope model can be expressed as
ωm(t)＝ω(t)+β(t)+ηv(t)， (4)
Figure GDA0003463531530000043
Wherein, ω ismIs a gyro angular velocity measurement, ηvIs zero mean Gaussian white noise of the gyroscope, and the deviation beta of the gyroscope is modeled as random walk
Figure GDA0003463531530000044
Wherein δ (t) is a Dirac delta function,
Figure GDA0003463531530000045
is the noise variance.
The model must be discretized for computer solution, and equation (3) can be expressed after discretization as
Figure GDA0003463531530000046
Wherein, Δ t is the gyro sampling period. Since ω has three components and q has four, q must satisfy the normalization constraint | | | q | | | 1 to define the relationship between ω and q.
For drift deviation, the forward Euler discretization method is
βk+1＝βk+ηu,k (8)
At this time, equation of state (7) can be abbreviated as
xk+1＝fk+1,k(xk,uk)+wk,k＝0,1,… (9)
Wherein the content of the first and second substances,
Figure GDA0003463531530000051
is the attitude quaternion q, u to be estimatedkIs a known input and the process noise is
Figure GDA0003463531530000052
Process noise variance of Qk。
Establishment of observation equation
The TAM measurement model at each discrete time k is
Bm,k＝A(qk)Hk+vk (10)
Wherein HkIs the earth magnetic field vector under the reference coordinate system; b ism,kFor geomagnetic field measurement in a carrier coordinate systemVector, measurement noise vkWhite Gaussian noise with zero mean and variance of Rk. At this time, the observation equation can be abbreviated as
yk+1＝hk+1(xk+1)+vk+1,k＝0,1,… (11)
Wherein the content of the first and second substances,
Figure GDA0003463531530000053
in order to obtain a measured value,
Figure GDA0003463531530000054
Rkrespectively, the measurement noise and the measurement noise variance.
In this way, a complete gyro/TAM autonomous attitude estimation system model is constructed.
Establishing MHE attitude estimation system with quaternion equality constraint
After the attitude estimation system model is established through the gyroscope and the TAM, how to obtain the current optimal attitude estimation of the aircraft according to the data of the sensor needs to be considered. Under the classic Kalman filtering framework, the nonlinear attitude estimation system has the problems that quaternion normalization constraint cannot be solved explicitly, the precision is low, the calculation is complex, and even the filtering divergence is caused. Besides considering the constraints, compared with the well-known suboptimal nonlinear state estimation, the nonlinear MHE has better performance in terms of accuracy and robustness, so the invention adopts an attitude estimation method based on the MHE framework.
The MHE only considers a fixed amount of data before the current time while approximately describing the effect of historical data on the estimate in a way that the amount of data participating in the optimization each time is constant, which is implemented as a rolling observation "window". This "window" contains past finite metric values, the last output of which is the current state estimate. Let L be the "window" length (i ═ 1,2, …, L), where the last point of the "window" (i ═ L) represents the current time k. For convenience, k is omitted below in the notation, as in state xikI-th point representing the "window" at time k, abbreviated as xi. Since quaternion attitude estimation only includes quaternion normalization equality constraints, the present invention establishes only the MHE including equality constraints, which can be expressed as
Figure GDA0003463531530000061
s.t.
xi+1＝fk+1,k(xi,ui)+wi,for i＝1,…,L-1, (12b)
yi+1＝hk+1(xi+1)+vi+1,for i＝1,…,L-1, (12c)
li(xi)＝0,for i＝1,…,L. (12d)
Wherein the first time point in the time domain is
Figure GDA0003463531530000062
Variance is P1 0. The first term in the cost function (12a) is the arrival cost. It is noted that the measurement value at time i-1 is not included in the cost function J, since it is assumed that all the information obtained from the measurement already includes the prior estimate
Figure GDA0003463531530000063
In (1). The equality constraint is given by equation (12 d). A gyro/TAM-based MHE attitude estimation system model is established.
The data vector defining the MHE problem is
Figure GDA0003463531530000064
(2) Calculation of approximate attitude estimate
The formula (12a) can be written as
Figure GDA0003463531530000065
s.t.
li(xi)＝0,for i＝1,…,L。 (14b)
Wherein, gamma (x)1) And
Figure GDA0003463531530000066
is the corresponding term in (12 a).
Thus, the gyro/TAM based MHE attitude estimation is transformed into an NLP optimization problem. The task is now to find the optimal state sequence when the cost function J is minimized in a window containing L pieces of history information
Figure GDA0003463531530000067
And extracting the current state of the system from the sequence
Figure GDA0003463531530000068
And meanwhile, the state of the current moment is continuously updated through the movement of the window.
At the next moment, when a new measurement y is obtainedk+1By discarding the old measurement ykIncorporation of yk+1To move the measurement vector forward by one sampling time to obtain the measurement vector { y at the next time3,…,yL+1}. In addition, the a priori state estimates are defined by
Figure GDA0003463531530000069
The updating is performed, and the state variance is generally updated by using an updating formula of the EKF. After a new data vector eta (k +1) is obtained, a new MHE problem U can be solvedL(η (k +1)), and the state at the next time is obtained
Figure GDA0003463531530000071
The detailed principle is shown in figure 2.
In the actual operation process, the MHE problem U is caused by the large number of degrees of freedom and the non-convexity caused by the sick equationLThe (η (k +1)) is computationally complex and cannot be directly solved. Cost function U of MHE problem of attitude estimation based on gyro/TAMLOn the basis of (eta), the invention further designs a solving method thereof.
One-step solution method based on Gauss-Newton iteration
Since the state estimation problem is now converted into a nonlinear programming problem, a numerical optimization method can be adopted to solve the problem. Defining parameter p ═ η (·), then problem ULThe Lagrangian function of (p) can be written as
Figure GDA0003463531530000072
Wherein the content of the first and second substances,
Figure GDA0003463531530000073
is the lagrange multiplier vector.
At this time, for the nonlinear optimization problem, we adopt the gauss newton iteration method to calculate the search direction by linearizing the KKT condition along the current trajectory
Figure GDA0003463531530000074
To solve. Finally, a linear KKT system can be obtained
Figure GDA0003463531530000075
The first term on the left side of the above formula is the KKT matrix and is expressed as follows
Figure GDA0003463531530000076
Calculated according to a cost function (12) to obtain
Figure GDA0003463531530000077
Figure GDA0003463531530000078
Figure GDA0003463531530000079
Figure GDA00034635315300000710
Wherein the content of the first and second substances,
Figure GDA0003463531530000081
Figure GDA0003463531530000082
Figure GDA0003463531530000083
Figure GDA0003463531530000084
Figure GDA0003463531530000085
Figure GDA0003463531530000086
Aa＝[-1,0,…,0]， (28)
Figure GDA0003463531530000087
Figure GDA0003463531530000088
finally, the solution to the optimization problem is
Figure GDA0003463531530000089
The solution to the optimization problem only requires a one-time matrix factorization, equation (31). At this time, the solution of the single-step MHE problem is limited to one step, so that the calculation time is limited and predictable, and the preliminary requirement of real-time performance is met.
Diagonal band of KKT matrix
In addition, the matrix H can be seen from the expressions (18), (19), (22), (24), (25), (27), (28), (30)ssinAnd matrix AeqHas a diagonal structure. However, it can be seen from equation (17) that the matrix K that needs to be factorized does not have a diagonal structure, as shown in fig. 3. If, we record the variable from
Figure GDA00034635315300000810
Become into
Figure GDA00034635315300000811
Then the matrix K will appear with a diagonal structure as shown in fig. 4.
The matrix K having a diagonal structure may be recorded using a sparse matrix. Factoring the sparse matrix K of fig. 4 during computer operation is much faster than conventional matrix recording methods. Therefore, the present invention adopts the variable recording method of the formula (33). And storing the KKT matrix by adopting a sparse matrix mode in the MATLAB language when the computer runs.
Thus, the solution of the MHE attitude estimation problem can be obtained by constructing a gauss-newton iteration method with a diagonal structure. However, operating in this manner, it is necessary to wait until all the gyro and TAM information is available before the entire problem can be solved. In fact, due to the fact that the state dimensions are multiple, the solving process is complex, delay is inevitably generated due to huge calculation amount, certain difficulty is brought to online application, and particularly a high-dynamic aircraft cannot meet real-time requirements. Therefore, the online waiting time is further eliminated, and the real-time performance is improved.
(3) Calculation of KKT System sensitivity matrix
In the gyro/TAM autonomous attitude estimation system, the gyro update rate is far greater than that of the TAM, and the update time of the gyro update rate and the TAM update time is not synchronous, so that the invention considers and utilizes the characteristic to divide the attitude estimation calculation into two parts. Firstly, a measurement value is predicted by gyro data and a system equation, an approximate solution is obtained according to the MHE solving method designed above, and then the approximate solution is quickly corrected after a real TAM measurement value is obtained, so that the online waiting time is greatly shortened.
From the function L, equation (15), all the original variables and multipliers become implicit functions with respect to p:
Figure GDA0003463531530000091
wherein the solution vector is defined as
Figure GDA0003463531530000092
And formula (34) has an optimization solution s*(p,L)。
Now study perturbation | p-p0For a given nominal solution s*(p0And L) the influence of the neighborhood. Firstly, the method
Hypothesis (NLP sensitivity hypothesis)
F (-), E (-), Γ (-), are continuously differentiable to the second order for all variables. At the same time, these functions and their differentials are bounded;
·UL(p0) Nominal solution of*(p0L) satisfy linear independent constraints (licaq), Sufficiently Second Order Conditions (SSOC) and strict complementary relaxation conditions.
SSOC requirements at point s*(p0L) calculated Lagrangian functionThe Hessian matrix of the number L is positive in any feasible direction. Due to the existence of this assumption, the solution vector s*(p, L) at nominal solution s*(p0L) is a continuous differentiable function and
Figure GDA0003463531530000101
is bounded and unique.
Then, under the assumption, equation (34) can be derived from the implicit function theorem
Figure GDA0003463531530000102
Wherein, K*(p0L) represents the problem UL(p0) At point s*(p0KKT matrix of L). Also, if the nominal solution satisfies the assumption, the KKT matrix is non-singular and thus can be used to calculate the sensitivity matrix in equation (35)
Figure GDA0003463531530000103
(4) Calculation of corrected attitude estimate
At this point, the solution to the neighborhood problem can be written as
Figure GDA0003463531530000104
The fast first order estimate of the solution can be expressed as
Figure GDA0003463531530000105
Wherein the content of the first and second substances,
Figure GDA0003463531530000106
is s*Approximate solution of (p, L). The MHE problem can be resolved by the formula (37). Because of the slow update rate of the TAM, the measurement value can be predicted by the system equation before the true measurement value of the TAM is not obtainedApproximate solution s is obtained by the above-mentioned established MHE problem solving method based on Gauss Newton iteration*(p0L) and sensitivity matrix
Figure GDA0003463531530000107
Once the TAM measurement is obtained, the data vector is updated and a corrected solution to the MHE problem is found from equation (37). By the method, the solution of the autonomous attitude estimation problem is divided into two parts by taking the acquisition time of the TAM measurement value as a boundary, so that the problem of calculating a huge MHE after all sensor data are acquired is avoided, and the online waiting time is reduced.
As can be seen from the assumption and equation (23),
Figure GDA0003463531530000108
satisfy the requirement of
Figure GDA0003463531530000109
Wherein L issIs a normal norm, | · | is a Euclidean norm. Meanwhile, if the above estimation strategy is adopted, the NLP sensitivity is disturbed as
Figure GDA00034635315300001010
So that the approximation error is
Figure GDA00034635315300001011
Furthermore, since the perturbation is very small, the perturbation approximation error of newton iteration is also infinitesimal of the same order.

Claims (3)

1. The high dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation is characterized by comprising the following steps of:
step one, substituting gyro data at the current moment into a discretized attitude dynamics model, obtaining an attitude quaternion at the next moment, substituting a disturbance-free measurement equation established by a three-axis magnetometer to predict a measurement value of the three-axis magnetometer at the next moment, and defining the predicted measurement value, a known measurement value in a past period of time, a state of a first point of a window and a state covariance of the first point as an extended data vector;
the discretized attitude dynamics model comprises the following steps:
Figure FDA0003463531520000011
wherein, delta t is a gyro sampling period;
the extended data vector is:
Figure FDA0003463531520000012
wherein η (k) is an extended data vector;
Figure FDA0003463531520000013
is the first time point in the time domain; p1 0Is the variance;
step two, substituting the expansion data vector into a rolling time domain estimation model, solving the model by using a Gauss Newton iteration method so as to obtain a KKT system, and multiplying two sides of an equation of the KKT system by the inverse of a KKT matrix so as to obtain an approximate attitude estimation value;
the rolling time domain estimation model is as follows:
Figure FDA0003463531520000014
s.t.
xi+1＝fk+1,k(xi,ui)+wi,for i＝1,…,L-1
yi+1＝hk+1(xi+1)+vi+1,for i＝1,…,L-1
li(xi)＝0,for i＝1,…,L
wherein J is a cost function;
the KKT system is as follows:
Figure FDA0003463531520000015
converting the attitude estimation problem into a nonlinear programming problem by using a rolling time domain estimation model, expressing the nonlinear programming problem into a hidden function about an unknown state according to Lagrange's theorem, and calculating by using the hidden function theorem to obtain a sensitivity matrix;
step four, bringing the real measurement value, the approximate attitude estimation value and the sensitivity matrix of the triaxial magnetometer into a first-order estimation model of a solution at the next moment, and obtaining a corrected attitude estimation value;
the first order estimation model of the solution is:
Figure FDA0003463531520000021
wherein the content of the first and second substances,
Figure FDA0003463531520000022
the corrected attitude estimation value is obtained; s*(p0L) is an approximate attitude estimate;
Figure FDA0003463531520000023
is a sensitivity matrix; p is an extended data vector comprising true measurement values; p is a radical of0Is an extended data vector that includes predicted metrology values.
2. The high-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation according to claim 1, characterized in that in step two, the KKT matrix is recorded in a manner that:
the recording sequence of the variables to be solved in the KKT system is changed into the state variables and the Lagrange multipliers are alternately arranged, i.e.
Figure FDA0003463531520000024
Wherein the content of the first and second substances,
Figure FDA0003463531520000025
represents the ith state variable in the window, i is 1, …, and L represents the length of the window;
Figure FDA0003463531520000026
and representing the ith Lagrange multiplier vector in the window, and then adjusting the recording mode of the KKT matrix according to the recording sequence of the variables.
3. The high-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation according to claim 1, characterized in that in step two, the KKT matrix is recorded in a manner that:
and storing the KKT matrix by adopting a sparse matrix mode in the MATLAB language when the computer runs.
CN202010437604.8A 2020-05-21 2020-05-21 High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation Active CN111578931B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010437604.8A CN111578931B (en) 2020-05-21 2020-05-21 High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010437604.8A CN111578931B (en) 2020-05-21 2020-05-21 High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation

Publications (2)

Publication Number Publication Date
CN111578931A CN111578931A (en) 2020-08-25
CN111578931B true CN111578931B (en) 2022-03-04

Family

ID=72125199

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010437604.8A Active CN111578931B (en) 2020-05-21 2020-05-21 High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation

Country Status (1)

Country Link
CN (1) CN111578931B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113029127B (en) * 2021-03-10 2023-05-09 中国人民解放军海军航空大学 Aircraft autonomous attitude estimation method based on distributed circulating structure
CN114152880B (en) * 2022-02-08 2022-04-12 湖南大学 Soft package battery sensor fault online detection method

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104913781A (en) * 2015-06-04 2015-09-16 南京航空航天大学 Unequal interval federated filter method based on dynamic information distribution
CN108007457A (en) * 2017-11-22 2018-05-08 哈尔滨工业大学 A kind of system monitor and navigation synchronous data fusion method based on subdivision timeslice
CN108762077A (en) * 2018-05-31 2018-11-06 浙江工业大学 A kind of mobile robot moving horizon estimation method with communication constraint
CN109000663A (en) * 2018-08-07 2018-12-14 中国人民解放军海军航空大学 Based on the self-adjusting moving horizon estimation arrival cost calculation method of time-varying
CN109343550A (en) * 2018-10-15 2019-02-15 北京航空航天大学 A kind of estimation method of the spacecraft angular speed based on moving horizon estimation
CN110531626A (en) * 2019-09-20 2019-12-03 河海大学 Magnetic suspension rotor vibration compensation control method and system based on moving horizon estimation

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4928106A (en) * 1988-07-14 1990-05-22 Ashtech Telesis, Inc. Global positioning system receiver with improved radio frequency and digital processing
JP6573978B2 (en) * 2014-08-04 2019-09-11 モデルウェイ・ソチエタ・ア・レスポンサビリタ・リミタータＭｏｄｅｌｗａｙ Ｓ．Ｒ．Ｌ． Method for predicting variables affecting vehicle behavior and corresponding virtual sensors

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104913781A (en) * 2015-06-04 2015-09-16 南京航空航天大学 Unequal interval federated filter method based on dynamic information distribution
CN108007457A (en) * 2017-11-22 2018-05-08 哈尔滨工业大学 A kind of system monitor and navigation synchronous data fusion method based on subdivision timeslice
CN108762077A (en) * 2018-05-31 2018-11-06 浙江工业大学 A kind of mobile robot moving horizon estimation method with communication constraint
CN109000663A (en) * 2018-08-07 2018-12-14 中国人民解放军海军航空大学 Based on the self-adjusting moving horizon estimation arrival cost calculation method of time-varying
CN109343550A (en) * 2018-10-15 2019-02-15 北京航空航天大学 A kind of estimation method of the spacecraft angular speed based on moving horizon estimation
CN110531626A (en) * 2019-09-20 2019-12-03 河海大学 Magnetic suspension rotor vibration compensation control method and system based on moving horizon estimation

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于滚动时域估计的飞行器姿态估计及三轴磁强计在线校正;赵国荣 等;《物理学报》;20151231;第64卷(第21期);第1-9页 *
基于稀疏矩阵QR分解的滚动时域姿态估计方法研究;李茂登 等;《2017中国自动化大会》;20171022;第909-913页 *

Also Published As

Publication number Publication date
CN111578931A (en) 2020-08-25

Similar Documents

Publication Publication Date Title
Cao et al. Predictive smooth variable structure filter for attitude synchronization estimation during satellite formation flying
Rhudy et al. Evaluation of matrix square root operations for UKF within a UAV GPS/INS sensor fusion application
CN108036785A (en) A kind of aircraft position and orientation estimation method based on direct method and inertial navigation fusion
Soken et al. UKF-based reconfigurable attitude parameters estimation and magnetometer calibration
CN109343550B (en) Spacecraft angular velocity estimation method based on rolling time domain estimation
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN111156987A (en) Inertia/astronomical combined navigation method based on residual compensation multi-rate CKF
CN105136145A (en) Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method
CN105300384A (en) Interactive filtering method for satellite attitude determination
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
CN111578931B (en) High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
CN112683274A (en) Unmanned aerial vehicle integrated navigation method and system based on unscented Kalman filtering
CN103884340A (en) Information fusion navigation method for detecting fixed-point soft landing process in deep space
Cilden et al. Nanosatellite attitude estimation from vector measurements using SVD-aided UKF algorithm
CN111190207B (en) PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method
Kottath et al. Window based multiple model adaptive estimation for navigational framework
Zhou et al. Spinning projectile’s angular measurement using crest and trough data of a geomagnetic sensor
CN112325886A (en) Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope
CN116086445A (en) Multi-source information time delay fusion navigation method based on factor graph optimization
Garcia et al. Unscented Kalman filter for spacecraft attitude estimation using quaternions and euler angles
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
Pourtakdoust et al. An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN115655285B (en) Unscented quaternion attitude estimation method for correcting weight and reference quaternion

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