CN115420285A - Multi-system combined navigation method and device for interactive robust filtering - Google Patents

Multi-system combined navigation method and device for interactive robust filtering Download PDF

Info

Publication number
CN115420285A
CN115420285A CN202210790966.4A CN202210790966A CN115420285A CN 115420285 A CN115420285 A CN 115420285A CN 202210790966 A CN202210790966 A CN 202210790966A CN 115420285 A CN115420285 A CN 115420285A
Authority
CN
China
Prior art keywords
filter
matrix
state
filtering
makf
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.)
Pending
Application number
CN202210790966.4A
Other languages
Chinese (zh)
Inventor
赵宾
曾庆化
刘建业
高春雷
朱小灵
乔伟
李一能
许睿
孙克诚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210790966.4A priority Critical patent/CN115420285A/en
Publication of CN115420285A publication Critical patent/CN115420285A/en
Pending legal-status Critical Current

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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Operations Research (AREA)
  • Navigation (AREA)

Abstract

The invention provides a multisystem combined navigation method and a multisystem combined navigation device for interactive robust filtering, wherein the method comprises the following steps: establishing a state equation according to an error model of the strapdown inertial navigation system, and establishing a position and speed measurement equation; secondly, based on the same performance index function of the conventional strong tracking filter (STKF), providing a self-adaptive filter algorithm (MAKF) aiming at the measurement information abnormity; then, the filtering initial values are interacted, and the STKF filter and the MAKF filter are adopted for parallel filtering to carry out state estimation; constructing a likelihood function by using residual errors in the STKF and MAKF filtering results, and updating the filtering probability; and finally, performing weighted fusion on the filtering output according to the filtering probability to obtain state estimation, and correcting the position, the speed and the attitude of the navigation output. The invention obtains better estimation precision under the conditions of system abnormality and measurement abnormality, and improves the precision and robustness of the integrated navigation.

Description

