CN113984054A - Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment - Google Patents

Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment Download PDF

Info

Publication number
CN113984054A
CN113984054A CN202111095186.XA CN202111095186A CN113984054A CN 113984054 A CN113984054 A CN 113984054A CN 202111095186 A CN202111095186 A CN 202111095186A CN 113984054 A CN113984054 A CN 113984054A
Authority
CN
China
Prior art keywords
information
measurement
filtering
adaptive
noise
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
CN202111095186.XA
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.)
Lanzhou Jiaotong University
Original Assignee
Lanzhou Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Lanzhou Jiaotong University filed Critical Lanzhou Jiaotong University
Priority to CN202111095186.XA priority Critical patent/CN113984054A/en
Publication of CN113984054A publication Critical patent/CN113984054A/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention relates to an improved Sage-Husa self-adaptive fusion filtering method and multi-source information fusion equipment based on information anomaly detection, wherein 1, sensor measurement information and GPS longitude and latitude information are obtained; 2, establishing a GPS/INS integrated navigation system model, and establishing a state equation and a measurement equation; 3, an information anomaly detection process, namely constructing test statistics according to the prediction residual vector, judging whether abnormal observation exists, and adopting improved Sage-Husa adaptive filtering to set Kalman filtering gain to zero and introduce an exponential decay adaptive factor to adjust observation measurement noise when the system has abnormal measurement detection; and 4, carrying out filtering processing on the integrated navigation system by using the improved Sage-Husa self-adaptive filtering method, and predicting and correcting the Q array and the R array in real time on the basis of standard Kalman filtering. The multi-source information fusion equipment comprises a sensor, a processor, an information acquisition unit and a data transmission and receiving unit. Has the advantages that: and an information abnormity detection process is added, so that the navigation precision and fault tolerance of the system are improved.

