Background
Observability refers to the ability of the system to determine the initial state of the system through measurement values within a limited time, and is a precondition for determining whether the Kalman filter is converged. The observability measure is an analysis theory which is provided on the basis of observability in order to evaluate the filtering precision of the system state, and is a quantitative representation of the observability degree of each state component. The high observability degree of the state component is a precondition for stable work of the Kalman filter, so that the filtering performance problem can be converted into a parameter problem, and the estimation performance of the system can be analyzed by replacing a state estimation error covariance matrix with the system observability degree under a certain condition. According to the matching of the system observability and the estimation precision, the system observability is taken as a means for designing the adaptive filter, and the method is a new direction for the development of the combined navigation technology.
The traditional observability measure is mostly defined by considering the characteristics of the system, and the default system is precisely known to be free of noise. However, in practical engineering, the existence of measurement noise is inevitable, so that when the interference of the noise is neglected, the observability analysis is influenced to a certain extent. A novel SINS/GPS adaptive feedback correction filtering method based on observability degree analysis is disclosed in the patent number: CN200610114271.5, the observability degree is defined by using an observability degree analysis method based on singular value decomposition, and the influence of system measurement noise on the calculation of the observability degree is not considered, so that the observability degree is not defined accurately, and the improvement of navigation precision is limited. A method of analyzing observability of an inertial navigation system, patent No.: the CN201510272159.3 emphasizes on considering each observable degree of state, and solves the problem that the conventional observable analysis method cannot solve the observable degree of a single state, but still does not consider the influence of noise on the observable degree.
The traditional combined navigation method completely feeds back the filter result to the system, but when some system states are not observable or the observability degree is very low, the estimation precision is also very low, and the estimation value with low precision is directly fed back, so that the precision of the combined navigation is reduced. If the quantitative relation between the feedback quantity of the system state variables and the filtering precision can be established, the feedback quantity of each system state is determined according to the filtering precision, and the precision of the integrated navigation system can be fundamentally improved.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the shortcomings of the prior art are overcome, and an adaptive hybrid correction method for a combined navigation system is provided. Aiming at an inertia/satellite combined navigation system, an observability matrix of the improved combined navigation system is defined by using a weighted least square method, the state observability degree of the combined navigation system is further defined, and the observability degree is normalized according to historical measurement data. And finally, taking the normalized observability degree as a self-adaptive factor to realize the self-adaptive mixed correction of the state of the inertial/satellite combined navigation system.
The technical solution of the invention is as follows: an inertial/satellite system adaptive hybrid correction method based on observability degree comprises the following implementation steps:
(1) establishing a strapdown inertia/satellite combined navigation system model, solving weighted least square estimation of an initial state, obtaining an optimal estimation error covariance matrix under the meaning of minimum mean square error, and defining an observability matrix;
(2) modifying the definition of the observability matrix, and defining the observability degree of each state variable;
(3) normalizing the observability degree of each state according to the historical data of the strapdown inertial/satellite integrated navigation system;
(4) and (4) taking the normalized observability degree obtained in the step (3) as an adaptive factor to design adaptive mixed correction for the strapdown inertial/satellite integrated navigation system.
The step (1) is specifically realized as follows:
the discretization strapdown inertia/satellite integrated navigation system equation is as follows:
wherein, XkIs the n-dimensional state at time k; xk-1Is the n-dimensional state at the time of k-1; zkM-dimensional measurement vectors at the k moment; phik,k-1One-step state transition moment of system from time k-1 to time kArraying; gamma-shapedk-1Driving the array for system noise; wk-1The system excitation noise sequence at the time k-1 and the variance is recorded as Qk-1;HkA measurement matrix at the time k; vkThe variance of the measured noise sequence is denoted as Rk;
Obtaining the observed value and the initial state X by using the measurement augmentation technology1The corresponding relation of (1):
wherein Θ is1,k=[H1 H2Φ2,1 ... HkΦk,k-1...Φ2,1]T,V1,k=[V1 V2 ... Vk]TIs Gaussian white noise, and the variance is recorded as R1,kAnd obtaining an initial state estimation value according to a weighted least square estimation method:
wherein W
1,kFor the weighting matrix, the deviation between the estimated value and the actual value of the initial state is recorded as the estimation error
The estimation error covariance matrix is expressed as:
weighting array
According to the Cauchy-Schwarz inequality, the minimum value of the covariance matrix of the estimation error can be obtained:
defining the observability matrix as:
the step (2) improves the definition of the observability matrix, defines the observability degree of each state variable, and is specifically realized as follows:
only the measurement noise at the k-th moment is considered in the observability analysis, and the improved observability matrix is as follows:
the observability measure defining the jth state variable is:
the index j indicates the jth element on the diagonal of the matrix.
The step (3) of normalizing the observability measure is specifically realized as follows:
wherein
Represents the maximum value in the observable corresponding to the jth state at the previous k moment.
The step (4) is specifically realized by designing the adaptive hybrid correction based on the observability degree as follows:
hybrid correction refers to estimated navigation parameter errors
Simultaneous correction of navigation system output X
kAnd error-shape in inertial navigation systemsState. The self-adaptive mixed correction is to introduce observability degree information corresponding to the system state in the mixed correction and correct the system state by using a filtering result weighted by the observability degree. The filter equation of the adaptive mixed correction comprises a time updating equation, a measurement updating equation and an adaptive equation:
the time update equation:
measurement update equation:
Pk=(I-KkHk)Pk,k-1
the adaptive equation:
wherein 0 represents a zero vector;
representing a state one-step prediction; k
kRepresenting a filter gain matrix; p
k.k-1Representing a one-step predicted mean square error; p
kRepresenting the estimated mean square error;
a matrix of observable measures representing the correspondence of different states of the system,
and the observable degree of the j state variable at the k moment is represented, and the function of the observable degree is realized by adaptively adjusting output correction and feedback correction. When the observability degree of a certain state of the system is close to 1, the error value estimated by the filter is more accurate, and the state error estimation value is more fully utilized; when the observability degree of a certain state of the system is close to 0, the state is not observable and is not corrected by the state error estimation value.
Has the advantages that:
the invention introduces the observability degree aiming at the condition of low state estimation precision of the combined navigation system part, improves the correction mode according to the matching of the estimation precision and the observability degree, and improves the navigation precision. Compared with the prior definition, the observability degree provided by the invention considers the influence of measurement noise more and has more accurate reaction on the state estimation precision. The hybrid correction mode provided by the invention is different from the prior complete feedback, the state estimation value is subjected to weighted feedback, and the influence of the lower-precision estimation value on the precision of the whole navigation system can be effectively reduced.
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, rather than all embodiments, and all other embodiments obtained by a person skilled in the art based on the embodiments of the present invention belong to the protection scope of the present invention without creative efforts.
As shown in fig. 1, the specific implementation steps of the present invention are as follows:
1. defining the observability matrix is implemented as follows:
the discretization strapdown inertia/satellite integrated navigation system equation is as follows:
wherein, XkIs the n-dimensional state at time k; xk-1Is the n-dimensional state at the time of k-1; zkM-dimensional measurement vectors at the k moment; phik,k-1A one-step state transition matrix of the system from the moment k-1 to the moment k; gamma-shapedk-1Driving the array for system noise; wk-1The system excitation noise sequence at the time k-1 and the variance is recorded as Qk-1;HkA measurement matrix at the time k; vkThe variance of the measured noise sequence is denoted as Rk。
Obtaining the observed value and the initial state X by using the measurement augmentation technology1The corresponding relation of (1):
wherein Θ is1,k=[H1 H2Φ2,1 ... HkΦk,k-1...Φ2,1]T,V1,k=[V1 V2 ... Vk]TIs Gaussian white noise, and the variance is recorded as R1,kAnd obtaining an initial state estimation value according to a weighted least square estimation method:
wherein W
1,kFor the weighting matrix, the deviation between the estimated value and the actual value of the initial state is recorded as the estimation error
The estimation error covariance matrix is expressed as:
weighting array
According to the Cauchy-Schwarz inequality, the minimum value of the covariance matrix of the estimation error can be obtained:
defining the observability matrix as:
2. the definition of the observability matrix is improved, and simultaneously, the definition of the observability degree of each state variable is given, and the specific implementation is as follows:
only the measurement noise at the k-th moment is considered in the observability analysis, and the improved observability matrix is as follows:
the observability measure defining the jth state variable is:
the index j indicates the jth element on the diagonal of the matrix.
3. The observability degree is normalized as follows:
wherein
Representing observable corresponding to jth state at the previous k momentThe maximum value in degrees.
4. The adaptive hybrid correction is designed based on the observability degree and is specifically realized as follows:
hybrid correction refers to estimated navigation parameter errors
Simultaneous correction of navigation system output X
kAnd error conditions in inertial navigation systems. The self-adaptive mixed correction is to introduce observability degree information corresponding to the system state in the mixed correction and correct the system state by using a filtering result weighted by the observability degree. The filter equation of the adaptive mixed correction comprises a time updating equation, a measurement updating equation and an adaptive equation.
The time update equation:
measurement update equation:
Pk=(I-KkHk)Pk,k-1
the adaptive equation:
wherein 0 represents a zero vector;
representing a state one-step prediction; k
kRepresenting a filter gain matrix; p
k.k-1Representing a one-step predicted mean square error; p
kRepresenting the estimated mean square error;
a matrix of observable measures representing the correspondence of different states of the system,
and the observable degree of the j state variable at the k moment is represented, and the function of the observable degree is realized by adaptively adjusting output correction and feedback correction. When the observability degree of a certain state of the system is close to 1, the error value estimated by the filter is more accurate, and the state error estimation value is more fully utilized; when the observability degree of a certain state of the system is close to 0, the state is not observable and is not corrected by the state error estimation value.
A block diagram of the system is shown in fig. 2. The inertial navigation and the satellite navigation provide measurement data, and an estimated value of the system state is obtained through Kalman filtering. Weighting the navigation parameter error estimation value by using the normalized observability degree, and finally correcting the output of the combined navigation system and the parameters of the inertial navigation system, wherein the corrected navigation parameters are
Although illustrative embodiments of the present invention have been described above to facilitate the understanding of the present invention by those skilled in the art, it should be understood that the present invention is not limited to the scope of the embodiments, but various changes may be apparent to those skilled in the art, and it is intended that all inventive concepts utilizing the inventive concepts set forth herein be protected without departing from the spirit and scope of the present invention as defined and limited by the appended claims.