Multi-system combined navigation method and device for interactive robust filtering
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to a multi-system combined navigation method and device for interactive robust filtering.
Background
Unmanned aerial vehicles are increasingly widely applied, and autonomous navigation thereof is concerned. GNSS/SINS (strapdown inertial navigation system/global navigation satellite system) is the mainstream integrated navigation system on drones, but system noise and measurement noise anomalies all cause the performance of the kalman filter to be affected.
The strong tracking filter STKF has strong tracking capability on the measurement information and good robustness on uncertainty of system noise, but the measurement information will generate large fluctuation when being interfered.
Disclosure of Invention
The purpose of the invention is as follows: in order to improve the estimation precision of the unmanned aerial vehicle integrated navigation under the conditions of system noise abnormality, measurement noise abnormality and the like, the invention provides an adaptive filtering algorithm (MAKF) aiming at measurement information abnormality and an interactive robust filtering integrated navigation method combining the advantages of the STKF and the MAKF.
The invention specifically provides a multisystem integrated (SINS/GNSS integrated) navigation method of interactive robust filtering, which comprises the following steps:
step 1, establishing a state equation and a position and speed measurement equation according to a strapdown inertial navigation system;
step 2, designing an adaptive filter MAKF with abnormal measurement information according to a performance index function with the minimum sum of mean square errors of all components of the state estimation error of the strong tracking filter STKF;
step 3, performing input interaction, and respectively calculating initial states and error variance matrixes of the strong tracking filter STKF and the adaptive filter MAKF;
step 4, performing state estimation on the state equation and the position and speed measurement equation by adopting parallel filtering of a strong tracking filter STKF and an adaptive filter MAKF;
step 5, constructing a likelihood function by using residual errors in filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF, and updating filtering probability;
and 6, performing weighted fusion on the filtering output according to the filtering probability to obtain state estimation, and correcting the position, the speed and the posture of the navigation output.
The step 1 comprises the following steps:
establishing a combined navigation system state equation as follows:
Figure BDA0003730249390000021
wherein X (t) is a state vector;
Figure BDA0003730249390000022
is the differential of the state vector; a (t) is a state transition matrix; g (t) is a system noise coefficient matrix; w (t) is the system noise vector;
Figure BDA0003730249390000023
wherein phi Ε 、φ N 、φ U Respectively representing east, north and sky directions of strapdown inertial navigation system navigation coordinate systemA mathematical plateau misalignment angle;
δv E 、δv N 、δv U respectively representing the speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction;
delta L, delta lambda and delta h respectively represent latitude errors, longitude errors and altitude errors of the strapdown inertial navigation system;
ε x 、ε y 、ε z respectively representing gyroscope drift of the strapdown inertial navigation system in the right direction, the front direction and the upper direction of a carrier coordinate system;
Figure BDA0003730249390000024
respectively representing the zero offset of the accelerometer of the strapdown inertial navigation system in the right, front and upper directions of a carrier coordinate system;
the position and speed measurement equation is as follows:
Figure BDA0003730249390000025
wherein Z (t) is a measurement vector, L I 、λ I 、h I Respectively representing the latitude, longitude, altitude, v, of the strapdown inertial navigation system solution IE 、v IN 、v IU Respectively represents the speed L in east, north and sky directions of a navigation coordinate system calculated by the strapdown inertial navigation system G 、λ G 、h G Respectively representing latitude, longitude and altitude of GNSS measurement; v. of GE 、v GN 、v GU Respectively representing the velocity of the GNSS measured navigation coordinate system in east, north and sky directions, H (t) is a measurement coefficient matrix, V (t) is a measurement noise vector, R M And R N Respectively a meridian radius of curvature and a prime radius of curvature, L is the local latitude, diag is a function for constructing a diagonal matrix, 0 3×6 Represents a 3 x 6 dimensional all-zero matrix;
discretizing to obtain:
Figure BDA0003730249390000031
wherein, X k Is the state vector at time k, X k-1 Is the state vector at time k-1, phi k/k-1 Is the transition matrix of the state vector from time k-1 to time k, Γ k-1 Is the noise coefficient matrix of the influence of the system noise at time k-1 on the state vector at time k, W k-1 Is the system noise vector at time k-1, H k Is a k time measurement vector Z k State vector X at time k k Matrix of inter-measurement coefficients, V k Is the measurement noise vector at time k.
The step 2 comprises the following steps:
step 2-1, calculating a state one-step prediction and prediction error variance matrix:
Figure BDA0003730249390000032
Figure BDA0003730249390000033
wherein P is k/k-1 Predicting an error variance matrix for the state one step;
Figure BDA0003730249390000034
one-step prediction for state; q k-1 Is a system noise variance matrix;
Figure BDA0003730249390000035
for state estimation, P k-1 A state estimation error variance matrix is adopted, and T represents matrix transposition;
step 2-2, calculating residual error r k And actual mean square error matrix
Figure BDA0003730249390000036
Figure BDA0003730249390000037
Figure BDA0003730249390000038
In the formula, r 1 Is the residual error at the time instant k =1,
Figure BDA0003730249390000039
the matrix is an actual residual mean square error matrix at the moment of k-1, and rho is a forgetting factor;
step 2-3, calculating a suboptimal fading factor lambda 2k ：
The state estimation error is recorded as:
Figure BDA0003730249390000041
Figure BDA0003730249390000042
is a random vector, adopts the same performance index as the strong tracking filter STKF,
Figure BDA0003730249390000043
the sum of the mean square errors of the components is minimal, which is equivalent to:
Figure BDA0003730249390000044
in the formula, K k Is a filter gain matrix.
Let the intermediate parameter S k Comprises the following steps:
Figure BDA0003730249390000045
the sub-optimal under the inaccurate system model is measured by the minimization of the performance index as follows:
Figure BDA0003730249390000046
wherein J represents a performance indicator function, S ij Is a matrix S k Row i and column j;
if the performance index function J is minimized, the optimal adaptive factor needs to satisfy:
Figure BDA0003730249390000047
in the conventional Kalman filtering
Figure BDA0003730249390000048
And
Figure BDA0003730249390000049
substitution into
Figure BDA00037302493900000410
And will correct lambda 2k R k In place of R k The derivation yields:
Figure BDA00037302493900000411
let an intermediate parameter N k And μm k Comprises the following steps:
Figure BDA00037302493900000412
Μ k ＝R k
in the formula, R k Measuring a noise variance matrix;
obtaining:
Figure BDA0003730249390000051
wherein tr represents a trace-solving operation on the matrix;
step 2-4, calculatingGain matrix K k ：
Figure BDA0003730249390000052
Step 2-5, calculating t k Time state estimation
Figure BDA0003730249390000053
Sum state estimation error variance matrix P k ：
Figure BDA0003730249390000054
P k ＝[I-K k H k ]P k/k-1 。
Wherein I represents an identity matrix.
The adaptive filter MAKF is then as follows:
Figure BDA0003730249390000055
the step 3 comprises the following steps:
step 3-1,p ij Representing the probability of transition from filter i to filter j, i =1,2,j =1,2, filter 1 representing the strong tracking filter STKF, filter 2 representing the adaptive filter MAKF,
Figure BDA0003730249390000056
the probability of the filter i at the k-1 moment is expressed, and the mixed probability of the filter i to be transferred to the filter j at the k-1 moment is calculated
Figure BDA0003730249390000057
Comprises the following steps:
Figure BDA0003730249390000058
step 3-2, estimating the state of the filter i according to the k-1 moment
Figure BDA0003730249390000059
Covariance matrix
Figure BDA00037302493900000510
And mixed probabilities
Figure BDA00037302493900000511
Computing hybrid state estimate for filter j
Figure BDA00037302493900000512
And mixed covariance
Figure BDA00037302493900000513
Figure BDA0003730249390000061
Step 4 comprises the following steps:
step 4-1, estimating according to the mixing state of each filter
Figure BDA0003730249390000062
Mixed covariance
Figure BDA0003730249390000063
And measurement information Z k Performing state estimation by using strong tracking filter STKF and adaptive filter MAKF for parallel filtering to obtain state estimation at k moment
Figure BDA0003730249390000064
And its covariance matrix
Figure BDA0003730249390000065
The filtering process of the strong tracking filter STKF comprises the following steps:
Figure BDA0003730249390000066
wherein, the strong tracking filtering self-adapting factor lambda 1k Comprises the following steps:
Figure BDA0003730249390000067
where max is the maximum value taken from the set elements.
The filtering process of the self-adaptive filter MAKF comprises the following steps:
Figure BDA0003730249390000068
the step 5 comprises the following steps:
step 5-1, respectively calculating and obtaining the residual error of the filter j at the k moment according to the filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF in the step 4
Figure BDA0003730249390000069
And its covariance matrix
Figure BDA00037302493900000610
Figure BDA0003730249390000071
Wherein
Figure BDA0003730249390000072
Is the measured noise variance matrix of the filter j;
step 5-2, according to the residual error
Figure BDA0003730249390000073
And its covariance matrix
Figure BDA0003730249390000074
Solving likelihood function of filter j at time k
Figure BDA0003730249390000075
Figure BDA0003730249390000076
Wherein n is the residual
Figure BDA0003730249390000077
Exp means performing exponential operation;
step 5-3, calculating the probability of the filter j at the moment k
Figure BDA0003730249390000078
Figure BDA0003730249390000079
The step 6 specifically comprises the following steps:
estimation of each filter state from time k
Figure BDA00037302493900000710
Covariance matrix
Figure BDA00037302493900000711
And probability of each filter at time k
Figure BDA00037302493900000712
Solving fused state estimates
Figure BDA00037302493900000713
And its covariance
Figure BDA00037302493900000714
Figure BDA00037302493900000715
The invention also provides a multisystem integrated navigation device for interactive robust filtering, which comprises:
the state equation and position and speed measurement equation building module is used for building a state equation and a position and speed measurement equation according to the strapdown inertial navigation system;
the adaptive filter MAKF designing module is used for designing the adaptive filter MAKF according to a performance index function with the minimum sum of mean square errors of all components of the state estimation error of the strong tracking filter STKF;
the initial state and error variance matrix calculation module is used for performing input interaction and respectively calculating the initial state and the error variance matrix of the STKF and the MAKF;
the state estimation module is used for carrying out state estimation on the state equation and the position and speed measurement equation by adopting parallel filtering of a strong tracking filter STKF and an adaptive filter MAKF;
the filtering probability updating module is used for constructing a likelihood function by using residual errors in filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF and updating the filtering probability;
and the navigation correction module is used for performing weighted fusion on the filtering output according to the filtering probability to obtain state estimation and correcting the position, the speed and the posture of the navigation output.
The invention has the following beneficial effects:
1. the invention provides a self-adaptive filtering algorithm MAKF aiming at measurement information abnormity based on a performance index function same as that of an STKF filtering algorithm. And obtaining a performance index function based on the principle of minimum estimation error. The adaptive factor of the covariance matrix of the measured noise is obtained through minimization of the performance index function, when the measured noise changes, the adaptive factor is adjusted in real time, effective information is extracted from residual errors to the maximum extent, and the state estimation precision under the condition of abnormal measured information is improved.
2. The invention provides an interactive robust filtering algorithm based on the STKF and the MAKF according to the characteristics of the STKF and the MAKF and the complementarity of application conditions. The STKF is inaccurate effective to the system model, and the MAKF is unusual effective to the measurement information, but unmanned aerial vehicle autonomous navigation in-process, above-mentioned situation all probably appears. The interactive robust filtering algorithm based on the STKF and the MAKF is provided, and the weight of each filter is adjusted according to the filtering effect: and calculating a likelihood function through the residual error and the error variance matrix thereof, updating the likelihood function to obtain a filtering probability, and interacting filtering output according to the filtering probability to obtain state estimation. And better estimation precision is obtained under the conditions of system abnormality and measurement abnormality.
Drawings
The foregoing and/or other advantages of the invention will become further apparent from the following detailed description of the invention when taken in conjunction with the accompanying drawings.
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
The invention provides a multisystem combined navigation method of interactive robust filtering, which comprises the following steps as shown in figure 1:
firstly, establishing a state equation according to an error model of a strapdown inertial navigation system, and establishing a position and speed measurement equation by taking the difference values of a calculated position and speed and a measured position and speed of a GNSS (global navigation satellite system) as measurement values based on the strapdown inertial navigation system:
selecting a mathematical platform misalignment angle phi in east-north-sky directions of a navigation coordinate system of the strapdown inertial navigation system E 、φ N 、φ U Velocity error δ v E 、δv N 、δv U Latitude error, longitude error and altitude error delta L, delta lambda and delta h of strapdown inertial navigation system, and gyroscope drift epsilon of carrier coordinate system in right-front-up three directions x 、ε y 、ε z And the zero offset of the accelerometer in the right-front-upper three directions of the carrier coordinate system is used as
Figure BDA0003730249390000091
For the state quantities of the combined navigation:
Figure BDA0003730249390000092
the combined navigation system state equation is:
Figure BDA0003730249390000093
wherein X (t) is a state vector;
Figure BDA0003730249390000094
is the differential of the state vector; a (t) is a state transition matrix; g (t) is a system noise coefficient matrix; w (t) is the system noise vector;
the latitude, longitude and altitude information resolved by the strapdown inertial navigation system is L I 、λ I 、h I Velocity information in the east-north-sky direction is v IE 、v IN 、v IU (ii) a The latitude, longitude and altitude information of GNSS measurement is L G 、λ G 、h G And the velocity information of the navigation coordinate system in the east-north-sky direction is v GE 、v GN 、v GU . And (3) taking the difference value between the resolved position and speed of the strapdown inertial navigation system and the measured position and speed of the GNSS as the measurement quantity, establishing a position and speed measurement equation as follows:
Figure BDA0003730249390000095
in the formula: z (t) is a measurement vector, H (t) is a measurement coefficient matrix, V (t) is a measurement noise vector, R M And R N Respectively a meridian radius of curvature and a prime radius of curvature, L is the local latitude, diag is a function for constructing a diagonal matrix, 0 m×n An all-zero matrix representing dimensions mxn;
performing discretization to obtain
Figure BDA0003730249390000096
In the formula, X k Is the state vector at time k, X k-1 Is the state vector at time k-1, phi k/k-1 Is the transition matrix of the state vector from time k-1 to time k, Γ k-1 Is the noise coefficient matrix of the influence of the system noise at time k-1 on the state vector at time k, W k-1 Is the system noise vector, H, at time k-1 k Is a k time measurement vector Z k State vector X at time k k Inter-measurement coefficient matrix, V k Is the measurement noise vector at time k.
And performing input interaction according to the filtering result at the k-1 moment, and respectively calculating the initial state and the error variance matrix of the STKF and the MAKF filters:
p ij representing the probability of transition from filter i to filter j, i =1,2,j =1,2, filter 1 representing the STKF filter, filter 2 representing the MAKF filter,
Figure BDA0003730249390000101
the probability of the filter i at the k-1 moment is expressed, and the mixed probability of the filter i to be transferred to the filter j at the k-1 moment is calculated
Figure BDA0003730249390000102
Comprises the following steps:
Figure BDA0003730249390000103
state estimation of filter i from time k-1
Figure BDA0003730249390000104
Covariance matrix
Figure BDA0003730249390000105
And mixed probabilities
Figure BDA0003730249390000106
Computing hybrid state estimates for filter j
Figure BDA0003730249390000107
And mixed covariance
Figure BDA0003730249390000108
Figure BDA0003730249390000109
Based on the hybrid state estimate of each filter
Figure BDA00037302493900001010
Mixed covariance
Figure BDA00037302493900001011
And measurement information Z k Performing state estimation on the established state equation and measurement equation by adopting parallel filtering of an STKF filter and an MAKF filter to obtain state estimation at the k moment
Figure BDA00037302493900001012
And its covariance matrix
Figure BDA00037302493900001013
The filtering process of the STKF filter is as follows:
Figure BDA00037302493900001014
wherein, the self-adaptive factor of the strong tracking filter is as follows:
Figure BDA0003730249390000111
the filtering process of the MAKF filter is as follows:
Figure BDA0003730249390000112
the adaptive filtering factor of the measured noise is as follows:
Figure BDA0003730249390000113
and constructing a likelihood function according to the filtering results of the STKF filter and the MAKF filter, and updating the filtering probability. Respectively calculating to obtain residual errors of the filter j at the moment k
Figure BDA0003730249390000114
And its covariance matrix
Figure BDA0003730249390000115
Figure BDA0003730249390000116
Based on the residual error
Figure BDA0003730249390000117
And its covariance matrix
Figure BDA0003730249390000118
Solving likelihood function of filter j at time k
Figure BDA0003730249390000119
Figure BDA00037302493900001110
Where n is the residual
Figure BDA00037302493900001111
Dimension (d) of (a).
Calculate the probability of filter j at time k:
Figure BDA00037302493900001112
estimating the state of each filter according to the obtained k time
Figure BDA00037302493900001113
Covariance matrix
Figure BDA00037302493900001114
And the probability of each filter, performing weighted fusion on the filtering output, and solving the state estimation after fusion
Figure BDA00037302493900001115
And its covariance
Figure BDA00037302493900001116
Correcting the position, speed and attitude of the navigation output:
Figure BDA0003730249390000121
in a specific implementation, the present application provides a computer storage medium and a corresponding data processing unit, where the computer storage medium is capable of storing a computer program, and the computer program, when executed by the data processing unit, may execute the inventive content of the interactive robust filtering multi-system combined navigation method provided by the present invention and some or all of the steps in each embodiment. The storage medium may be a magnetic disk, an optical disk, a read-only memory (ROM), a Random Access Memory (RAM), or the like.
It is obvious to those skilled in the art that the technical solutions in the embodiments of the present invention can be implemented by means of a computer program and its corresponding general-purpose hardware platform. Based on such understanding, the technical solutions in the embodiments of the present invention may be embodied in the form of a computer program, that is, a software product, which may be stored in a storage medium and includes several instructions to enable a device (which may be a personal computer, a server, a single chip microcomputer MUU or a network device) including a data processing unit to execute the method in each embodiment or some parts of the embodiments of the present invention.
The present invention provides a method and an apparatus for multi-system combined navigation with interactive robust filtering, and a plurality of methods and approaches for implementing the technical solution are provided, and the above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, a plurality of improvements and modifications may be made without departing from the principle of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. All the components not specified in the present embodiment can be realized by the prior art.