Description

Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment
Technical Field
The invention belongs to the technical field of navigation positioning, and relates to an improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment.
Background
The multi-source information fusion is a technology for summarizing and integrating collected data output by each sensor and combining an optimal effect by adopting a certain rule. In the field of navigation and positioning of intelligent traffic systems, the most classical, efficient and feasible method for multi-source information fusion is the Kalman Filtering (KF). However, the algorithm has some limitations, and systematic errors are continuously accumulated in the calculation process of the Kalman filter, so that the positive nature of the error covariance matrix is affected, and the filtering estimation result is inaccurate, so that improvement is needed.
Disclosure of Invention
The invention aims to provide an improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment.
The technical scheme of the invention is as follows: an improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection comprises the following steps: acquiring measurement information of a sensor; a gyroscope and an accelerometer in the inertial measurement unit output corresponding measurement information of angular velocity and specific force, and a GPS outputs corresponding longitude and latitude measurement information;
step two: establishing a GPS/INS integrated navigation system model, determining a multidimensional state quantity formed by position, speed, attitude and deviation quantity, and establishing a state equation and a measurement equation;
step three: in the information anomaly detection process, test statistic is constructed according to the prediction residual vector, and whether anomaly observation exists is judged; when the system has no abnormal quantity measurement, the test statistic does not exceed the confidence limit, and standard Kalman filtering is adopted for prediction and correction, so that a fusion filtering result is output; when the system has abnormal measurement detection, adopting improved Sage-Husa adaptive filtering, setting Kalman filtering gain to zero, and introducing an exponential decay adaptive factor to adjust observation measurement noise;
step four: the improved Sage-Husa self-adaptive filtering method carries out filtering processing on the integrated navigation system, carries out real-time prediction and correction on a Q array and an R array on the basis of standard Kalman filtering, feeds back the self-adaptive adjusting process of adjusting the filtering gain K, and simultaneously sets the filtering gain to zero when the information is abnormal and restores the filtering gain to the standard Kalman, thereby realizing the purpose of inhibiting the influence of the information abnormality on the filtering; the improved Sage-Husa adaptive filtering algorithm flow is as follows:
initialized state estimates and covariance
Figure BDA0003268953820000021
Figure BDA0003268953820000022
Figure BDA0003268953820000023
Judgment of
Figure BDA0003268953820000024
If the signal is in the confidence space, if so, standard Kalman filtering is carried out, otherwise,
then, performing robust adaptive filtering, the process is:
by
Figure BDA0003268953820000025
To obtain
Figure BDA0003268953820000026
And K (: i) is 0, gives,
Figure BDA0003268953820000027
Figure BDA0003268953820000028
Pk＝[I-KkHk]Pk，k-1
Figure BDA0003268953820000029
Figure BDA00032689538200000210
finally, obtain
Figure BDA0003268953820000031
Figure BDA0003268953820000032
For one-step prediction of state, phik，k-1In order to be a state transition matrix,
Figure BDA0003268953820000033
is the state estimator at time k-1,
Figure BDA0003268953820000034
is the mean value of white noise of the system at the time k, Pk，k-1To predict the state covariance matrix, Φk，k-1Being a state transition matrix, Pk-1In order to observe the matrix, the system,
Figure BDA0003268953820000035
in the form of a covariance matrix,
Figure BDA0003268953820000036
for prediction residual estimation, ZkTo predict residual, HkIs the state quantity of the system, and the state quantity of the system,
Figure BDA0003268953820000037
is rkThe estimated amount of (a) is,
Figure BDA0003268953820000038
in order to predict the residual estimate, the residual estimate is,
Figure BDA0003268953820000039
to predict the mean square error of the residual, dkIn order to be a factor for the adaptation,
Figure BDA00032689538200000310
is rk-1The estimated amount of (a) is,
Figure BDA00032689538200000311
as a noise covariance matrix, betakIs the same as defined in formula (16),
Figure BDA00032689538200000312
for observing noise covariance matrix, RmaxFor observing noise covariance matrix minimum，
Figure BDA00032689538200000313
Is the same as defined in formula (16),
Figure BDA00032689538200000314
is ZkThe transpose matrix of (a) is,
Figure BDA00032689538200000315
is HkTransposed matrix of (2), RmaxFor observing the maximum value of the noise covariance matrix, KkIn order to be a matrix of gains, the gain matrix,
Figure BDA00032689538200000316
in order to perform a one-step pre-measurement of the state,
Figure BDA00032689538200000317
is a covariance matrix of the state noise,
Figure BDA00032689538200000318
in order to be a state estimator,
Figure BDA00032689538200000319
for prediction residual estimation, PkIn order to observe the matrix, the system,
Figure BDA00032689538200000320
Figure BDA00032689538200000321
white noise mean at time k, dkIn order to be a factor for the adaptation,
Figure BDA00032689538200000322
is the average value of the white noise of the system at the moment k-1,
Figure BDA00032689538200000323
for state estimators at time k, phik，k-1In order to be a state transition matrix,
Figure BDA00032689538200000324
in the form of a covariance matrix,
Figure BDA00032689538200000325
in order to predict the residual error(s),
Figure BDA00032689538200000326
in order to be a transpose of the prediction residual,
Figure BDA00032689538200000327
for transposing the gain matrix,. phik，k-1Being a state transition matrix, phik，k-1 TIs a transpose of the state transition matrix.
In the second step, the established state equation is as follows:
Figure BDA00032689538200000328
in the formula (1), X (k) is a state variable, F (k) is a system state transition matrix, and G (k) is a system noise transition matrix; w (k) is the system noise vector, X (k) is the state variable,
selecting a state variable X as:
Figure BDA0003268953820000041
in the formula (2), [ phi ]E φN φU]The attitude misalignment angles in the east, north and sky directions of the inertial platform are shown, and the unit is an angle division; [ Delta VEδVN δVU]The unit of the speed error is meter/second, wherein the unit of the speed error is east, north and sky; [ Delta L Delta Lambda Delta h]Errors representing latitude, longitude, altitude, in meters; [ epsilon ]x εy εz]The constant drift error of the gyroscope is unit degree/hour;
Figure BDA0003268953820000042
is the drift error of the accelerometer, in ug,
the measurement equation established in the second step is as follows:
Figure BDA0003268953820000043
in the formula (9), Zv(t) is a velocity measurement vector, Zp(t) is the position measurement vector, V (t) is the observation noise, and the velocity measurement vector is:
Figure BDA0003268953820000044
in the formula (10), Hv＝[03×3 diag(1 1 1) 03×9]，Vv＝[vGE vGN vGU]T，vGE、vGN、vGUAre speed errors of the GNSS along the east, north and sky directions respectively,
the position measurement vector is:
Figure BDA0003268953820000045
in the formula (11), Hp＝[03×6 diag(1 1 1) 03×6]，Vp＝[NGE NGN NGU]T，NGE、NGN、NGUThe position errors of the GNSS in the east, north and sky directions are respectively.
In the information anomaly detection process in the third step, the prediction residual vector is used for constructing test statistic, so as to judge whether an observation anomaly error exists or not,
prediction residual
Figure BDA0003268953820000051
Actual measurement value Z representing time kkAnd measure one-step prediction
Figure BDA0003268953820000052
The error between the two is defined as:
Figure BDA0003268953820000053
in the formula (12), X (k) is a state vector at the time k, H (k) is a system measurement moment, VkIn order to observe the noise, it is,
Figure BDA0003268953820000054
in order to predict the state in one step,
Figure BDA0003268953820000055
mean square error matrix of prediction residuals
Figure BDA0003268953820000056
Then
Figure BDA0003268953820000057
In the formula (13), the reaction mixture is,
Figure BDA0003268953820000058
Figure BDA0003268953820000059
Figure BDA00032689538200000510
Hkmeasuring the noise matrix, V, for the systemkIn order to observe the noise matrix,
the measurement information prediction residual error is a white noise sequence, the obedient mean value is zero, and the variance is
Figure BDA00032689538200000511
Is normally distributed, i.e.
Figure BDA00032689538200000512
Normalizing the test data to obtain test statistic as follows:
Figure BDA00032689538200000513
in the formula (15), the reaction mixture is,
Figure BDA00032689538200000514
Figure BDA00032689538200000515
the ith row element of the observation matrix representing the time instant at time k,
Figure BDA0003268953820000061
represents the diagonal elements of the observed noise covariance matrix,
assume confidence level of
Figure BDA00032689538200000610
If the test statistic does not exceed the confidence limit, the observation is not abnormal; if the confidence limit is exceeded, an exponential decay adaptive factor is introduced to adjust the observation noise covariance matrix, and the purpose of identifying and inhibiting the abnormal influence of the observed quantity is achieved.
The exponential decay adaptive factor automatically adjusts the observation noise process: the variance of the prediction residual can be derived from equation (13)
Figure BDA0003268953820000062
The expression of (a) is:
Figure BDA0003268953820000063
the variance of the prediction residual represents the lumped average of the random sequence, and can be replaced by the time average in the discretization equation, and equation (14) is shifted by terms, and the observed noise covariance matrix can be rewritten as:
Figure BDA0003268953820000064
in the above formula (15)
Figure BDA0003268953820000065
By b, 0<b<1, instead of, let
Figure BDA0003268953820000066
Formula (14) to:
Figure BDA0003268953820000067
considering that the observation anomaly error may be large, the noise covariance calculated by equation (16) will increase the effect of anomaly observation
Figure BDA0003268953820000068
Equation (16) can be expressed as:
Figure BDA0003268953820000069
when an exponential decay adaptive factor is introduced to update the filter, observation noise is automatically adjusted according to the prediction residual error, the upper limit and the lower limit of the noise variance are set, the filtering precision is prevented from being reduced when the matrix inversion is negative, and meanwhile, if the difference between two adjacent iterations does not exceed the limit, the iteration is stopped.
A multi-source information fusion device includes a memory unit for storing a computer program;
a processor unit for implementing the improved Sage-Husa adaptive fusion filtering method as described above when executing a computer program; the processor unit receives external information to be processed.
The system further comprises an information acquisition unit and a data transmission and receiving unit, wherein the information acquisition unit comprises an IMU inertial navigation sensor and a GPS satellite receiver and is used for acquiring the measurement information of the angular velocity and the specific force output by a gyroscope and an accelerometer and outputting the corresponding longitude and latitude measurement information by the GPS;
and the data transmission and receiving unit is used for transmitting the information obtained by the information acquisition unit to the processor unit.
The invention has the beneficial effects that: 1. compared with the traditional Sage-Husa self-adaptive fusion filtering, the method adds an information anomaly detection process, and improves the navigation precision and fault tolerance of the system. 2. The method solves the problems that in Sage-Husa self-adaptive fusion filtering, information is continuously set to zero when continuous information abnormity occurs, and the system navigation resolving error is increased, adaptively adjusts the measurement noise variance, and effectively controls the influence of the measurement abnormity on the filtering result.
Drawings
FIG. 1 is a process diagram of an improved Sage-Husa adaptive fusion filtering scheme based on information anomaly detection;
FIG. 2 is a graph of experimental simulation traces;
FIG. 3 is a comparison plot of the positioning error of the prior Sage-Husa method and the modified Sage-Husa method.
Detailed Description
An improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection comprises the following steps: acquiring measurement information of a sensor; a gyroscope and an accelerometer in the inertial measurement unit output corresponding measurement information of angular velocity and specific force, and a GPS outputs corresponding longitude and latitude measurement information;
step two: establishing a GPS/INS integrated navigation system model, determining a multidimensional state quantity formed by position, speed, attitude and deviation quantity, and establishing a state equation and a measurement equation;
step three: in the information anomaly detection process, test statistic is constructed according to the prediction residual vector, and whether anomaly observation exists is judged; when the system has no abnormal quantity measurement, the test statistic does not exceed the confidence limit, and standard Kalman filtering is adopted for prediction and correction, so that a fusion filtering result is output; when the system has abnormal measurement detection, adopting improved Sage-Husa adaptive filtering, setting Kalman filtering gain to zero, and introducing an exponential decay adaptive factor to adjust observation measurement noise;
step four: the improved Sage-Husa self-adaptive filtering method carries out filtering processing on the integrated navigation system, carries out real-time prediction and correction on a Q array and an R array on the basis of standard Kalman filtering, feeds back the self-adaptive adjusting process of adjusting the filtering gain K, and simultaneously sets the filtering gain to zero when the information is abnormal and restores the filtering gain to the standard Kalman, thereby realizing the purpose of inhibiting the influence of the information abnormality on the filtering; the algorithm flow is shown in fig. 1.
In the second step, the established state equation is as follows:
Figure BDA0003268953820000081
in the formula (1), X (k) is a state variable, F (k) is a system state transition matrix, and G (k) is a system noise transition matrix; w (k) is the system noise vector,
selecting a state variable X as:
Figure BDA0003268953820000082
in the formula (2), [ phi ]E φN φU]The attitude misalignment angles in the east, north and sky directions of the inertial platform are shown, and the unit is an angle division; [ Delta VEδVN δVU]The unit of the speed error is meter/second, wherein the unit of the speed error is east, north and sky; [ Delta L Delta Lambda Delta h]Errors representing latitude, longitude, altitude, in meters; [ epsilon ]x εy εz]The constant drift error of the gyroscope is unit degree/hour;
Figure BDA0003268953820000083
is the drift error of the accelerometer, in ug,
the measurement equation established in the second step is as follows:
Figure BDA0003268953820000084
in the formula (9), Zv(t) is a velocity measurement vector, Zp(t) is the position measurement vector, V (t) is the observation noise, and the velocity measurement vector is:
Figure BDA0003268953820000085
in the formula (10), Hv＝[03×3 diag(1 1 1) 03×9]，Vv＝[vGE vGN vGU]T，vGE、vGN、vGUAre speed errors of the GNSS along the east, north and sky directions respectively,
the position measurement vector is:
Figure BDA0003268953820000091
in the formula (11), Hp＝[03×6 diag(1 1 1) 03×6]，Vp＝[NGE NGN NGU]T，NGE、NGN、NGUThe position errors of the GNSS in the east, north and sky directions are respectively.
In the information anomaly detection process in the third step, the prediction residual vector is used for constructing test statistic, so as to judge whether an observation anomaly error exists or not,
prediction residual
Figure BDA0003268953820000092
Actual measurement value Z representing time kkAnd measure one-step prediction
Figure BDA0003268953820000093
The error between the two is defined as:
Figure BDA0003268953820000094
mean square error matrix of prediction residuals
Figure BDA0003268953820000095
Then
Figure BDA0003268953820000096
The measurement information prediction residual error is a white noise sequence, the obedient mean value is zero, and the variance is
Figure BDA0003268953820000097
Is normally distributed, i.e.
Figure BDA0003268953820000098
Normalizing the test data to obtain test statistic as follows:
Figure BDA0003268953820000099
in the formula (15), the reaction mixture is,
Figure BDA00032689538200000910
Figure BDA00032689538200000911
the ith row element of the observation matrix representing the time instant at time k,
Figure BDA0003268953820000101
represents the diagonal elements of the observed noise covariance matrix,
assume confidence level of
Figure BDA00032689538200001010
If the test statistic does not exceed the confidence limit, the observation is not abnormal; if the confidence limit is exceeded, an exponential decay adaptive factor is introduced to adjust the observation noise covariance matrix, and the purpose of identifying and inhibiting the abnormal influence of the observed quantity is achieved.
The exponential decay adaptive factor automatically adjusts the observation noise process: the variance of the prediction residual can be derived from equation (13)
Figure BDA0003268953820000102
The expression of (a) is:
Figure BDA0003268953820000103
the variance of the prediction residual represents the lumped average of the random sequence, and can be replaced by the time average in the discretization equation, and equation (14) is shifted by terms, and the observed noise covariance matrix can be rewritten as:
Figure BDA0003268953820000104
in the above formula (15)
Figure BDA0003268953820000105
By b, 0<b<1, instead of, let
Figure BDA0003268953820000106
Formula (14) to:
Figure BDA0003268953820000107
considering that the observation anomaly error may be large, the noise covariance calculated by equation (16) will increase the effect of anomaly observation
Figure BDA0003268953820000108
Equation (16) can be expressed as:
Figure BDA0003268953820000109
when an exponential decay adaptive factor is introduced to update the filter, observation noise is automatically adjusted according to the prediction residual error, the upper limit and the lower limit of the noise variance are set, the filtering precision is prevented from being reduced when the matrix inversion is negative, and meanwhile, if the difference between two adjacent iterations does not exceed the limit, the iteration is stopped.
The algorithm flow of the improved Sage-Husa adaptive filtering in the fourth step is shown in FIG. 1.
The method is realized by using a multi-source information fusion device which comprises a memory unit and a multi-source information fusion unit, wherein the memory unit is used for storing a computer program; the processor unit is used for realizing the improved Sage-Husa self-adaptive fusion filtering method when executing a computer program; the processor unit receives external information to be processed. The system further comprises an information acquisition unit and a data transmission and receiving unit, wherein the information acquisition unit comprises an IMU inertial navigation sensor and a GPS satellite receiver and is used for acquiring the measurement information of the angular velocity and the specific force output by a gyroscope and an accelerometer and outputting the corresponding longitude and latitude measurement information by the GPS; and the data transmission and receiving unit is used for transmitting the information obtained by the information acquisition unit to the processor unit.
As seen from FIG. 3, RAKF fluctuation is obviously smaller than AKF, which shows that the method has obvious improvement effect compared with the original method, and the maximum error value is reduced from about 3.8m to about 2.8 m.

