CN112525188B - Combined navigation method based on federal filtering - Google Patents
Combined navigation method based on federal filtering Download PDFInfo
- Publication number
- CN112525188B CN112525188B CN202011473337.6A CN202011473337A CN112525188B CN 112525188 B CN112525188 B CN 112525188B CN 202011473337 A CN202011473337 A CN 202011473337A CN 112525188 B CN112525188 B CN 112525188B
- Authority
- CN
- China
- Prior art keywords
- error
- filtering
- follows
- matrix
- navigation
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
The invention discloses a combined navigation method based on federal filtering, which comprises the following steps of 1: analyzing error mechanisms and modeling errors of the sensors and the navigation equipment; step 2: establishing a state space equation of each sub-filtering system; and step 3: a filtering effectiveness evaluation mechanism; and 4, step 4: an adaptive federal filter design. The invention introduces observability and stability as an effectiveness evaluation mechanism of each subsystem, designs the federal filtering distribution factor based on observability and stability, can enhance the fault tolerance of the federal filtering, ensures that the proportion of good observability and stability is larger, and greatly improves the filtering precision.
Description
Technical Field
The invention relates to the field of navigation, in particular to a combined navigation method based on federal filtering.
Background
ARJ21-700 airplane current configuration and function can not meet the performance requirement of RNP AR (required navigation performance required for authorization) commonly used in high altitude airports, and Flight Management System (FMS) is in the core position in RNP AR, which realizes most of RNP AR functions and performances.
Therefore, in order to realize the high altitude RNP AR function, the existing functions and algorithms of the FMS need to be optimized and designed according to the RNP AR requirements, and particularly, the FMS key combined navigation algorithm needs to decompose the precision indexes, study various navigation sensor mode switching and fusion algorithms according to RNP AR 0.3 and fly-by (MA)0.3, and perform software modeling and simulation work on the algorithms.
Meanwhile, the problem of 'GPS unavailability' in the operation of ARJ21-700 airplane airline will also affect the RNP AR function, and the combined navigation optimization design scheme meeting the RNP AR precision index requirement is provided based on the influence of the GPS problem on the combined navigation algorithm of the existing flight management system.
Disclosure of Invention
Aiming at the requirements of ARJ21-700 airplanes on high precision and high reliability of comprehensive navigation performance under the RNP AR operation condition and thoroughly solving the problem of 'GPS unavailability' in airplane airline operation, the invention provides a combined navigation method based on the Federal filtering method to perform information fusion on a plurality of sensors and navigation equipment to achieve the navigation requirement of high performance based on inertial sensors (accelerometers, gyroscopes and magnetometers), global satellite navigation systems (GPS/BDS), atmospheric data systems, land-based radio navigation systems, instrument landing systems and the like.
In order to achieve the above purpose, the technical solution for solving the technical problem is as follows:
a combined navigation method based on federal filtering comprises the following steps:
step 1: analyzing error mechanisms and modeling errors of the sensor and the navigation equipment;
step 2: establishing a state space equation of each sub-filtering system;
and step 3: a filtering effectiveness evaluation mechanism;
and 4, step 4: an adaptive federal filter design.
Further, step 1 comprises the following steps:
1.1, analyzing and modeling errors of a strapdown inertial navigation sensor, wherein the attitude errors are as follows:
wherein the content of the first and second substances,
respectively the rate of change of attitude error angle in x, y and z directions, V N Is the north velocity, V E East speed, Δ V N Is a V N Speed change of delta V E Is a V E H is the geographical altitude, w en And w ie Respectively the body position rate and the earth rotation rate; r e Is the radius of curvature in the meridian plane of the earth;
the position error is:
wherein E, N, Z represents northeast, R e Is the radius of the earth;
the speed error is:
wherein, δ f n Is the coupling error with specific force caused by the attitude error angle,
is the wrong involved acceleration caused by navigation parameter errors,
coriolis acceleration, δ g, which is a fault caused by errors in navigation parameters n Is the gravity acceleration calculation error introduced by the height error;
gyroscope error:
ε b ＝ε b +ε r +ω g (4)
wherein epsilon b Is a random constant, epsilon r For the first-order Markov process, ω g Is white noise;
accelerometer error:
wherein the content of the first and second substances,
for the scale factor error of the accelerometer,
the offset is zero for the accelerometer,
in order to account for the accelerometer installation error,
is the accelerometer non-linearity error;
step 1.2, error analysis and modeling of satellite navigation system
In the GPS error model, the distance error delta caused by clock error is mainly considered t Speed error delta caused by clock frequency error f Wherein δ f For the first order markov process:
wherein, T f Is the correlation time, ω b Is clock error white noise, omega f White noise in the clock frequency error Markov process;
step 1.3. error analysis and modeling of atmospheric data system
The main error in barometric altitude measurement is the barometric method error, which is caused by the difference between the actual reference sea level barometric pressure at which the aircraft is located and the standard barometric pressure, and this error can be approximately represented by a first-order markov process:
wherein, tau b Is the correlation time;
step 1.4. error analysis and modeling of land-based radio system
The land-based radio navigation system comprises a range finder, wherein the range error of the range finder is as follows:
where Δ c is the velocity error of the radio wave during spatial transmission, Δ t R Measuring the time delay error of the target response signal;
step 1.5 Instrument Landing System (ILS) error analysis and modeling
The position error model for the ILS system is as follows:
wherein, γ G Is a lower slip angle, gamma G Is the heading declination angle, θ B Is the angle, psi, of the reference channel to the ground plane γ Is the angle between the local geographical coordinate system and the due north direction, delta L Is the heading deviation angle error, delta G For lower slip angle error, δ L k The scale factor error of the DME distance measuring machine is shown.
Further, step 2 comprises the following steps:
step 2.1, an inertial navigation and satellite navigation sub-filtering system:
the state equation is as follows:
according to the strapdown inertial navigation error model and the GPS error model, the following state equations are established:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
selecting the difference between the inertial navigation range and the satellite pseudo range and the pseudo range rate as an observed quantity:
the measurement equation is divided into a pseudo-range measurement equation and a pseudo-range rate measurement equation;
step 2.2, inertial navigation and atmospheric data sub-filtering system
The state equation is as follows:
the error model can be established as:
the state equation can be expressed as:
X k+1 ＝Φ k X k +GW (18)
wherein the process noise is: q ═ E [ WW ] T ]，W＝[0，ω a ω baro ω baro ] T Where A is the power spectral density of the accelerometer, α, β and σ 2 The air pressure error parameter is given in the air pressure error model;
the measurement equation:
the measurement of the system is the height and the air pressure height of the inertial navigation system, and the measurement equation is as follows:
step 2.3, inertial navigation and airborne electronic filtering system
The state equation is as follows:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
method for solving two slant distances D of aircraft relative to two range finder stations by combining filtered observed quantity and utilizing self-position information for inertial navigation system ce (e ═ i, j) two slant distances D of the aircraft to the two rangefinder stations measured with two onboard DMEs De The difference of (e ═ i, j), i.e.
In the formula of se 、L se And h se The longitude, dimension and height of the e (e ═ i, j) th station;
the observation equation is then:
wherein N (t) is ground-based observation noise, and the coefficient matrix of the observation equation is
Step 2.4: inertial navigation and instrument landing sub-filtering system
The state equation is as follows:
the state equation is consistent with the state equation of the inertial navigation and satellite navigation sub-filtering system;
the measurement equation:
according to the distance measurement of the gliding beacon and the course beacon of the ILS and the distance measurer, the positions of the warp, the weft and the height can be obtained, then the difference is made between the position output by the ILS and the position output by the inertial navigation, and a measurement equation can be obtained, and can be expressed as follows:
Z ILS/INS (t)＝H ILS/INS (t)X(t)+v(t) (28)
further, step 3 comprises the following steps:
step 3.1: observability analysis of sub-filtering systems
A singular value decomposition method is adopted, and a PWCS discrete system model is set as follows:
wherein X (k) e R n ,F(j)∈R n×n ,Z(k)∈R m ,H(j)∈R m×n J-1, 2, …, r, matrix F for each time segment j j ，H j Are all steady, the system observations can be expressed as a function of X (1):
the overall observation equation is:
Z＝Q(r)X(1) (31)
where Z is the overall observation of the observation vector:
since the observable matrix for each time segment j is defined as:
therefore, Q (r) can be expressed as:
q (r) is the total observability matrix of the discrete PWCS, denoted as TOM of the discrete system, defining Q V (r) extracted observability matrix for discrete PWCS
Q 6 (r)＝[Q 1 Q 2 …Q r ] T (35)
When the segmentation time is sufficiently short, Q is available V The null space of (r) replaces the null space of Q (r), and Q may be used s (r) instead of Q (r) for analyzing observability of the system, performing singular value decomposition on an observability matrix extracted from the system
Q s * (j)＝UΛV T (j＝1，2，…，q) (36)
In the formula:
rσ j are respectively as
Rank and singular value of, and σ i 0, i is 1,2, …, r, U is m.n.j order orthogonal matrix; v is an n-order orthogonal matrix, and if the system meets the requirement of PWCS analysis theorem, Q is * (j) Instead of using Q (j), one can obtain:
X 0 ＝(UΛV T ) -1 Z (38)
the orthogonal matrices U and V are expressed by column vectors formed by respective columns
U＝[u 1 ，u 2 ，…，u m.n.j ]，V＝[v 1 ，v 2 ，…，v m.n.j ] (39)
The above formula can be further written as
Numerically, σ i Represents X 0，i Obtaining the singular characteristics corresponding to the element with the maximum absolute value, if sigma i If the value of (A) is larger, the system state is corresponding toThe state can obtain estimation with higher precision; if σ is i If the value of (b) is smaller, the corresponding system state may be singular and fall into an unobservable space, because the orders of the stripped observability matrix Q (j) are continuously accumulated along with the increase of the number of time periods;
step 3.2: stability analysis of sub-filtering systems
For linearizing a nonlinear system one can obtain:
if for any initial state
And
always have
That is, for any given μ > 0, T ═ T (μ) > 0 can always be found, when T is greater than 0 k ≥t 0 At the time of + T, the temperature of the alloy is higher,
it holds true that the system is called consistent asymptotic stable, known by the principle of the Klaman filter algorithm:
therefore, the filter stability can be judged as the stability problem of the linear system, so that the stability can be judged according to the one-step transfer matrix (I-K) k H k )Φ k The filtering stability is analyzed, and the Kalman filtering stability theorem can be used for deducing the progressive theorem of the filtering error variance matrix, namely if the filter is consistent and gradually stable, the mean square error matrix P of the system k Is also progressively stable.
Further, step 4 comprises the following steps:
the design steps of the adaptive federal filter are as follows:
step 4.1: initialization
The filters for federal filtering are initialized as follows:
wherein, beta i (i-1, 2, … N, m) satisfies the principle of conservation of information, i.e. β 1 +β 2 +…+β m ＝1；
Step 4.2: independent time updating is carried out on each sub-filter and the main filter
Step 4.3: measurement update of each sub-filter
Step 4.4: performing optimal fusion to obtain the filtering value and variance information of the main filter
After state space equations of the four subsystems are established, each subsystem can carry out filtering estimation based on adaptive extended Kalman filtering, state quantities and variances output by the sub-filtering systems and weights of the state quantities are optimally fused in a main filter, and filtering values and variance information can be obtained:
distributing and resetting information to each sub-filter according to a certain information distribution principle, wherein the main filter can perform optimal estimation based on methods such as weighted least squares and the like;
step 4.5: and distributing and resetting information to each sub-filter according to a certain information distribution principle:
step 4.6: repeat step 4.2 and subsequent steps
The allocation factor of the federal filter depends on the observability of variables and the stability of the system:
in the formula, P k，i And P 0，i The error covariance matrices for the ith subsystem at time k and 0, respectively,
to require a pre-designed threshold, X 0ij Is the objectivity X of the ith system 0 The jth component of (a).
Due to the adoption of the technical scheme, compared with the prior art, the invention has the following advantages and positive effects:
1. the invention provides a combined navigation method based on federal filtering, and simultaneously designs a filtering mode switching strategy, thereby realizing information fusion and seamless navigation of heterogeneous multi-source sensors and effectively solving the problem of unavailable GPS (global positioning system) in airplane route operation;
2. the method carries out self-adaptive design on the information distribution factor of the federated filtering based on observability and stability, can improve the self-adaptive capacity of local filtering, and enables a navigation system to have better fault tolerance and higher filtering precision;
3. the invention designs a filtering effectiveness evaluation mechanism for each subsystem based on the observability theory of the system, and can further improve the reliability and the precision of filtering.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. It is obvious that the drawings in the following description are only some embodiments of the invention, and that for a person skilled in the art, other drawings can be derived from them without inventive effort. In the drawings:
FIG. 1 is a flow chart of a combined navigation method based on federated filtering according to the present invention;
FIG. 2 is a system block diagram corresponding to the combined navigation method based on federated filtering according to the present invention;
FIG. 3 is a block diagram of a federated filtering structure in the federated filtering-based integrated navigation method of the present invention.
Detailed Description
While the embodiments of the present invention will be described in detail and fully hereinafter with reference to the accompanying drawings, it is to be understood that the invention is not limited to the details of the embodiments, but may be embodied in various forms without departing from the spirit or scope of the present invention.
As shown in fig. 1-3, the invention discloses a combined navigation method based on federal filtering, which comprises the following steps:
step 1: analyzing error mechanisms and modeling errors of the sensor and the navigation equipment;
step 2: establishing a state space equation of each sub-filtering system;
and 3, step 3: a filtering effectiveness evaluation mechanism;
and 4, step 4: an adaptive federal filter design.
Further, step 1 comprises the following steps:
1.1, analyzing and modeling errors of a strapdown inertial navigation sensor, wherein the attitude errors are as follows:
wherein the content of the first and second substances,
respectively the rate of change of attitude error angle in x, y and z directions, V N Is the north velocity, V E East speed, Δ V N Is a V N Speed change of delta V E Is a V E H is the geographical altitude, w en And w ie Respectively the body position rate and the earth rotation rate; r e Is the radius of curvature in the meridian plane of the earth;
the position error is:
wherein E, N, Z represents northeast, R e Is the radius of the earth;
the speed error is:
wherein, δ f n Is the coupling error with specific force caused by the attitude error angle,
is the wrong involved acceleration caused by navigation parameter errors,
is the wrong Coriolis acceleration, δ g, caused by navigation parameter errors n Is the gravity acceleration calculation error introduced by the height error;
gyroscope error:
ε b ＝ε b +ε r +ω g (4)
wherein epsilon b Is a random constant, epsilon r For the first-order Markov process, ω g Is white noise;
accelerometer error:
wherein the content of the first and second substances,
for the scale factor error of the accelerometer,
the offset is zero for the accelerometer,
in order to account for the accelerometer installation error,
is the accelerometer non-linearity error;
step 1.2, error analysis and modeling of satellite navigation system
In the GPS error model, the distance error delta caused by clock error is mainly considered t Speed error delta caused by clock frequency error f Wherein δ f For the first order markov process:
wherein, T f Is the correlation time, ω b Is clock error white noise, omega f White noise in the clock frequency error Markov process;
step 1.3. error analysis and modeling of atmospheric data system
The main error in barometric altitude measurement is the barometric method error, which is caused by the difference between the actual reference sea level barometric pressure at which the aircraft is located and the standard barometric pressure, and this error can be approximately represented by a first-order markov process:
wherein, tau b Is the correlation time;
step 1.4. error analysis and modeling of land-based radio system
The land-based radio navigation system comprises a Distance meter (Distance Measure Equipment is abbreviated as DME), and the Distance measurement error of the Distance meter is as follows:
where Δ c is the velocity error of the radio wave during spatial transmission, Δ t R Measuring the time delay error of the target response signal;
step 1.5 Instrument Landing System (ILS) error analysis and modeling
The position error model for the ILS system is as follows:
wherein, γ G Is a lower slip angle, gamma L Is the heading declination angle, θ B Is the angle, psi, of the reference channel to the ground plane γ Is the angle between the local geographical coordinate system and the due north direction, delta L Is the course declination error, delta G For lower slip angle error, δ L k The scale factor error of the DME distance measuring machine is shown.
Further, step 2 comprises the following steps:
step 2.1, an inertial navigation and satellite navigation sub-filtering system:
the state equation is as follows:
according to the strapdown inertial navigation error model and the GPS error model, the following state equations are established:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
selecting the difference between the inertial navigation range and the satellite pseudo range and the pseudo range rate as an observed quantity:
the measurement equation is divided into a pseudo-range measurement equation and a pseudo-range rate measurement equation;
step 2.2, inertial navigation and atmospheric data sub-filtering system
The state equation is as follows:
the error model can be established as:
the state equation can be expressed as:
X k+1 ＝Φ k X k +GW (18)
wherein the process noise is: q ═ E [ WW ] T ]，W＝[0，ω a ω baro ω baro ] T Where A is the power spectral density of the accelerometer, α, β and σ 2 The air pressure error parameter is given in the air pressure error model;
the measurement equation:
the measurement of the system is the height and the air pressure height of the inertial navigation system, and the measurement equation is as follows:
step 2.3, inertial navigation and airborne electronic filtering system
The state equation is as follows:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
method for solving two slant distances D of aircraft relative to two range finder stations by combining filtered observed quantity and utilizing self-position information for inertial navigation system ce (e ═ i, j) two slant distances D of the aircraft to the two rangefinder stations measured with two onboard DMEs De The difference of (e ═ i, j), i.e.
In the formula of se 、L se And h se The longitude, dimension and height of the e (e ═ i, j) th station;
the observation equation is then:
wherein N (t) is ground-based observation noise, and the coefficient matrix of the observation equation is
Step 2.4: inertial navigation and instrument landing sub-filtering system
The state equation is as follows:
the state equation is consistent with the state equation of the inertial navigation and satellite navigation sub-filtering system;
the measurement equation:
according to the distance measurement of the gliding beacon and the course beacon of the ILS and the distance measurer, the positions of the warp, the weft and the height can be obtained, then the difference is made between the position output by the ILS and the position output by the inertial navigation, and a measurement equation can be obtained, and can be expressed as follows:
Z ILS/INS (t)＝H ILS/INS (t)X(t)+v(t) (28)
further, step 3 comprises the following steps:
step 3.1: observability analysis of sub-filtering systems
A singular value decomposition method is adopted, and a PWCS discrete system model is set as follows:
wherein X (k) e R n ,F(j)∈R n×n ,Z(k)∈R m ,H(j)∈5 m×n J-1, 2, …, r, matrix F for each time segment j j ，H j Are all steady, the system observations can be expressed as a function of X (1):
the overall observation equation is:
Z＝Q(r)X(1) (31)
where Z is the overall observation of the observation vector:
since the observable matrix for each time segment j is defined as:
therefore, Q (r) can be expressed as:
q (r) is the total Observability matrix of the discrete PWCS, denoted as TOM (Total observer matrix) of the discrete system, and defines Q V (r) isExtraction observability matrix for scattered PWCS
Q S (r)＝[Q 1 Q 2 …Q r ] T (35)
When the segmentation time is sufficiently short, Q is available s The null space of (r) replaces the null space of Q (r), and Q may be used s (r) instead of Q (r) for analyzing observability of the system, performing singular value decomposition on an observability matrix extracted from the system
Q s * (j)＝UΛV T (j＝1，2，…，q) (36)
In the formula:
r，σ j are respectively as
Rank and singular value of, and σ i 0, i is 1,2, …, r, U is m.n.j order orthogonal matrix; v is an n-order orthogonal matrix, and if the system meets the requirement of PWCS analysis theorem, Q is * (j) Instead of using Q (j), one can obtain:
X 0 ＝(UΛV T ) -1 Z (38)
the orthogonal matrices U and V are expressed by column vectors formed by respective columns
U＝[u 1 ，u 2 ，…，u m.n.j ]，V＝[v 1 ，v 2 ，…，v m.n.j ] (39)
The above formula can be further written as
Numerically, σ i Represents X 0，i Obtaining the singular characteristics corresponding to the element of maximum absolute value (system state), if sigma i The corresponding system state can be obtained relatively moreHigh-precision estimation; if σ i If the value of (a) is small, the corresponding system state may be singular and fall into an unobservable space, since the order of the stripped observability matrix q (j) is continuously accumulated with the increase of the number of time periods. This is not conducive to real-time computation, so the definition time interval of the observability matrix is considered to be limited to the segment interval, so that the computation workload of singular value decomposition is greatly reduced, and the requirement of on-line observation can be met.
The observability of the state of each subsystem is calculated and normalized, then the same state quantity of each subsystem is given a weight according to the size of the observability, the given principle is that the weight with large observability is large, the weight with small observability is small, and the sum of all weights satisfies the number of sub-filters.
Step 3.2: stability analysis of sub-filtering systems
For linearizing a nonlinear system one can obtain:
if for any initial state
And
always have
That is, for any given μ > 0, T ═ T (μ) > 0 can always be found, when T is greater than 0 k ≥t 0 At the time of + T, the temperature of the alloy is higher,
if it is constant, the system is called as uniformly asymptotically stable, and the algorithm principle of the Klaman filter is known as follows:
therefore, the filter stability can be judged as the stability problem of the linear system, so that the stability of the linear system can be judged according to the one-step transfer matrix (I-K) k H k )Φ k The filtering stability is analyzed, and the Kalman filtering stability theorem can be used for deducing the progressive theorem of the filtering error variance matrix, namely if the filter is consistent and gradually stable, the mean square error matrix P of the system k Is also progressively stable. Thus, P k Whether or not the filter is asymptotically stable may be used as a method of determining whether or not the filter is diverging.
In order for the whole filtering to operate effectively, a mode switching mechanism is designed. Based on the filter stability judgment criterion, the sub-filters with unstable filtering or divergence tendency can compensate the error covariance matrix of the filtering based on a singular value compensation method to inhibit divergence, and meanwhile, a mode switching mechanism is started to close the sub-filters with obvious divergence, only other effective sub-filtering is reserved, and the mode switching is started under the condition that filtering data of some sub-systems are unavailable or invalid, so that the whole filtering can normally run.
Further, step 4 comprises the following steps:
the design steps of the adaptive federal filter are as follows:
step 4.1: initialization
The filters for federal filtering are initialized as follows:
wherein, beta i (i-1, 2, … N, m) satisfies the principle of conservation of information, i.e. β 1 +β 2 +…+β m ＝1；
Step 4.2: independent time updating is carried out on each sub-filter and main filter
Step 4.3: measurement update of each sub-filter
Step 4.4: performing optimal fusion to obtain the filtering value and variance information of the main filter
After state space equations of the four subsystems are established, each subsystem can perform filtering estimation based on adaptive extended kalman filter (EKF for short), and optimal fusion is performed on the state quantity and the variance output by the sub-filtering system and the weight of each state quantity in the main filter, so that a filtering value and variance information can be obtained:
distributing and resetting information to each sub-filter according to a certain information distribution principle, wherein the main filter can perform optimal estimation based on methods such as weighted least squares and the like;
step 4.5: and distributing and resetting information to each sub-filter according to a certain information distribution principle:
step 4.6: repeat step 4.2 and subsequent steps
The allocation factor of the federal filter depends on the observability of variables and the stability of the system:
in the formula, P k，i And P 0，i The error covariance matrices for the ith subsystem at time k and 0, respectively,
to require a pre-designed threshold, X 0ij Is the objectivity X of the ith system 0 The jth component of (a).
The above description is only for the preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (3)
1. A combined navigation method based on federal filtering is characterized by comprising the following steps:
step 1: analyzing error mechanisms and modeling errors of the sensor and the navigation equipment;
the step 1 comprises the following steps:
step 1.1: analyzing and modeling errors of the strapdown inertial navigation sensor;
step 1.2: analyzing and modeling errors of a satellite navigation system;
step 1.3: analyzing and modeling the error of the atmospheric data system;
step 1.4: analyzing and modeling errors of a land-based radio system;
step 1.5: analyzing and modeling an Instrument Landing System (ILS) error;
step 2: establishing a state space equation of each sub-filtering system;
the step 2 comprises the following steps:
step 2.1: an inertial navigation and satellite navigation sub-filtering system;
step 2.2: an inertial navigation and atmospheric data sub-filtering system;
step 2.3: an inertial navigation and airborne navigation electronic filtering system;
step 2.4: a sexual navigation and instrument landing sub-filtering system;
and step 3: a filtering effectiveness evaluation mechanism;
the step 3 comprises the following steps:
step 3.1: observability analysis of sub-filtering systems
A singular value decomposition method is adopted, and a PWCS discrete system model is set as follows:
wherein X (k) e R n ,F(j)∈R n×n ,Z(k)∈R m ,H(j)∈R m×n J-1, 2, …, r, matrix F for each time segment j j ，H j Are all steady, the system observations can be expressed as a function of X (1):
the overall observation equation is:
Z＝Q(r)X(1) (31)
where Z is the overall observation of the observation vector:
since the observable matrix for each time segment j is defined as:
therefore, Q (r) can be expressed as:
q (r) is the total observability matrix of the discrete PWCS, denoted as TOM of the discrete system, defining Q s (r) extracted observability matrix for discrete PWCS
Q S (r)＝[Q 1 Q 2 … Q r ] T (35)
When the segmentation time is sufficiently short, Q is available s The null space of (r) replaces the null space of Q (r), and Q may be used s (r) instead of Q (r) for analyzing observability of the system, performing singular value decomposition on an observability matrix extracted from the system
Q s * (j)＝UΛV T (j＝1,2,…,q) (36)
In the formula:
r,σ j are respectively as
Rank and singular value of, and σ i 0, i is 1,2, …, r, U is m.n.j order orthogonal matrix; v is an n-order orthogonal matrix, and if the system meets the requirement of PWCS analysis theorem, Q is * (j) Instead of using Q (j), one can obtain:
X 0 ＝(UΛV T ) -1 Z (38)
the orthogonal matrices U and V are expressed by column vectors formed by respective columns
U＝[u 1 ,u 2 ,…,u m.n.j ]，V＝[v 1 ,v 2 ,…,v m.n.j ] (39)
The above formula can be further written as
Numerically, σ i Represents X 0,i Obtaining the singular characteristics corresponding to the element with the maximum absolute value, if sigma i If the value of (a) is larger, the corresponding system state can be estimated with higher precision; if σ is i If the value of (b) is smaller, the corresponding system state may be singular and fall into an unobservable space, because the orders of the stripped observability matrix Q (j) are continuously accumulated along with the increase of the number of time periods;
step 3.2: stability analysis of sub-filtering systems
For linearizing a nonlinear system one can obtain:
if for any initial state
And
always have
That is, for any given μ > 0, T ═ T (μ) > 0 can always be found, when T is greater than 0 k ≥t 0 At the time of + T, the temperature of the alloy is higher,
if it is constant, the system is called as uniformly asymptotically stable, and the algorithm principle of the Klaman filter is known as follows:
therefore, the filter stability can be judged as the stability problem of the linear system, so that the stability can be judged according to the one-step transfer matrix (I-K) k H k )Φ k The filtering stability is analyzed, and the Kalman filtering stability theorem can be used for deducing the progressive theorem of the filtering error variance matrix, namely if the filter is consistent and gradually stable, the mean square error matrix P of the system k Is also asymptotically stable;
and 4, step 4: designing a self-adaptive federal filter;
the step 4 comprises the following steps:
the design steps of the adaptive federal filter are as follows:
step 4.1: initialization
The filters for federal filtering are initialized as follows:
wherein, beta i (i-1, 2, … N, m) satisfies the principle of conservation of information, i.e. β 1 +β 2 +…+β m ＝1；
Step 4.2: independent time updating is carried out on each sub-filter and main filter
Step 4.3: measurement update of each sub-filter
Step 4.4: performing optimal fusion to obtain the filtering value and variance information of the main filter
After state space equations of the four subsystems are established, each subsystem can carry out filtering estimation based on adaptive extended Kalman filtering, state quantities and variances output by the sub-filtering systems and weights of the state quantities are optimally fused in a main filter, and filtering values and variance information can be obtained:
distributing and resetting information to each sub-filter according to a certain information distribution principle, wherein the main filter can perform optimal estimation based on methods such as weighted least squares and the like;
step 4.5: and distributing and resetting information to each sub-filter according to a certain information distribution principle:
step 4.6: repeat step 4.2 and subsequent steps
The allocation factor of the federal filter depends on the observability of variables and the stability of the system:
in the formula, P k,i And P 0,i The error covariance matrices for the ith subsystem at time k and 0, respectively,
to require a pre-designed threshold, X 0ij Is the objectivity X of the ith system 0 The jth component of (a).
2. The combined navigation method based on the federal filtering as claimed in claim 1, wherein the step 1 comprises the following steps:
1.1, analyzing and modeling errors of a strapdown inertial navigation sensor, wherein the attitude errors are as follows:
wherein the content of the first and second substances,
respectively the rate of change of attitude error angle in x, y and z directions, V N Is the north velocity, V E East speed, Δ V N Is a V N Speed change of delta V E Is a V E H is the geographical altitude, w en And w ie Respectively the body position rate and the earth rotation rate; r e Is the radius of curvature in the meridian plane of the earth;
the position error is:
wherein E, N, Z represents northeast, R e Is the radius of the earth;
the speed error is:
wherein, δ f n Is the coupling error with specific force caused by the attitude error angle,
is the wrong involved acceleration caused by navigation parameter errors,
coriolis acceleration, δ g, which is a fault caused by errors in navigation parameters n Is the gravity acceleration calculation error introduced by the height error;
gyroscope error:
ε b ＝ε b +ε r +ω g (4)
wherein epsilon b Is a random constant, epsilon r For the first-order Markov process, ω g Is white noise;
accelerometer error:
▽＝▽ k +▽ b +▽ r +▽ g (5)
wherein + k Is the accelerometer scale factor error + b Is zero bias of accelerometer r Is an accelerometer installation error + g Is the accelerometer non-linearity error;
step 1.2, error analysis and modeling of satellite navigation system
In the GPS error model, the distance error delta caused by clock error is mainly considered t Speed error delta caused by clock frequency error f Wherein δ f For the first order markov process:
wherein, T f Is the correlation time, ω b Is clock error white noise, omega f White noise in the clock frequency error Markov process;
step 1.3. error analysis and modeling of atmospheric data system
The main error in barometric altitude measurement is the barometric method error, which is caused by the difference between the actual reference sea level barometric pressure at which the aircraft is located and the standard barometric pressure, and this error can be approximately represented by a first-order markov process:
wherein, tau b Is the correlation time;
step 1.4. error analysis and modeling of land-based radio system
The land-based radio navigation system comprises a range finder, wherein the range error of the range finder is as follows:
where Δ c is the velocity error of the radio wave during spatial transmission, Δ t R Measuring the time delay error of the target response signal;
step 1.5 Instrument Landing System (ILS) error analysis and modeling
The position error model for the ILS system is as follows:
wherein, γ G Is a lower slip angle, gamma L Is the heading declination angle, θ B Is the angle, psi, of the reference channel to the ground plane γ Is the angle between the local geographical coordinate system and the due north direction, delta L Is the course declination error, delta G For lower slip angle error, δ L k The scale factor error of the DME distance measuring machine is shown.
3. The integrated navigation method based on federal filtering according to claim 1, wherein the step 2 comprises the following steps:
step 2.1, an inertial navigation and satellite navigation sub-filtering system:
the state equation is as follows:
according to the strapdown inertial navigation error model and the GPS error model, the following state equations are established:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
selecting the difference between the inertial navigation range and the satellite pseudo range and the pseudo range rate as an observed quantity:
the measurement equation is divided into a pseudo-range measurement equation and a pseudo-range rate measurement equation;
step 2.2, inertial navigation and atmospheric data sub-filtering system
The state equation is as follows:
the error model can be established as:
the state equation can be expressed as:
X k+1 ＝Φ k X k +GW (18)
wherein the process noise is: q ═ E [ WW ] T ]，W＝[0,ω a ,ω baro ,ω baro ] T Where A is the power spectral density of the accelerometer, α, β and σ 2 The air pressure error parameter is given in the air pressure error model;
the measurement equation:
the measurement of the system is the height and the air pressure height of the inertial navigation system, and the measurement equation is as follows:
step 2.3, inertial navigation and airborne electronic filtering system
The state equation is as follows:
wherein, X (t) is a state variable, F (t) is a system matrix, W (t) is a system noise vector, G (t) is a system noise matrix, and the state quantities are selected as follows:
f (t) is as follows:
the system noise vector w (t) is as follows:
the system noise matrix g (t) is:
the measurement equation:
method for solving two slant distances D of aircraft relative to two range finder stations by combining filtered observed quantity and utilizing self-position information for inertial navigation system ce (e ═ i, j) two slant distances D of the aircraft to the two rangefinder stations measured with two onboard DMEs De The difference of (e ═ i, j), i.e.
In the formula of lambda se 、L se And h se The longitude, dimension and height of the e (e ═ i, j) th station;
the observation equation is then:
wherein N (t) is ground-based observation noise, and the coefficient matrix of the observation equation is
Step 2.4: inertial navigation and instrument landing sub-filtering system
The state equation is as follows:
the state equation is consistent with the state equation of the inertial navigation and satellite navigation sub-filtering system;
the measurement equation:
according to the distance measurement of the gliding beacon and the course beacon of the ILS and the distance measurer, the positions of the warp, the weft and the height can be obtained, then the difference is made between the position output by the ILS and the position output by the inertial navigation, and a measurement equation can be obtained, and can be expressed as follows:
Z ILS/INS (t)＝H ILS/INS (t)X(t)+v(t) (28)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011473337.6A CN112525188B (en) | 2020-12-15 | 2020-12-15 | Combined navigation method based on federal filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011473337.6A CN112525188B (en) | 2020-12-15 | 2020-12-15 | Combined navigation method based on federal filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112525188A CN112525188A (en) | 2021-03-19 |
| CN112525188B true CN112525188B (en) | 2022-08-05 |
Family
ID=74999844
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011473337.6A Active CN112525188B (en) | 2020-12-15 | 2020-12-15 | Combined navigation method based on federal filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112525188B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113124863B (en) * | 2021-04-15 | 2022-10-25 | 南京航空航天大学 | Mixed particle federated filtering data processing method based on KLD sampling |
| CN114509073B (en) * | 2022-01-28 | 2024-03-26 | 中国商用飞机有限责任公司 | Course signal processing method and device, storage medium and aircraft |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103323007A (en) * | 2013-06-17 | 2013-09-25 | 南京航空航天大学 | Robust federated filtering method based on time-variable measurement noise |
| CN104913781A (en) * | 2015-06-04 | 2015-09-16 | 南京航空航天大学 | Unequal interval federated filter method based on dynamic information distribution |
| CN105652306A (en) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | Dead reckoning-based low-cost Big Dipper and MEMS tight-coupling positioning system and method |
| CN106813664A (en) * | 2017-03-06 | 2017-06-09 | 四川咖范网络科技有限公司 | A kind of navigation method and device |
| CN110095800A (en) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | A kind of self-adapted tolerance federated filter Combinated navigation method of multi-source fusion |
| CN110579740A (en) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | unmanned ship integrated navigation method based on adaptive federal Kalman filtering |
| CN111189441A (en) * | 2020-01-10 | 2020-05-22 | 山东大学 | Multi-source self-adaptive fault-tolerant federal filtering combined navigation system and navigation method |
-
2020
- 2020-12-15 CN CN202011473337.6A patent/CN112525188B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103323007A (en) * | 2013-06-17 | 2013-09-25 | 南京航空航天大学 | Robust federated filtering method based on time-variable measurement noise |
| CN104913781A (en) * | 2015-06-04 | 2015-09-16 | 南京航空航天大学 | Unequal interval federated filter method based on dynamic information distribution |
| CN105652306A (en) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | Dead reckoning-based low-cost Big Dipper and MEMS tight-coupling positioning system and method |
| CN106813664A (en) * | 2017-03-06 | 2017-06-09 | 四川咖范网络科技有限公司 | A kind of navigation method and device |
| CN110095800A (en) * | 2019-04-19 | 2019-08-06 | 南京理工大学 | A kind of self-adapted tolerance federated filter Combinated navigation method of multi-source fusion |
| CN110579740A (en) * | 2019-09-17 | 2019-12-17 | 大连海事大学 | unmanned ship integrated navigation method based on adaptive federal Kalman filtering |
| CN111189441A (en) * | 2020-01-10 | 2020-05-22 | 山东大学 | Multi-source self-adaptive fault-tolerant federal filtering combined navigation system and navigation method |
Non-Patent Citations (2)
| Title |
|---|
| A Nonlinear Double Model for Multisensor-Integrated Navigation Using the Federated EKF Algorithm for Small UAVs;Yue Yang ，et al;《Sensors 2020》;20200524;第1-23页 * |
| 基于联邦滤波的多源融合导航算法;谭聚豪等;《导航与控制》;20200430;第19卷(第2期);第10-18页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112525188A (en) | 2021-03-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110095800B (en) | Multi-source fusion self-adaptive fault-tolerant federal filtering integrated navigation method | |
| Rönnbäck | Developement of a INS/GPS navigation loop for an UAV | |
| Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
| US8082099B2 (en) | Aircraft navigation using the global positioning system and an attitude and heading reference system | |
| Soken et al. | UKF-based reconfigurable attitude parameters estimation and magnetometer calibration | |
| US8560280B2 (en) | Method for calculating a navigation phase in a navigation system involving terrain correlation | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| Rad et al. | Optimal attitude and position determination by integration of INS, star tracker, and horizon sensor | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN112525188B (en) | Combined navigation method based on federal filtering | |
| Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
| CN112697154A (en) | Self-adaptive multi-source fusion navigation method based on vector distribution | |
| CN111780750B (en) | Multi-sensor-based high-precision positioning method, equipment and medium for civil aircraft | |
| Shen et al. | Research on high-precision measurement systems of modern aircraft | |
| Gross et al. | A systematic approach for extended kalman filter tuning and low-cost inertial sensor calibration within a GPS/INS application | |
| JPH01500714A (en) | Distributed Kalman filter | |
| Gray | Inflight detection of errors for enhanced aircraft flight safety and vertical accuracy improvement using digital terrain elevation data with an inertial navigation system, global positioning system and radar altimeter | |
| Ismaeel | Design of Kalman Filter of Augmenting GPS to INS Systems | |
| CN111473786A (en) | Two-layer distributed multi-sensor combined navigation filtering method based on local feedback | |
| Islam et al. | Loosely coupled GPS/INS integrated navigation system based on Kalman filter and complementary filter for aircraft | |
| Kaniewski | Closed-loop INS/TACAN/ALT positioning system | |
| Bo et al. | Accurate integrated navigation method based on medium precision strapdown inertial navigation system | |
| Iqbal et al. | Nonlinear modeling of azimuth error for 2D car navigation using parallel cascade identification augmented with Kalman filtering | |
| RU2264598C1 (en) | Method for deterination of coordinates of flight vehicle | |
| US20050143872A1 (en) | Aircraft gps instrumentation system and relative method |
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 |