Claims (10)

1. A multisystem combined navigation method of interactive robust filtering is characterized by comprising the following steps:
step 1, establishing a state equation and a position and speed measurement equation according to a strapdown inertial navigation system;
step 2, designing a self-adaptive filter MAKF with abnormal measurement information according to a performance index function with the minimum sum of mean square errors of all components of the state estimation error of the strong tracking filter STKF;
step 3, performing input interaction, and respectively calculating initial states and error variance matrixes of the strong tracking filter STKF and the adaptive filter MAKF;
step 4, performing state estimation on the state equation and the position and speed measurement equation by adopting parallel filtering of a strong tracking filter STKF and an adaptive filter MAKF;
step 5, constructing a likelihood function by using residual errors in filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF, and updating filtering probability;
and step 6, performing weighted fusion on the filtering output according to the filtering probability to obtain state estimation, and correcting the position, the speed and the attitude of the navigation output.
2. The method of claim 1, wherein step 1 comprises:
establishing a combined navigation system state equation as shown in the following:
Figure FDA0003730249380000011
wherein X (t) is a state vector;
Figure FDA0003730249380000012
is the differential of the state vector; a (t) is a state transition matrix; g (t) is a system noise figure matrix; w (t) is the system noise vector;
Figure FDA0003730249380000013
wherein phi is Ε 、φ N 、φ U Respectively representing the mathematical platform misalignment angles in east, north and sky directions of a navigation coordinate system of the strapdown inertial navigation system;
δv E 、δv N 、δv U respectively representing the speed errors in the east direction, the north direction and the sky direction of a strapdown inertial navigation system navigation coordinate system;
delta L, delta lambda and delta h respectively represent latitude errors, longitude errors and altitude errors of the strapdown inertial navigation system;
ε x 、ε y 、ε z respectively representing gyroscope drift of the strapdown inertial navigation system in the right, front and upper directions of a carrier coordinate system;
Figure FDA0003730249380000014
respectively representing the zero offset of the accelerometer of the strapdown inertial navigation system in the right, front and upper directions of a carrier coordinate system;
the position and speed measurement equation is as follows:
Figure FDA0003730249380000021
wherein Z (t) is a measurement vector, L I 、λ I 、h I Respectively representing the latitude, longitude and altitude, v, of the strapdown inertial navigation system IE 、v IN 、v IU Respectively representVelocity L of the navigation coordinate system east, north and sky calculated by the strapdown inertial navigation system G 、λ G 、h G Respectively representing latitude, longitude and altitude of the GNSS measurement; v. of GE 、v GN 、v GU Respectively representing the speed of the GNSS measurement in the east, north and sky directions, H (t) is a measurement coefficient matrix, V (t) is a measurement noise vector, R M And R N Respectively a meridian radius of curvature and a prime radius of curvature, L is the local latitude, diag is a function for constructing a diagonal matrix, 0 3×6 Represents a 3 x 6 dimensional all-zero matrix;
discretizing to obtain:
Figure FDA0003730249380000022
wherein, X k Is the state vector at time k, X k-1 Is the state vector at time k-1, phi k/k-1 Is the transition matrix of the state vector from time k-1 to time k, Γ k-1 Is the noise coefficient matrix of the influence of the system noise at time k-1 on the state vector at time k, W k-1 Is the system noise vector at time k-1, H k Is a k time measurement vector Z k State vector X at time k k Matrix of inter-measurement coefficients, V k Is the measurement noise vector at time k.
3. The method of claim 2, wherein step 2 comprises:
step 2-1, calculating a state one-step prediction and prediction error variance matrix:
Figure FDA0003730249380000023
Figure FDA0003730249380000031
wherein P is k/k-1 Predicting an error variance matrix for the state one step;
Figure FDA0003730249380000032
one-step prediction for state; q k-1 Is a system noise variance matrix;
Figure FDA0003730249380000033
for state estimation, P k-1 A state estimation error variance matrix is adopted, and T represents matrix transposition;
step 2-2, calculating residual error r k And actual mean square error matrix
Figure FDA0003730249380000034
Figure FDA0003730249380000035
Figure FDA0003730249380000036
In the formula, r 1 Is the residual error at the time instant k =1,
Figure FDA0003730249380000037
the matrix is an actual residual mean square error matrix at the moment of k-1, and rho is a forgetting factor;
step 2-3, calculating a suboptimal fading factor lambda 2k ：
The state estimation error is recorded as:
Figure FDA0003730249380000038
Figure FDA0003730249380000039
is a random vector, and adopts the same method as that of strong tracking filter STKFThe performance index of (a) is,
Figure FDA00037302493800000310
the sum of the mean square errors of the components is minimal, which is equivalent to:
Figure FDA00037302493800000311
in the formula, K k Is a filter gain matrix;
let the intermediate parameter S k Comprises the following steps:
Figure FDA00037302493800000312
the sub-optimal under the inaccurate system model is measured by the minimization of the performance index as follows:
Figure FDA00037302493800000313
wherein J represents a performance indicator function, S ij Is a matrix S k Row i and column j;
if the performance index function J is minimized, the optimal adaptive factor needs to satisfy:
Figure FDA0003730249380000041
in the conventional Kalman filtering
Figure FDA0003730249380000042
And
Figure FDA0003730249380000043
substitution into
Figure FDA0003730249380000044
And will correct lambda 2k R k In place of R k The derivation yields:
Figure FDA0003730249380000045
let an intermediate parameter N k And μm k Comprises the following steps:
Figure FDA0003730249380000046
Μ k ＝R k
in the formula, R k Measuring a noise variance matrix;
obtaining:
Figure FDA0003730249380000047
wherein tr represents a tracing operation on the matrix;
step 2-4, calculating a gain matrix K k ：
Figure FDA0003730249380000048
Step 2-5, calculating t k Time state estimation
Figure FDA0003730249380000049
Sum state estimation error variance matrix P k ：
Figure FDA00037302493800000410
P k ＝[I-K k H k ]P k/k-1 ，
Wherein, I represents an identity matrix;
the adaptive filter MAKF is then as follows:
Figure FDA0003730249380000051
4. the method of claim 3, wherein step 3 comprises:
step 3-1,p ij Representing the probability of transition from filter i to filter j, i =1,2,j =1,2, filter 1 representing the strong tracking filter STKF, filter 2 representing the adaptive filter MAKF,
Figure FDA0003730249380000052
the probability of the filter i at the k-1 moment is expressed, and the mixed probability of the filter i to be transferred to the filter j at the k-1 moment is calculated
Figure FDA0003730249380000053
Comprises the following steps:
Figure FDA0003730249380000054
step 3-2, estimating the state of the filter i according to the k-1 moment
Figure FDA0003730249380000055
Covariance matrix
Figure FDA0003730249380000056
And mixed probabilities
Figure FDA0003730249380000057
Computing hybrid state estimates for filter j
Figure FDA0003730249380000058
And mixed covariance
Figure FDA0003730249380000059
Figure FDA00037302493800000510
5. The method of claim 4, wherein step 4 comprises:
step 4-1, estimating according to the mixing state of each filter
Figure FDA00037302493800000511
Mixed covariance
Figure FDA00037302493800000512
And measurement information Z k Performing state estimation by using strong tracking filter STKF and adaptive filter MAKF for parallel filtering to obtain state estimation at k moment
Figure FDA00037302493800000513
And its covariance matrix
Figure FDA00037302493800000514
The filtering process of the strong tracking filter STKF comprises the following steps:
Figure FDA00037302493800000515
wherein, the strong tracking filtering self-adapting factor lambda 1k Comprises the following steps:
Figure FDA0003730249380000061
where max is the maximum value taken from the set elements;
the filtering process of the self-adaptive filter MAKF comprises the following steps:
Figure FDA0003730249380000062
6. the method of claim 5, wherein step 5 comprises:
step 5-1, respectively calculating and obtaining the residual error of the filter j at the k moment according to the filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF in the step 4
Figure FDA0003730249380000063
And its covariance matrix
Figure FDA0003730249380000064
Figure FDA0003730249380000065
Wherein
Figure FDA0003730249380000066
Is the measured noise variance matrix of the filter j;
step 5-2, according to the residual error
Figure FDA0003730249380000067
And its covariance matrix
Figure FDA0003730249380000068
Solving likelihood function of filter j at time k
Figure FDA0003730249380000069
Step 5-3, calculating the probability of the filter j at the moment k
Figure FDA00037302493800000610
7. The method of claim 6, wherein in step 5-2, the likelihood function of the filter j at time k is solved using the following formula
Figure FDA00037302493800000611
Figure FDA00037302493800000612
Where n is the residual
Figure FDA00037302493800000613
Exp means to do an exponential operation.
8. The method according to claim 7, wherein, in step 5-3, the probability of filter j at time k is calculated using the following formula
Figure FDA00037302493800000614
Figure FDA0003730249380000071
9. The method according to claim 8, wherein step 6 specifically comprises:
estimation of each filter state from time k
Figure FDA0003730249380000072
Covariance matrix
Figure FDA0003730249380000073
And probability of each filter at time k
Figure FDA0003730249380000074
Solving fused state estimates
Figure FDA0003730249380000075
And its covariance
Figure FDA0003730249380000076
Figure FDA0003730249380000077
10. An interactive robust filtered multisystem integrated navigation device, comprising:
the state equation and position and speed measurement equation building module is used for building a state equation and a position and speed measurement equation according to the strapdown inertial navigation system;
the self-adaptive filter MAKF design module is used for designing the self-adaptive filter MAKF according to a performance index function with the minimum sum of the mean square errors of all components of the state estimation error of the strong tracking filter STKF;
the initial state and error variance matrix calculation module is used for performing input interaction and respectively calculating the initial state and the error variance matrix of the STKF and the MAKF;
the state estimation module is used for carrying out state estimation on the state equation and the position and speed measurement equation by adopting parallel filtering of a strong tracking filter STKF and an adaptive filter MAKF;
the filtering probability updating module is used for constructing a likelihood function by using residual errors in filtering results of the strong tracking filter STKF and the self-adaptive filter MAKF and updating the filtering probability;
and the navigation correction module is used for performing weighted fusion on the filtering output according to the filtering probability to obtain state estimation and correcting the position, the speed and the attitude of the navigation output.
CN202210790966.4A 2022-07-05 2022-07-05 Multi-system combined navigation method and device for interactive robust filtering Pending CN115420285A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210790966.4A CN115420285A (en) 2022-07-05 2022-07-05 Multi-system combined navigation method and device for interactive robust filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210790966.4A CN115420285A (en) 2022-07-05 2022-07-05 Multi-system combined navigation method and device for interactive robust filtering