Claims (7)

1. An improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection is characterized by comprising the following steps: the method comprises the following steps: acquiring measurement information of a sensor; a gyroscope and an accelerometer in the inertial measurement unit output corresponding measurement information of angular velocity and specific force, and a GPS outputs corresponding longitude and latitude measurement information;
step two: establishing a GPS/INS integrated navigation system model, determining a multidimensional state quantity formed by position, speed, attitude and deviation quantity, and establishing a state equation and a measurement equation;
step three: in the information anomaly detection process, test statistic is constructed according to the prediction residual vector, and whether anomaly observation exists is judged; when the system has no abnormal quantity measurement, the test statistic does not exceed the confidence limit, and standard Kalman filtering is adopted for prediction and correction, so that a fusion filtering result is output; when the system has abnormal measurement detection, adopting improved Sage-Husa adaptive filtering, setting Kalman filtering gain to zero, and introducing an exponential decay adaptive factor to adjust observation measurement noise;
step four: the improved Sage-Husa self-adaptive filtering method carries out filtering processing on the integrated navigation system, carries out real-time prediction and correction on a Q array and an R array on the basis of standard Kalman filtering, feeds back the self-adaptive adjusting process of adjusting the filtering gain K, and simultaneously sets the filtering gain to zero when the information is abnormal and restores the filtering gain to the standard Kalman, thereby realizing the purpose of inhibiting the influence of the information abnormality on the filtering; the improved Sage-Husa adaptive filtering algorithm flow is as follows:
initialized state estimates and covariance
Figure FDA0003268953810000011
Figure FDA0003268953810000012
Figure FDA0003268953810000013
Judgment of
Figure FDA0003268953810000014
If the signal is in the confidence space, if so, standard Kalman filtering is carried out, otherwise,
then, performing robust adaptive filtering, the process is:
by
Figure FDA0003268953810000015
To obtain
Figure FDA0003268953810000021
And K (: i) is 0, gives
Figure FDA0003268953810000022
Figure FDA0003268953810000023
Pk＝[I-KkHk]Pk，k-1
Figure FDA0003268953810000024
Figure FDA0003268953810000025
Finally, obtain
Figure FDA0003268953810000026
2. The improved Sage-Husa adaptive fusion filtering method based on information anomaly detection according to claim 1, characterized in that: in the second step, the established state equation is as follows:
Figure FDA0003268953810000027
in the formula (1), X (k) is a state variable, F (k) is a system state transition matrix, and G (k) is a system noise transition matrix; w (k) is the system noise vector, X (k) is the state variable,
selecting a state variable X as:
Figure FDA0003268953810000028
in the formula (2), [ phi ]E φN φU]The attitude misalignment angles in the east, north and sky directions of the inertial platform are shown, and the unit is an angle division; [ Delta VE δVN δVU]The unit of the speed error is meter/second, wherein the unit of the speed error is east, north and sky; [ Delta L Delta Lambda Delta h]Errors representing latitude, longitude, altitude, in meters; [ epsilon ]x εy εz]The constant drift error of the gyroscope is unit degree/hour;
Figure FDA0003268953810000031
the drift error of the accelerometer is in ug.
3. The improved Sage-Husa adaptive fusion filtering method based on information anomaly detection according to claim 1, characterized in that: the measurement equation established in the second step is as follows:
Figure FDA0003268953810000032
in the formula (9), Zv(t) is a velocity measurement vector, Zp(t) is a position measurement vector, Hv、HpRespectively, a system state parameter, X (t) is a state vector, Vv(t) velocity observation noise, Vp(t) position observation noise, H (t) system measurement matrix, and V (t) observation noise;
the velocity measurement vector is:
Figure FDA0003268953810000033
in the formula (10), Hv＝[03×3 diag(1 1 1) 03×9]，Vv＝[vGE vGN vGU]T，vGE、vGN、vGUVelocity errors of GNSS along east, north and sky directions, vIE、vIN、vIUVelocity errors of IMU along east, north and sky directions, X (t) is a state vector, Vv(t) velocity observation noise;
the position measurement vector is:
Figure FDA0003268953810000034
in the formula (11), the reaction mixture is,
Figure FDA0003268953810000041
indicating the position error H in three directions along the northeastp＝[03×6 diag(1 1 1) 03×6]，Vp＝[NGE NGN NGU]T，NGE、NGN、NGUThe position errors of the GNSS in the east, north and sky directions are respectively.
4. The improved Sage-Husa adaptive fusion filtering method based on information anomaly detection according to claim 1, characterized in that: in the information anomaly detection process in the third step, the prediction residual vector is used for constructing test statistic, so as to judge whether an observation anomaly error exists or not,
prediction residual
Figure FDA0003268953810000042
Actual measurement value Z representing time kkAnd measure one-step prediction
Figure FDA0003268953810000043
The error between the two is defined as:
Figure FDA0003268953810000044
in the formula (12), X (k) is a state vector at the time k, H (k) is a system measurement moment, VkIn order to observe the noise, it is,
Figure FDA0003268953810000045
in order to predict the state in one step,
Figure FDA0003268953810000046
mean square error matrix of prediction residuals
Figure FDA0003268953810000047
Then
Figure FDA0003268953810000048
In the formula (13), the reaction mixture is,
Figure FDA0003268953810000049
Figure FDA00032689538100000410
Hkmeasuring the noise matrix, V, for the systemkIn order to observe the noise matrix,
the measurement information prediction residual error is a white noise sequence, the obedient mean value is zero, and the variance is
Figure FDA00032689538100000411
Is normally distributed, i.e.
Figure FDA0003268953810000051
Normalizing the test data to obtain test statistic as follows:
Figure FDA0003268953810000052
in the formula (15), the reaction mixture is,
Figure FDA0003268953810000053
Figure FDA0003268953810000054
the ith row element of the observation matrix representing the time instant at time k,
Figure FDA0003268953810000055
is composed of
Figure FDA0003268953810000056
The transpose matrix of (a) is,
Figure FDA0003268953810000057
represents the diagonal elements of the observed noise covariance matrix,
assume confidence level of
Figure FDA0003268953810000058
If the test statistic does not exceed the confidence limit, the observation is not abnormal; if the confidence limit is exceeded, an exponential decay adaptive factor is introduced to adjust the observation noise covariance matrix, and the purpose of identifying and inhibiting the abnormal influence of the observed quantity is achieved.
5. The improved Sage-Husa adaptive fusion filtering method based on information anomaly detection according to claim 4, characterized in that: the exponential decay adaptive factor automatically adjusts the observation noise process: the variance of the prediction residual is obtained by equation (13)
Figure FDA0003268953810000059
The expression of (a) is:
Figure FDA00032689538100000510
the variance of the prediction residual represents the lumped average of the random sequence, and is replaced by the time average in the discretization equation, equation (14) is shifted, and the observed noise covariance matrix can be rewritten as:
Figure FDA00032689538100000511
in the above formula (15)
Figure FDA00032689538100000512
By b, 0<b<1, instead of, let
Figure FDA00032689538100000513
Formula (14) to:
Figure FDA00032689538100000514
considering that the observation anomaly error may be large, the noise covariance calculated by equation (16) will increase the effect of anomaly observation
Figure FDA00032689538100000515
Equation (16) is therefore expressed as:
Figure FDA0003268953810000061
when an exponential decay adaptive factor is introduced to update the filter, observation noise is automatically adjusted according to the prediction residual error, the upper limit and the lower limit of the noise variance are set, the filtering precision is prevented from being reduced when the matrix inversion is negative, and meanwhile, if the difference between two adjacent iterations does not exceed the limit, the iteration is stopped.
6. A multi-source information fusion device is characterized in that: comprises a memory unit for storing a computer program;
a processor unit for implementing the improved Sage-Husa adaptive fusion filtering method of claim 1 or 2 or 3 or 4 or 5 when executing a computer program; the processor unit receives external information to be processed.
7. The multi-source information fusion device of claim 6, wherein: the system comprises an information acquisition unit and a data transmission and receiving unit, wherein the information acquisition unit comprises an IMU inertial navigation sensor and a GPS satellite receiver and is used for acquiring the measurement information of angular velocity and specific force output by a gyroscope and an accelerometer and outputting the corresponding longitude and latitude measurement information by the GPS;
and the data transmission and receiving unit is used for transmitting the information obtained by the information acquisition unit to the processor unit.
CN202111095186.XA 2021-09-17 2021-09-17 Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment Pending CN113984054A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111095186.XA CN113984054A (en) 2021-09-17 2021-09-17 Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111095186.XA CN113984054A (en) 2021-09-17 2021-09-17 Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment

