CN112304309B - Method for calculating combined navigation information of hypersonic vehicles based on cardiac array - Google Patents
Method for calculating combined navigation information of hypersonic vehicles based on cardiac array Download PDFInfo
- Publication number
- CN112304309B CN112304309B CN202011130925.XA CN202011130925A CN112304309B CN 112304309 B CN112304309 B CN 112304309B CN 202011130925 A CN202011130925 A CN 202011130925A CN 112304309 B CN112304309 B CN 112304309B
- Authority
- CN
- China
- Prior art keywords
- state
- navigation
- information
- sub
- 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
Links
- 230000000747 cardiac effect Effects 0.000 title claims abstract description 40
- 238000000034 method Methods 0.000 title claims abstract description 40
- 210000003169 central nervous system Anatomy 0.000 claims abstract description 40
- 238000001914 filtration Methods 0.000 claims abstract description 37
- 230000004927 fusion Effects 0.000 claims abstract description 24
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 22
- 239000011159 matrix material Substances 0.000 claims description 79
- 238000005259 measurement Methods 0.000 claims description 20
- 238000012545 processing Methods 0.000 claims description 6
- 230000008030 elimination Effects 0.000 claims description 5
- 238000003379 elimination reaction Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 239000006185 dispersion Substances 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 27
- 230000008569 process Effects 0.000 description 10
- 238000007499 fusion processing Methods 0.000 description 6
- 238000003672 processing method Methods 0.000 description 4
- 238000007792 addition Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000010219 correlation analysis Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 230000006798 recombination Effects 0.000 description 1
- 238000005215 recombination Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/52—Determining velocity
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Astronomy & Astrophysics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a method for resolving combined navigation information of a hypersonic vehicle based on a cardiac array, which respectively acquires system state information output by an INS, speed and position information output by a GNSS (global navigation satellite system) and attitude information output by a CNS (central nervous system); inputting the system state information and the speed and position information into a first local filter for filtering; inputting the system state information and the attitude information into a second local filter for filtering; decomposing the state information of the inertial navigation/satellite integrated navigation subsystem and the state information of the inertial navigation/astronomical integrated navigation subsystem into a plurality of sub-states; performing data fusion on each substate and the system predicted value output by the main filter to obtain final system state information of the HV; the invention adopts a cardiac array structure based on the Faddeeva algorithm to realize the real-time performance of the HV navigation system.
Description
Technical Field
The invention belongs to the technical field of hypersonic aircraft navigation, and particularly relates to a hypersonic aircraft integrated navigation information resolving method based on a cardiac array.
Background
The Hypersonic Vehicle (HV) refers to an aerospace Vehicle with a flight speed greater than 5 times the speed of sound. As HV has the characteristics of extremely wide flight airspace, extremely fast flight speed, extremely strong maneuverability and the like, the HV ensures that the huge military and civil values of HV are widely regarded and deeply researched by the world aerospace strong country and are known as the strategic high point in the future aerospace field. However, as the practical progress of HV is increasing, the speed advantage of HV also puts new demands on the accuracy and real-time performance of the navigation system, and the high-performance navigation technology suitable for HV will become the focus of research in the field of HV in the future.
Currently, HV Navigation usually adopts an INS/GNSS/CNS combined System constructed by an Inertial Navigation System (INS), a Global Navigation Satellite System (GNSS) and an astronomical Navigation System (CNS), which can simultaneously correct an INS velocity error, a position error and an attitude error, and therefore, the System has high precision and strong autonomy, and is considered as an optimal combined Navigation scheme for HV. The INS/GNSS/CNS combined system has been applied in exploration in HV, such as hypersonic tip leading edge flight test SHEFEX-2 carried out in 2012 in Germany, and the pilot aircraft adopts the INS/GNSS/CNS combined navigation system, so that the usability of the system is verified; in the subsequent hypersonic repeatable flight experiment ReFEx, the test aircraft still adopts the combined navigation mode.
For HV navigation, it is only possible to provide navigation information with high precision to ensure that the HV performs the intended task on a safe fly. However, there is a contradiction between the real-time performance and the precision of the navigation information calculation, and the higher the calculation precision of the navigation algorithm is, the greater the calculation complexity is, and the worse the real-time performance is. For a combined INS/GNSS/CNS system suitable for HV, a federal filtering structure is typically used to integrate the INS, GNSS and CNS output heterogeneous navigation information. However, in the local filter and the main filter of the federal filtering, a large number of matrix operations related to the system state are involved, resulting in high computational complexity; particularly, due to the fact that the INS/GNSS/CNS combined system is high in state dimensionality, the calculation amount is increased sharply, the requirement of the HV for high real-time navigation calculation is difficult to meet, and further the overall performance of the HV navigation system is further improved by elbow control.
Disclosure of Invention
The invention aims to provide a method for solving combined navigation information of a hypersonic vehicle based on a cardiac array, so as to solve the problem of poor real-time performance in the navigation information solving process.
The invention adopts the following technical scheme: a method for solving combined navigation information of a hypersonic vehicle based on a cardiac array comprises the following steps:
respectively acquiring system state information output by an INS, speed and position information output by a GNSS (global navigation satellite system) and attitude information output by a CNS (central nervous system);
inputting system state information, speed and position information into a first local filter for filtering to obtain state information of the inertial navigation/satellite combined navigation subsystem and sending the state information to a main filter;
inputting the system state information and the attitude information into a second local filter for filtering to obtain state information of the inertial navigation/astronomical combined navigation subsystem and sending the state information to a main filter;
decomposing the state information of the inertial navigation/satellite integrated navigation subsystem and the state information of the inertial navigation/astronomical integrated navigation subsystem into a plurality of sub-states;
performing data fusion on each sub-state and the state predicted value output by the main filter to obtain the final system state information of the HV;
matrix operation in the first local filter, the second local filter and the main filter is realized by adopting a cardiac array.
Further, the measurement equations of the first local filter and the second local filter are:
z i (k)＝H i (k)x(k)+v i (k)(i＝1,2)，
wherein z is i (k) Is the measurement vector of the ith local filter at time k, H i (k) For the measurement matrix, x (k) is the discrete system state vector, v i (k) For measuring noise, the variance is R i (k)(i＝1,2)；
The filtering method of the first local filter and the second local filter comprises the following steps:
wherein,
the local state estimate obtained for the ith local filter at time k,
the predicted value of the ith local filter at the kth time,
f (k) is a state transition matrix after the k-th time dispersion,
the local state estimate obtained for the ith local filter at time (k-1),
is composed of
Error covariance matrix of (1), I n Is an identity matrix of order n,
is an error covariance matrix of predicted values of the ith local filter at the kth time,
is composed of
Error covariance matrix of (2), Q i (k) For the ith local filterOf the system noise variance, R i (k) The noise variance on both sides of the ith local filter at time k.
Further, the data fusion of each sub-state with the state prediction value output by the main filter includes:
decomposing the state prediction value output by the main filter into sub-state prediction values corresponding to the sub-states according to each sub-state;
fusing each sub-state and the sub-part state predicted value to obtain a sub-state fusion solution;
and combining the obtained sub-state fusion solutions to obtain the final system state information of the HV.
Further, the sub-states are attitude errors, velocity position errors, or constant errors of the gyro accelerometer.
Further, fusing each sub-state and the sub-state prediction value comprises:
wherein,
for the fusion solution of the jth sub-state at time k,
is composed of
Corresponding error covariance matrix.
Further, combining the obtained sub-state fusion solutions includes:
resetting the state estimates of the main filter and the local filter using the fused state information and correcting the INS error
Further, matrix operations in the first local filter, the second local filter and the main filter are all realized by adopting a cardiac array structure based on a Faddeev algorithm:
wherein the cardiac array structure comprises a plurality of interconnected processing units;
the Faddeev algorithm includes:
combining the four input matrixes A, B, C and D into a new matrix M,
the matrix M is transformed such that A becomes an upper triangular matrix, -C becomes a zero matrix and the lower triangular elements of D are not eliminated, i.e.
Wherein, D + CA -1 B is an output matrix, and matrix transformation is realized by a Gaussian elimination method.
The invention has the beneficial effects that: the invention adopts a cardiac array structure based on Faddeeva algorithm, which can improve the calculation speed of matrix operation related to federal filtering; meanwhile, the main filter for combined navigation federal filtering is improved, and a state quantity classification processing method is adopted, so that the dimension of the input state of the main filter is reduced, and the calculation complexity of the data fusion process of the main filter is further reduced; thereby realizing the real-time performance of the HV navigation system.
Drawings
FIG. 1 is a schematic flow chart of a combined navigation information resolving method of a hypersonic vehicle based on a cardiac array;
FIG. 2 is a structural diagram of a cardiac array implementation of the Faddeeva algorithm in an embodiment of the present invention;
FIG. 3 is a graph comparing the horizontal position error of the INS/GNSS/CNS integrated navigation method according to the present invention;
FIG. 4 is a graph comparing the filtering time and the data fusion time of the INS/GNSS/CNS integrated navigation method according to the embodiment of the present invention.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
The invention discloses a method for resolving combined navigation information of a hypersonic vehicle based on a cardiac array, which comprises the following steps of:
respectively acquiring system state information output by an INS, speed and position information output by a GNSS (global navigation satellite system) and attitude information output by a CNS (central nervous system); inputting system state information output by the INS and speed and position information output by the GNSS into a first local filter for filtering to obtain state information of the inertial navigation/satellite combined navigation subsystem and sending the state information to a main filter; inputting the system state information output by the INS and the attitude information output by the CNS into a second local filter for filtering to obtain the state information of the inertial navigation/astronomical combined navigation subsystem and sending the state information to a main filter; decomposing the state information of the inertial navigation/satellite integrated navigation subsystem and the state information of the inertial navigation/astronomical integrated navigation subsystem into a plurality of sub-states; performing data fusion on each sub-state and the state prediction value output by the main filter respectively to obtain final system state information of the HV; matrix operation in the first local filter, the second local filter and the main filter is realized by adopting a cardiac array.
The invention adopts a cardiac array structure based on Faddeeva algorithm, which can improve the calculation speed of matrix operation related to federal filtering; meanwhile, the main filter of the combined navigation federal filtering is improved, and a state quantity classification processing method is adopted, so that the dimension of the input state of the main filter is reduced, and the calculation complexity of the data fusion process of the main filter is further reduced; thereby improving the real-time performance of the HV navigation system.
In an embodiment of the invention, the filter is divided into a local filter and a main filter, and the local filter estimates the state of the navigation subsystem. In this embodiment, two navigation subsystems are included, namely an INS/GNSS navigation subsystem and an INS/CNS navigation subsystem.
Firstly, an INS/GNSS/CNS integrated navigation system state equation is established according to an error equation of the INS navigation system.
Selecting an east-north-sky geographic coordinate system (g system) as a navigation coordinate system (n system), recording an inertia coordinate system as an i system, an earth coordinate system as an e system, recording a carrier coordinate system as a b system, and calculating a navigation coordinate system as an n' system. The INS attitude error equation and the velocity error equation are:
wherein,
in order to be an attitude error,
is composed of
In the calculation of the projection of the navigational coordinate system,
is the angular velocity of the rotation of the earth,
angular velocity for the motion of a hypersonic vehicle, phi = (phi) E ,φ N ,φ U ) T For attitude angle errors of Hypersonic Vehicles (HV),
is composed of
The error in the calculation of (a) is,
in order to be the attitude transformation matrix,
in order to measure the error of the gyroscope,
in order to continue the speed error,
for specific force output of accelerometer, δ f b In order to measure the error of the accelerometer,
is composed of
In the calculation of the projection of the navigation coordinate system,
is composed of
In the calculation of the projection of the navigation coordinate system, δ v n ＝(δv E ,δv N ,δv U ) T For speed errors of a hyper aircraft (HV),
is composed of
The error in the calculation of (a) is,
is composed of
The calculation error of (2).
The position error equation for INS is:
wherein δ p = (δ L, δ λ, δ h) is a position error of HV,
and
the east and north velocities of HV;
and
as latitude and altitude of HV, δ L and δ h as latitude error and longitude error; r M And R N The main curvature radius of the earth meridian and prime unit circle; delta v N 、δv E 、δv U Representing the velocity errors in the north, east and sky directions, respectively.
Typically, there is a constant drift ε corresponding to the gyroscope and accelerometer b Biased from a constant ^ v b Random constants can be used to describe, namely:
defining the system state:
x(t)＝[φ,δv n ,δp,ε b ,▽ b ] T (5)
according to the defined system state, combining the equations (1) to (4), the state equation of the combined navigation system of the hypersonic vehicle can be established:
wherein F (t) is a system state transition matrix,
in order to be the system noise vector,
and
in order to be the attitude transformation matrix,
representing the noise of the accelerometer and gyroscope, respectively.
Secondly, establishing a measurement equation of the INS/GNSS navigation subsystem of the hypersonic vehicle (namely the measurement equation of the first local filter):
taking the difference between the speed and the position output by the GNSS and the INS as the measurement, the measurement equation of the INS/GNSS subsystem can be expressed as:
wherein z is 1 (k) Is the measurement vector of the first local filter at the kth time, x (k) is the discretized system state, v 1 (k) In order to measure the noise, the noise measurement device is provided with a noise measurement circuit,
v v (k) And v p (k) For measuring noise, corresponding to the speed error and the position error of the GNSS receiver, H v ＝[0 3×3 ,diag(1,1,1),0 3×9 ]。
Thirdly, establishing a measurement equation of the INS/CNS navigation subsystem of the hypersonic vehicle:
taking the difference between the postures output by the CNS and the INS as a measure, the measurement equation of the INS/CNS subsystem can be expressed as:
z 2 (k)＝H 2 (k)x(k)+v 2 (k) (8)
wherein z is 2 (k) Is the measurement vector of the second local filter at the k-th time, H 2 (k)＝[I 3×3 ,0 3×12 ]；v 2 (k) For measuring noise, corresponding to the measurement error of the star sensor, I 3×3 Is a 3 x 3 order identity matrix.
In the embodiment of the invention, a federated filtering structure with dimension reduction and classification features is designed, as shown in figure 1. In the first layer of the designed Federal filtering structure, local state estimation values of an INS/GNSS and an INS/CNS navigation subsystem are respectively obtained in a parallel mode by adopting a Kalman filtering algorithm. The method comprises the following specific steps:
discretizing the state equation (6) of the combined navigation system of the hypersonic vehicle to obtain a discrete system state equation
x(k)＝F(k)x(k-1)+w(k) (9)
In the formula, x (k) is a system state, F (k) is a state transition matrix after discretization, w (k) is system noise, and the variance is Q (k).
The measurement equations for the INS/GNSS and INS/CNS navigation subsystems can be expressed as:
z i (k)＝H i (k)x(k)+v i (k)(i＝1,2) (10)
wherein,
is the measurement vector of the ith local filter; h i (k) Is a measurement matrix; v. of i (k) For measuring noise, the variance is R i (k)(i＝1,2)。
And respectively acquiring local state estimation values of the INS/GNSS and the INS/CNS navigation subsystems by adopting Kalman filtering. To remove the correlation between the local filter solutions, an upper bound of variance technique is applied to the system noise variance and the state estimation error covariance, i.e.
Q i (k)＝β i -1 Q(k) (11)
In the formula beta i (i =1, 2) assigns a factor to the information.
Specifically, the filtering method of the first local filter and the second local filter is as follows:
wherein,
is the local state estimate available at the ith local filter at the kth,
the predicted value of the ith local filter at the kth time,
f (k) is a state transition matrix after the k-th time dispersion,
the local state estimate obtained for the ith local filter at time (k-1),
is composed of
The error covariance matrix of (a) is obtained,
is an error covariance matrix of predicted values of the ith local filter at the kth time,
is composed of
Error covariance matrix of (2), Q i (k) System noise variance, R, for the ith local filter i (k) The measured noise variance of the ith local filter at time k.
In a federal filtering structure of an INS/GNSS/CNS integrated navigation system, filtering navigation information of an INS/GNSS subsystem and an INS/CNS subsystem by using a Kalman filtering algorithm (KF), and acquiring local state estimation values of the navigation subsystems in a parallel mode; in the Kalman filtering calculation process, a corresponding matrix operation is carried out by adopting a cardiac array structure based on a Faddeev algorithm.
In the last step, the obtained local state estimation values of the INS/GNSS subsystem and the INS/CNS subsystem are respectively input into a main filter of federal filtering, and the system state vector is decomposed into three sub-states according to the correlation analysis result among the state components of the navigation subsystem; for each sub-state, respectively carrying out data fusion on the state estimation output by each navigation subsystem and the output of the main filter to obtain a data fusion result of each sub-state estimation value; and recombining the obtained sub-state data fusion solution to obtain the global estimation of the system state, resetting the state estimation values of the main filter and the local filter by adopting the fused state information, and correcting the error of the INS.
After parallel computation of the two local filters is completed, a local state estimate can be obtained
And error covariance matrix thereof
And performing data fusion on the obtained local state estimation value and the time updating solution output by the main filter to obtain the global estimation of the system state. It should be noted that, in the kalman filtering process, a cardiac array structure based on the Faddeev algorithm is adopted to process the relevant matrix operation.
In the INS/GNSS and INS/CNS subsystems, the estimation of attitude error and the estimation of speed and position error have very weak mutual influence. Thus, as shown in FIG. 2, the attitude error and the velocity and position errors in the system state vector are first decomposed into two different sub-states. In engineering applications, gyro constant drift and accelerometer zero offset are sometimes not included in the state vector of the integrated navigation system for the purpose of pursuing real-time performance. Thus, gyro constant drift and accelerometer zero bias are listed as a third sub-state for user selection.
Rewriting a system state vector as
In the formula, x (1) ＝(φ E ,φ N ,φ U ) T ，x (2) ＝(δv E ,δv N ,δv U ,δL,δλ,δh) T ，x (3) ＝(ε x ,ε y ,ε z ,▽ x ,▽ y ,▽ z ) T 。
The state estimate obtained by each local filter can then be expressed as
Note book
Error covariance matrix of
In the formula,
to estimate corresponding to local state
Error covariance matrix of (2).
At this time, local state estimation
And error covariance matrix thereof
Is broken down into three sub-states, i.e.
And
similarly, the time-updated solution (i.e., the predicted value) of the main filter
And error covariance matrix thereof
Can also be decomposed into
And
after decomposing the system state vector, performing data fusion on the local state estimation values output by the INS/GNSS and INS/CNS navigation subsystems and the sub-local state estimation values output by the main filter time updating solution to obtain a fusion solution of the local state estimation values, and further obtaining the global estimation of the INS/GNSS/CNS integrated navigation system state.
The fusing each sub-state and the sub-state prediction value comprises:
wherein,
for the fusion solution for the jth sub-state at time k,
is composed of
Corresponding error covariance matrix.
Based on the above, fusion solution for sub-state estimation
And
by recombination, a global estimate of the system state is obtained
Namely:
due to the fact that
The error covariance matrix of (c) can be expressed as
And
substituting the formula (19) into the formula (22),
can be easily obtained.
Finally, the system state estimates output by the local filter and the main filter are reset by global state estimation:
at the same time, the errors of the INS are corrected.
In the data fusion process, the calculation efficiency of corresponding matrix operation is improved by adopting the cardiac array structure. The cardiac array implementation structure of the Faddeeva algorithm designed by the embodiment of the invention can improve the operation efficiency of matrix operation related to navigation calculation.
A cardiac array implementation structure of the Faddeeva algorithm is designed, and a powerful tool is provided for improving the matrix operation efficiency involved in navigation calculation.
The Faddeev algorithm implements matrix operations by combining four input matrices into a new matrix and then performing gaussian elimination on the matrix. The algorithm is as follows
Suppose A, B, C, D are four matrices, where A is a non-singular matrix. Let matrix M be:
the Faddeev algorithm is implemented by transforming the matrix M such that A becomes an upper triangular matrix, C becomes a zero matrix, and the lower triangular elements of D are not eliminated, i.e., D
Wherein, D + CA -1 B is output matrix, and the matrix transformation can utilize highAnd (4) realizing the method of the elimination. Thus, by appropriate selection of a, B, C, and D, matrix operations such as addition (subtraction), multiplication, inversion, etc. can be solved using the faddev algorithm. For example: if D is 0 matrix, B and C are identity matrix, then D + CA -1 B＝A -1 And not only the matrix inversion operation is realized.
In the process of carrying out Gaussian elimination on the matrix M, the calculation is carried out by adopting a cardiac array structure, so that the calculation speed of the algorithm is improved. The cardiac array is composed of a group of simple and repeated Processing units (PE), input and output data are connected with the boundary PE, and the boundary PE is connected with the internal PE to form a super large scale integrated circuit for calculation. The greatest advantage of the cardiac array is to increase the computation speed, for example, if the conventional structure uses one PE, the computation speed is 500 ten thousand times/s, and if the cardiac array is composed of n PEs with the same clock frequency, the computation speed is 500 × n ten thousand times/s. The computation of matrices, in particular the multiplication of matrices, is particularly suitable for the computation of cardiac arrays. Based on such characteristics, a cardiac array implementation scheme as shown in fig. 1 is designed to improve the calculation speed of the Faddeev algorithm.
Fig. 2 is a cardiac array implementation of the Faddeeva algorithm designed in this invention, which includes p interconnected PE elements. The matrix multiplication is taken as an example for explanation: assuming that the matrix A is of an n × p order, the matrix B is of a p × m order, and the matrices A and B are multiplied to obtain an n × m order matrix C. The number of multiplications and the number of additions involved in the above matrix multiplication are n × p × m. The designed cardiac array implementation unit comprises p PEs, the multiplication operation required to be processed by each PE unit is only n x m times, and the addition operation is also n x m times, so that the matrix multiplication efficiency is improved by p times.
Verification of the examples:
taking an INS/GNSS/CNS integrated navigation system as an example, the performance of the method provided by the invention is evaluated through computer simulation. In a simulation experiment, the initial position of the hypersonic aircraft is set to 106.022 east longitude, 34.246 north latitude and 45km height; initial speeds in the east, north and sky directions are 150m/s,2100m/s and 0m/s; the accelerometer has a zero offset and white noise of 10 respectively -3 g and
the constant drift of the gyro and the white noise are 0.1 degree/h and
the data update rate of the gyroscope and the accelerometer is 100Hz; the GNSS horizontal positioning precision is 5m, the height error is 8m, the speed error is 0.05m/s, and the data updating rate is 20Hz; the attitude error of the CNS is 5' in root mean square and the data update rate is 10Hz. The filtering periods of the INS/GNSS and INS/CNS navigation subsystems are respectively 0.05s and 0.1s, the data fusion period of the main filter is 0.1s, and the simulation time is 1000s.
In a simulation experiment, a cardiac array structure containing 2 PEs is adopted to process matrix operation involved in a navigation information resolving process.
Defining the horizontal position error of the INS/GNSS/CNS integrated navigation system of the hypersonic vehicle as follows:
in the formula, L and delta L are latitude and latitude estimation errors of the hypersonic vehicle, and delta lambda is longitude estimation error of the hypersonic vehicle.
FIG. 3 shows the calculated altitude error of the hypersonic vehicle calculated according to the Federal Filter and the proposed navigation information solution method. It can be seen that the positioning accuracy of the method of the invention is similar to that of the federal filtering, and almost no accuracy loss exists. The main reasons are that: 1) The cardiac array implementation structure designed by the invention has no precision loss when matrix operation is carried out; 2) Although the Federal filtering structure with dimension reduction and classification characteristics decomposes and reconstructs the system state in the main filter, the mutual influence of attitude error estimation, speed error estimation and position error estimation in INS/GNSS and INS/CNS subsystems is very weak, and the precision loss of the decomposition and reconstruction process is very small.
FIG. 4 shows the calculation time of the navigation solution by using the federal filtering and the proposed navigation information solution method, wherein FIG. 4 (a) is a comparison graph of the time consumed by the filtering solution of the navigation subsystems (INS/GNSS and INS/CNS systems); fig. 4 (b) is a comparison graph of the computation time consumption of the main filter data fusion process. It can be seen that because a cardiac array structure is adopted to perform relevant matrix operation, and the dimension of the input state of the main filter is reduced according to a state quantity classification processing method, the calculation time consumption of the proposed navigation information resolving method in the filtering process and the data fusion process is respectively reduced by 54.7% and 78.5% compared with that of federal filtering, the calculation efficiency of federal filtering is remarkably improved, and the real-time performance of the INS/GNSS/CNS combined navigation system of the hypersonic vehicle is improved.
The invention provides a rapid solving method of HV combined navigation information by taking an INS/GNSS/CNS combined system as an object, so as to solve the contradiction between accuracy and instantaneity in the HV navigation solving process; meanwhile, the main filter of the combined navigation federal filtering is improved, and a state quantity classification processing method is adopted, so that the dimension of the input state of the main filter is reduced, and the calculation complexity of the data fusion process of the main filter is further reduced; the invention can effectively improve the real-time performance of the HV navigation system.
Claims (6)
1. A method for solving combined navigation information of a hypersonic vehicle based on a cardiac array is characterized by comprising the following steps:
respectively acquiring system state information output by an INS, speed and position information output by a GNSS (global navigation satellite system) and attitude information output by a CNS (central nervous system);
inputting the system state information and the speed and position information into a first local filter for filtering to obtain state information of the inertial navigation/satellite combined navigation subsystem and sending the state information to a main filter;
inputting the system state information and the attitude information into a second local filter for filtering to obtain state information of the inertial navigation/astronomical combined navigation subsystem and sending the state information to a main filter;
decomposing the state information of the inertial navigation/satellite integrated navigation subsystem and the state information of the inertial navigation/astronomical integrated navigation subsystem into a plurality of sub-states;
performing data fusion on each substate and a state predicted value output by a main filter to obtain final system state information of the HV;
matrix operation in the first local filter, the second local filter and the main filter is realized by adopting a cardiac array structure based on a Faddeev algorithm;
wherein the cardiac array structure comprises a plurality of interconnected processing units; the input and output data are connected with the boundary processing unit, and the boundary processing unit is connected with the internal processing unit;
the Faddeev algorithm includes:
combining four input matrixes of A, B, C and D into a new matrix M,
the matrix M is transformed such that A becomes an upper triangular matrix, -C becomes a zero matrix and the lower triangular elements of D are not eliminated, i.e.
Wherein, D + CA -1 B is an output matrix, and matrix transformation is realized by a Gaussian elimination method.
2. The integrated hyper-navigation information solution method based on the cardiac array as set forth in claim 1, wherein the measurement equations of the first local filter and the second local filter are:
z i (k)＝H i (k)x(k)+v i (k)，i＝1,2，
wherein z is i (k) Is the measurement vector of the ith local filter at the kth time, H i (k) For the measurement matrix, x (k) is the discrete system state vector, v i (k) To measure noise;
the filtering method of the first local filter and the second local filter comprises the following steps:
wherein,
the local state estimate obtained for the ith local filter at time k,
the predicted value of the ith local filter at the kth time,
f (k) is a state transition matrix after the k-th moment dispersion,
the local state estimate obtained for the ith local filter at time (k-1),
is composed of
Error covariance matrix of (I) n Is an identity matrix of order n,
is an error covariance matrix of the predicted values of the ith local filter at time k,
is composed of
Error covariance matrix of (2), Q i (k) System noise variance, R, for the ith local filter i (k) The measured noise variance of the ith local filter at time k.
3. The method for solving the combined navigation information of the hyper-aircraft based on the cardiac array as claimed in claim 2, wherein the data fusion of each sub-state with the state prediction value output by the main filter comprises:
decomposing the state prediction value output by the main filter into sub-state prediction values corresponding to the sub-states according to each sub-state;
fusing each sub-state with the sub-state prediction value to obtain a sub-state fusion solution;
and combining the obtained sub-state fusion solutions to obtain the final system state information of the HV.
4. The method for solving the combined navigation information of the hypersonic aircraft based on the cardiac array as claimed in claim 3, wherein the sub-state is an attitude error, a speed position error or a constant error of a gyro accelerometer.
5. The cardiac array-based combined hyper-aircraft navigation information solution method according to claim 4, wherein fusing each of the sub-states and the sub-state prediction values comprises:
wherein,
for the fusion solution for the jth sub-state at time k,
is composed of
Corresponding error covariance matrix.
6. The method for solving the combined navigation information of the hyper-aircraft based on the cardiac array as claimed in claim 5, wherein the combination of the obtained sub-state fusion solutions comprises:
and resetting the state estimation values of the main filter and the local filter by using the fused state information, and correcting the error of the INS.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011130925.XA CN112304309B (en) | 2020-10-21 | 2020-10-21 | Method for calculating combined navigation information of hypersonic vehicles based on cardiac array |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011130925.XA CN112304309B (en) | 2020-10-21 | 2020-10-21 | Method for calculating combined navigation information of hypersonic vehicles based on cardiac array |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112304309A CN112304309A (en) | 2021-02-02 |
| CN112304309B true CN112304309B (en) | 2022-10-04 |
Family
ID=74328604
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011130925.XA Active CN112304309B (en) | 2020-10-21 | 2020-10-21 | Method for calculating combined navigation information of hypersonic vehicles based on cardiac array |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112304309B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113776527B (en) * | 2021-09-13 | 2023-04-07 | 中国民用航空飞行学院 | Integrated navigation system and navigation method for civil aircraft full time and space |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4823299A (en) * | 1987-04-01 | 1989-04-18 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Systolic VLSI array for implementing the Kalman filter algorithm |
| ITMI981280A1 (en) * | 1998-06-05 | 1999-12-05 | Italtel Spa | RAPID CONVERGENCE SPACE AND TEMPORAL RQUALIZATION METHOD FOR THE CANCELLATION OF STATIONARY AND NON-STATIONARY ISOFREQUENTIAL INTERFERENTS |
| CN101178312A (en) * | 2007-12-12 | 2008-05-14 | 南京航空航天大学 | Spacecraft shading device combined navigation methods based on multi-information amalgamation |
| WO2019018315A1 (en) * | 2017-07-17 | 2019-01-24 | Kaarta, Inc. | Aligning measured signal data with slam localization data and uses thereof |
| CN110428376A (en) * | 2019-07-24 | 2019-11-08 | 桂林理工大学 | Geometric correction method on a kind of line array CCD satellite image star based on FPGA |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5574650A (en) * | 1993-03-23 | 1996-11-12 | Litton Systems, Inc. | Method and apparatus for calibrating the gyros of a strapdown inertial navigation system |
| CN102519470B (en) * | 2011-12-09 | 2014-05-07 | 南京航空航天大学 | Multi-level embedded integrated navigation system and navigation method |
| CN103697894B (en) * | 2013-12-27 | 2016-05-25 | 南京航空航天大学 | Multi-source information unequal interval federated filter method based on the correction of wave filter variance battle array |
| CN107036598A (en) * | 2017-03-30 | 2017-08-11 | 南京航空航天大学 | Dual quaterion inertia/celestial combined navigation method based on gyro error amendment |
| CN109737959A (en) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter |
| CN111473786A (en) * | 2020-04-28 | 2020-07-31 | 烟台南山学院 | Two-layer distributed multi-sensor combined navigation filtering method based on local feedback |
-
2020
- 2020-10-21 CN CN202011130925.XA patent/CN112304309B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4823299A (en) * | 1987-04-01 | 1989-04-18 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Systolic VLSI array for implementing the Kalman filter algorithm |
| ITMI981280A1 (en) * | 1998-06-05 | 1999-12-05 | Italtel Spa | RAPID CONVERGENCE SPACE AND TEMPORAL RQUALIZATION METHOD FOR THE CANCELLATION OF STATIONARY AND NON-STATIONARY ISOFREQUENTIAL INTERFERENTS |
| CN101178312A (en) * | 2007-12-12 | 2008-05-14 | 南京航空航天大学 | Spacecraft shading device combined navigation methods based on multi-information amalgamation |
| WO2019018315A1 (en) * | 2017-07-17 | 2019-01-24 | Kaarta, Inc. | Aligning measured signal data with slam localization data and uses thereof |
| CN110428376A (en) * | 2019-07-24 | 2019-11-08 | 桂林理工大学 | Geometric correction method on a kind of line array CCD satellite image star based on FPGA |
Non-Patent Citations (3)
| Title |
|---|
| 一种高超声速飞行器组合导航新方法;李海林等;《现代防御技术》;20121231(第06期);全文 * |
| 多传感器组合导航系统评述;衣晓等;《火力与指挥控制》;20030830(第04期);全文 * |
| 脉动阵列执行Kalman滤波计算;慕德俊等;《控制理论与应用》;19931031(第05期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112304309A (en) | 2021-02-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN105737823B (en) | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF | |
| Bingbing et al. | Mahalanobis distance-based fading cubature Kalman filter with augmented mechanism for hypersonic vehicle INS/CNS autonomous integration | |
| CN111238467B (en) | Bionic polarized light assisted unmanned combat aircraft autonomous navigation method | |
| CN110006427B (en) | BDS/INS tightly-combined navigation method in low-dynamic high-vibration environment | |
| CN102353378B (en) | Adaptive federal filtering method of vector-form information distribution coefficients | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN104655131A (en) | Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) | |
| CN109507706B (en) | GPS signal loss prediction positioning method | |
| CN106597507A (en) | High-precision rapid filtering and smoothing algorithm of GNSS/SINS tight combination | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| Raol et al. | On the orbit determination problem | |
| CN107036598A (en) | Dual quaterion inertia/celestial combined navigation method based on gyro error amendment | |
| CN103542853B (en) | The absolute Navigation method of a kind of estimated acceleration meter drift | |
| CN110243377A (en) | A kind of cluster aircraft collaborative navigation method based on hierachical structure | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| Chiang et al. | Constrained filtering method for attitude determination using GPS and gyro | |
| CN113916226B (en) | Minimum variance-based interference rejection filtering method for integrated navigation system | |
| CN112304309B (en) | Method for calculating combined navigation information of hypersonic vehicles based on cardiac array | |
| Chang et al. | Pseudo open-loop unscented quaternion estimator for attitude estimation | |
| Chen et al. | Multi-geomagnetic-component assisted localization algorithm for hypersonic vehicles | |
| Sanyal | Optimal attitude estimation and filtering without using local coordinates part i: Uncontrolled and deterministic attitude dynamics | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN105180928B (en) | A kind of boat-carrying star sensor localization method based on inertial system gravity characteristic | |
| Qian et al. | Novel integration strategy for GNSS-aided inertial integrated navigation |
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 |