Publications (1)

Publication Number Publication Date
CN115420285A true CN115420285A (en) 2022-12-02

Family

ID=84197146

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210790966.4A Pending CN115420285A (en) 2022-07-05 2022-07-05 Multi-system combined navigation method and device for interactive robust filtering

Country Status (1)

Country Link
CN (1) CN115420285A (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110277978A (en) * 2019-07-04 2019-09-24 苏州妙益科技股份有限公司 A kind of Kalman filtering current acquisition method based on variable fading factor
CN110375731A (en) * 2019-07-01 2019-10-25 东南大学 A kind of mixing interacting multiple model filters method
CN111707292A (en) * 2020-07-18 2020-09-25 东南大学 Fast transfer alignment method of self-adaptive filtering

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375731A (en) * 2019-07-01 2019-10-25 东南大学 A kind of mixing interacting multiple model filters method
CN110277978A (en) * 2019-07-04 2019-09-24 苏州妙益科技股份有限公司 A kind of Kalman filtering current acquisition method based on variable fading factor
CN111707292A (en) * 2020-07-18 2020-09-25 东南大学 Fast transfer alignment method of self-adaptive filtering

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
DANIAL SABZEVARI: "INS/GPS Sensor Fusion based on Adaptive Fuzzy EKF with Sensitivity to Disturbances", IET RADAR, SONAR & NAVIGATION, 23 June 2021 (2021-06-23), pages 1535 - 1549, XP006112759, DOI: 10.1049/rsn2.12144 *
XIYUAN CHEN: "Novel hybrid of strong tracking Kalman filter and wavelet neural network for GPS/INS during GPS outages", MEASUREMENT, 31 December 2013 (2013-12-31), pages 3847 *

Similar Documents

Publication Publication Date Title
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN110196443A (en) A kind of fault-tolerance combined navigation method and system of aircraft
CN109855617A (en) A kind of vehicle positioning method, vehicle locating device and terminal device
CN109459019A (en) A kind of vehicle mounted guidance calculation method based on cascade adaptive robust federated filter
CN111102978A (en) Method and device for determining vehicle motion state and electronic equipment
CN110058288B (en) Course error correction method and system for unmanned aerial vehicle INS/GNSS combined navigation system
CN115143954B (en) Unmanned vehicle navigation method based on multi-source information fusion
CN105865441B (en) Composite layered adaptive filter for multi-source disturbance system
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
CN111024124B (en) Combined navigation fault diagnosis method for multi-sensor information fusion
CN103776449B (en) A kind of initial alignment on moving base method that improves robustness
CN115451952A (en) Multi-system combined navigation method and device for fault detection and robust adaptive filtering
CN113984054A (en) Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment
CN113175931A (en) Cluster networking collaborative navigation method and system based on constraint Kalman filtering
CN111750865A (en) Self-adaptive filtering navigation method for dual-function deep sea unmanned submersible vehicle navigation system
Jørgensen et al. Underwater position and attitude estimation using acoustic, inertial, and depth measurements
CN114689047A (en) Deep learning-based integrated navigation method, device, system and storage medium
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN113009816A (en) Method and device for determining time synchronization error, storage medium and electronic device
CN111982126A (en) Design method of full-source BeiDou/SINS elastic state observer model
CN106886037A (en) Suitable for the POS data method for correcting error of weak GNSS signal condition
CN115420285A (en) Multi-system combined navigation method and device for interactive robust filtering
US20230078005A1 (en) Navigation assistance method for a mobile carrier
CN113959433A (en) Combined navigation method and device
CN111473786A (en) Two-layer distributed multi-sensor combined navigation filtering method based on local feedback

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