Publications (1)

Publication Number Publication Date
CN113984054A true CN113984054A (en) 2022-01-28

Family

ID=79736040

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111095186.XA Pending CN113984054A (en) 2021-09-17 2021-09-17 Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment

Country Status (1)

Country Link
CN (1) CN113984054A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115291253A (en) * 2022-08-02 2022-11-04 东北大学 Vehicle positioning integrity monitoring method and system based on residual error detection
CN115342820A (en) * 2022-10-18 2022-11-15 深圳市诚王创硕科技有限公司 Navigation positioning system for automobile driving at night
CN116086466A (en) * 2022-12-28 2023-05-09 淮阴工学院 Method for improving INS error precision

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system
CN108844540A (en) * 2018-09-11 2018-11-20 北京机械设备研究所 A kind of adaptive filter method of combination covariance and Sage-Husa filtering technique
CN109974714A (en) * 2019-04-29 2019-07-05 南京航空航天大学 A kind of adaptive Unscented kalman filtering attitude data fusion method of Sage-Husa
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN112254718A (en) * 2020-08-04 2021-01-22 东南大学 Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system
CN108844540A (en) * 2018-09-11 2018-11-20 北京机械设备研究所 A kind of adaptive filter method of combination covariance and Sage-Husa filtering technique
CN109974714A (en) * 2019-04-29 2019-07-05 南京航空航天大学 A kind of adaptive Unscented kalman filtering attitude data fusion method of Sage-Husa
CN111024064A (en) * 2019-11-25 2020-04-17 东南大学 SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN112254718A (en) * 2020-08-04 2021-01-22 东南大学 Motion constraint assisted underwater combined navigation method based on improved Sage-Husa adaptive filtering

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
刘江 等: "一种组合导航的自适应信息融合算法", 北京交通大学学报, vol. 32, no. 2 *
王冲 等: "基于Sage-husa自适应滤波算法的AUV组合导航系统设计", 电子设计工程, vol. 21, no. 15 *
葛宝爽 等: "基于新息异常检测的改进抗差自适应卡尔曼滤波算法", 导航定位与授时, vol. 7, no. 1 *
高怡 等: "抗差自适应Sage滤波及其在组合导航中的应用", 测控技术, vol. 34, no. 4 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115291253A (en) * 2022-08-02 2022-11-04 东北大学 Vehicle positioning integrity monitoring method and system based on residual error detection
CN115291253B (en) * 2022-08-02 2023-12-05 东北大学 Vehicle positioning integrity monitoring method and system based on residual error detection
CN115342820A (en) * 2022-10-18 2022-11-15 深圳市诚王创硕科技有限公司 Navigation positioning system for automobile driving at night
CN116086466A (en) * 2022-12-28 2023-05-09 淮阴工学院 Method for improving INS error precision
CN116086466B (en) * 2022-12-28 2024-03-26 淮阴工学院 Method for improving INS error precision

Similar Documents

Publication Publication Date Title
CN111780755B (en) Multi-source fusion navigation method based on factor graph and observability analysis
CN113984054A (en) Improved Sage-Husa self-adaptive fusion filtering method based on information anomaly detection and multi-source information fusion equipment
CN108827310B (en) Marine star sensor auxiliary gyroscope online calibration method
US6498996B1 (en) Vibration compensation for sensors
CN110487271A (en) Elman neural network aiding tight integration air navigation aid when a kind of GNSS signal is obstructed
CN111337020A (en) Factor graph fusion positioning method introducing robust estimation
CN112013836A (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
CN109724599A (en) A kind of Robust Kalman Filter SINS/DVL Combinated navigation method of anti-outlier
CN112505737B (en) GNSS/INS integrated navigation method
CN110954132B (en) GRNN-assisted self-adaptive Kalman filtering navigation fault identification method
CN109945859B (en) Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering
CN111024124B (en) Combined navigation fault diagnosis method for multi-sensor information fusion
CN112432644B (en) Unmanned ship integrated navigation method based on robust adaptive unscented Kalman filtering
CN112507281A (en) SINS/DVL tight combination system based on two-state multi-factor robust estimation
Cao et al. Anti-disturbance fault tolerant initial alignment for inertial navigation system subjected to multiple disturbances
CN113203429B (en) Online estimation and compensation method for temperature drift error of gyroscope
CN114689047B (en) Deep learning-based integrated navigation method, device, system and storage medium
CN112798021A (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
Sokolović et al. INS/GPS navigation system based on MEMS technologies
CN109974695B (en) Robust adaptive filtering method of surface ship navigation system based on Krein space
CN116147624A (en) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
Wang et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN109655057B (en) Filtering optimization method and system for accelerator measurement value of six-push unmanned aerial vehicle
CN115014321B (en) Bionic polarization multisource fusion orientation method based on adaptive robust filtering

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