CN112013836A - Attitude heading reference system algorithm based on improved adaptive Kalman filtering - Google Patents

Attitude heading reference system algorithm based on improved adaptive Kalman filtering Download PDF

Info

Publication number
CN112013836A
CN112013836A CN202010820409.3A CN202010820409A CN112013836A CN 112013836 A CN112013836 A CN 112013836A CN 202010820409 A CN202010820409 A CN 202010820409A CN 112013836 A CN112013836 A CN 112013836A
Authority
CN
China
Prior art keywords
attitude
noise
magnetic field
matrix
measurement
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.)
Granted
Application number
CN202010820409.3A
Other languages
Chinese (zh)
Other versions
CN112013836B (en
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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN202010820409.3A priority Critical patent/CN112013836B/en
Publication of CN112013836A publication Critical patent/CN112013836A/en
Application granted granted Critical
Publication of CN112013836B publication Critical patent/CN112013836B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention discloses an attitude and heading reference system algorithm based on improved adaptive Kalman filtering, which comprises the following steps: recovering the measured value of the interference magnetic field according to the reference value provided by the international geomagnetic reference field, and taking the residual magnetic field error as the measurement noise of the noise statistical characteristic change; providing a two-step adjustment adaptive Kalman filtering algorithm according to innovation covariance and residual covariance to process the observed quantity of noise statistical characteristic change; and establishing a dynamic equation and a measurement equation of the Kalman filter according to the attitude quaternion updating model and the sensor error model, and realizing the estimation of the attitude quaternion by adopting a two-step adjustment self-adaptive Kalman filtering algorithm. The invention can solve the technical problem of real-time calculation of the carrier attitude and the course in the presence of environmental magnetic interference.

Description

Attitude heading reference system algorithm based on improved adaptive Kalman filtering
Technical Field
The invention belongs to the technical field of strapdown attitude heading reference, and particularly relates to an attitude heading reference system algorithm based on improved adaptive Kalman filtering.
Background
An Attitude and Heading Reference System (AHRS) is a measuring device composed of a three-axis magnetometer, a three-axis gyroscope and a three-axis accelerometer, can provide Attitude and Heading information of a carrier under a space coordinate System, and is widely applied to the fields of aerospace, robots, automatic automobile drivers and pedestrian navigation positioning. The AHRS takes the gravity field and the geomagnetic field as reference vectors, and corrects the attitude calculated by the output value of the gyroscope by utilizing the output values of the accelerometer and the magnetometer, thereby avoiding the problem of attitude divergence and obtaining more accurate attitude and heading reference information. Although the AHRS can provide real-time attitude and heading information for the carrier, the application environment of the carrier is complex, and the magnetometer is often interfered by an external magnetic field, so that the output value cannot obtain accurate heading information, thereby affecting the control of the carrier and even causing the system to be incapable of operating normally.
The adaptive Kalman filtering technique is used in filtering systems where the state noise variance matrix and the measurement noise variance matrix cannot be accurately acquired. Common adaptive filtering methods include constructing a variance scaling function, multi-model combining, and online estimation of noise parameters. The online Estimation of noise parameters includes Innovation-based Adaptive Estimation (IAE) and Residual-based Adaptive Estimation (RAE) methods. The IAE obtains a more accurate noise variance matrix by comparing the estimated innovation covariance matrix with the theoretical innovation covariance matrix. The estimated innovation covariance matrix is typically obtained by innovation computation within a window. Theoretically, the larger the window length is, the more accurate the innovation covariance estimation is, but the sensitivity of noise variance matrix adjustment is affected, so that the filtering precision is improved slightly. Conversely, reducing the window length can increase the sensitivity of noise covariance adjustment, but can reduce the accuracy of the innovation covariance estimation, even negating the estimated noise covariance, further causing filtering divergence. The RAE obtains a more accurate noise variance matrix by comparing the estimated residual covariance with the theoretical residual covariance. The estimated residual covariance is calculated from the residual within a window. Compared with IAE, even if the window length is very small and the residual covariance estimation is not completely accurate, the RAE can also ensure that the estimated noise variance is positive, so that the risk of divergence of a filtering system is reduced, but the RAE lags behind the noise parameter estimation by IAE for one filtering period, so that the adjustment lags, and the problem that the filtering precision is influenced due to inaccurate residual covariance matrix estimation caused by improper window length is also encountered.
Therefore, how to provide an attitude and heading reference system algorithm based on the improved adaptive kalman filtering is a problem that needs to be solved by those skilled in the art.
Disclosure of Invention
In view of the above, the invention provides an improved adaptive kalman filter-based attitude and heading reference system algorithm, which can solve the technical problem of real-time calculation of the attitude and heading of a carrier in the presence of environmental magnetic interference.
In order to achieve the purpose, the invention adopts the following technical scheme:
an improved adaptive Kalman filtering-based attitude and heading reference system algorithm, comprising:
recovering the measured value of the interference magnetic field according to the reference value provided by the international geomagnetic reference field, and taking the residual magnetic field error as the measurement noise of the noise statistical characteristic change;
forming a two-step adjustment adaptive Kalman filtering algorithm according to the innovation covariance and the residual covariance to process the observed quantity of the noise statistical characteristic change;
and establishing a dynamic equation and a measurement equation of the Kalman filter according to the attitude quaternion updating model and the sensor error model, and realizing the estimation of the attitude quaternion by adopting a two-step adjustment self-adaptive Kalman filtering algorithm.
Preferably, a magnetic field reference value under a navigation system provided by an international geomagnetic reference field and an attitude matrix at a previous moment are used to obtain a geomagnetic field estimation value under a carrier system at the previous moment, the geomagnetic field estimation value is compared with a magnetic field measurement value to obtain a magnetic field interference value at the previous moment, and under the premise that the magnetic field interference values at the previous two measurement moments are not changed greatly, the magnetic field measurement value at the current moment is corrected by using the magnetic field interference value at the previous moment to form an interference magnetic field recovery algorithm so as to recover the interference magnetic field measurement value.
Preferably, the interference magnetic field measurement value is recovered according to the reference value provided by the international geomagnetic reference field, and the residual magnetic field error is regarded as the measurement noise of the noise statistical characteristic change, wherein the carrier system b is the magnetic field interference value at the k-1 moment
Figure BDA0002634234540000031
From the attitude matrix calculated at the time k-1 and the geomagnetic field reference value, i.e.
Figure BDA0002634234540000032
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000033
for magnetometer measurements containing magnetic interference,
Figure BDA0002634234540000034
as a magnetometer measurement at time k-1, m, containing magnetic interferencenA magnetic field reference value obtained for an international geomagnetic reference field,
Figure BDA0002634234540000035
a magnetic field reference value obtained for the international geomagnetic reference field at the time k-1,
Figure BDA0002634234540000036
in the form of a matrix of poses,
Figure BDA0002634234540000037
the attitude matrix at the moment of k-1, and n represents a navigation coordinate system;
the measured value at the k moment is corrected by the magnetic field interference value at the k-1 moment and the measured value of the geomagnetic field at the k moment is recovered, namely the measured value at the k moment is recovered by setting the magnetic field interference change between the k-1 moment and the k moment to be small
Figure BDA0002634234540000038
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000039
removing the geomagnetic estimation value of the external magnetic field interference for the k moment; measurement noise with residual magnetic field error as change in noise statistical properties
Figure BDA00026342345400000310
And further processed by an adaptive kalman filter.
Preferably, an innovation-based and residual-based noise parameter online estimation method is utilized, and two steps of adjustment strategies are utilized to combine the innovation-based adaptive estimation method and the residual-based adaptive estimation method, wherein in the first step, the Kalman filtering measurement updating is carried out by using an estimated noise covariance matrix provided by the innovation-based adaptive estimation, and in the second step, the noise covariance matrix obtained in the first step is corrected by using the estimated noise covariance matrix provided by the residual-based adaptive estimation for updating and resolving in the next filtering period, so that a two-step adjustment adaptive Kalman filtering algorithm is formed.
Preferably, the two-step adaptive kalman filtering algorithm formed according to the innovation covariance and the residual covariance to process the observed quantity of the noise statistical characteristic change comprises:
calculating innovation d of Kalman filtering at moment kkEstimate of innovation covariance
Figure BDA0002634234540000041
And obtaining an innovation-based measurement noise estimation covariance matrix by using variance matching variances
Figure BDA0002634234540000042
Namely, it is
Figure BDA0002634234540000043
Figure BDA0002634234540000044
Figure BDA0002634234540000045
In the formula, ZkIs an observed quantity at time k, HkIs a measurement matrix for the time k,
Figure BDA0002634234540000046
for the predicted state at time k obtained by one-step recursion, N is the length of the innovation sequence window used to estimate the innovation covarianceDegree, superscript T as transposition, Pk/k-1A prediction state error covariance matrix in Kalman filtering;
by
Figure BDA0002634234540000047
Obtaining the first adjusted k-time adaptive noise covariance matrix for Kalman update, i.e.
Figure BDA0002634234540000048
In the formula, Radapt,k-1Obtaining an adaptive noise covariance matrix based on the estimated residual covariance matrix at the time k-1;
calculating residual v of k momentkEstimate of residual covariance
Figure BDA0002634234540000049
And obtaining a residual-based measurement noise estimation covariance matrix by using variance matching variance
Figure BDA00026342345400000410
Namely, it is
Figure BDA00026342345400000411
Figure BDA00026342345400000412
Figure BDA00026342345400000413
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400000414
for the optimum filter state at time k, Pk/kIs a state error covariance matrix in Kalman filtering, vk-iResidual error at the k-i moment is obtained;
by using
Figure BDA0002634234540000051
Adaptive noise covariance matrix for first adjustment
Figure BDA0002634234540000052
Making a second correction, i.e.
Figure BDA0002634234540000053
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000054
for a theoretical residual covariance matrix, the symbol trace (·) represents the trace of the sampling matrix; the result of the quadratic correction will be used in the filtering solution for the next cycle.
Preferably, a nonlinear state equation is constructed by taking the attitude quaternion and the gyroscope zero offset as states, a nonlinear observation equation is constructed by taking the accelerometer measurement value and the restored magnetometer measurement value, the residual geomagnetic error is taken as measurement noise, two-step adjustment adaptive Kalman filtering is adopted to carry out adaptive adjustment on the measurement noise parameters, and the attitude quaternion and the gyroscope zero offset are estimated by utilizing a Kalman filtering technology to form an attitude heading reference system algorithm of the adaptive Kalman filtering.
Preferably, a dynamic equation and a measurement equation of the extended kalman filter are established according to the attitude quaternion updating model and the sensor error model, and the estimation of the attitude quaternion is realized by adopting a two-step adjustment adaptive extended kalman filtering algorithm, which comprises the following steps:
selecting an attitude quaternion q ═ q0 q1 q2 q3]TZero bias with gyroscopebIs a state quantity in which the differential equation of a quaternion is
Figure BDA0002634234540000055
In the formula, symbol
Figure BDA0002634234540000056
Which represents the multiplication of a quaternion,
Figure BDA0002634234540000057
is composed of
Figure BDA0002634234540000058
The formed zero-scalar quaternion is used for a low-precision micro-mechanical inertia device which cannot sense the rotational angular velocity of the earth
Figure BDA0002634234540000059
Figure BDA00026342345400000510
In order to take the measurements of the gyroscope,
Figure BDA00026342345400000511
is the true value of the angular velocity;bfor a first-order Markov process, i.e. the presence of differential equations
Figure BDA00026342345400000512
In the formula, wgyroIs white gaussian noise;
discretizing the formula (11) and the formula (12) to obtain
Figure BDA0002634234540000061
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000062
is a state recurrence function, T is a time interval, qk-1Is the posture four element at the moment of k-1,
Figure BDA0002634234540000063
is the gyroscope zero offset at time k-1,
Figure BDA0002634234540000064
the gyroscope sample value at time k-1,
Figure BDA0002634234540000065
is a noise coefficient matrix, 04×3Is a 4 × 3 zero matrix, I3Is a 3-dimensional unit array, Wk-1＝wgyro,k-1Is process noise;
selecting accelerometer output values
Figure BDA0002634234540000066
And recovered magnetometer output values
Figure BDA0002634234540000067
Constructing a measurement equation of
Figure BDA0002634234540000068
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000069
for the purpose of the measurement function,
Figure BDA00026342345400000610
is in a state qkThe formed attitude matrix is used for forming the attitude matrix,
Figure BDA00026342345400000611
and
Figure BDA00026342345400000612
the gravity and magnetic field values under the navigation system are respectively,
Figure BDA00026342345400000613
in order to measure the noise, the noise is measured,
Figure BDA00026342345400000614
and
Figure BDA00026342345400000615
respectively the measurement noise of the accelerometer and the magnetometer under the carrier system,
Figure BDA00026342345400000616
is an additional acceleration in addition to gravity;
linearizing the nonlinear equation, and applying two-step adjustment adaptive Kalman filtering to obtain an adaptive extended Kalman filtering algorithm of
Figure BDA00026342345400000617
Figure BDA00026342345400000618
Figure BDA00026342345400000619
Figure BDA00026342345400000620
Pk＝(I-KkHk)Pk/k-1 (19)
In the formula, Qk-1Is a process noise covariance matrix, phik/k-1And HkAre respectively as
Figure BDA00026342345400000621
And h (X)k) In that
Figure BDA00026342345400000622
And
Figure BDA00026342345400000623
at a first order Taylor expansion, i.e.
Figure BDA0002634234540000071
Figure BDA0002634234540000072
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000073
the gyroscope sample value at time k-1,
Figure BDA0002634234540000074
is a filtered estimate of the gyroscope zero bias at time k-1,
Figure BDA0002634234540000075
the filtered predictor of the quaternion vector portion at time k-1,
Figure BDA0002634234540000076
the filter prediction value of the real part of the quaternion at the moment of k-1,
Figure BDA0002634234540000077
the filtered value of the quaternion vector portion at time k,
Figure BDA0002634234540000078
filtered values of the real part of the quaternion at time k, I7Is a 7-dimensional identity matrix, I3Is a 3-dimensional identity matrix, 03×1Is a zero vector of 3 × 1 dimensions, 03×3Zero matrix, symbol of 3 x 3 dimensions
Figure BDA0002634234540000079
Represents the vector r ═ r1 r2 r3]TForming an anti-symmetric matrix.
An improved adaptive kalman filter-based attitude and heading reference system, comprising:
the method is characterized in that a disturbing magnetic field recovery algorithm, a two-step adjustment adaptive Kalman filtering algorithm and an adaptive Kalman filtering attitude estimation algorithm are programmed to be realized, calculation of attitude and heading is realized by utilizing a calculation processor, and an attitude and heading reference system is formed, wherein the calculation processor is a common computer or an embedded processor, or is transplanted into a front embedded system to form the embedded attitude and heading reference system.
The invention has the beneficial effects that:
according to the method, an international geomagnetic reference field is introduced, geomagnetic field measurement interfered by an external magnetic field can be roughly recovered under the condition that the variation of external magnetic field interference value is slow, a geomagnetic field observation value with noise statistical characteristic variation is formed, innovation-based adaptive estimation and residual-based adaptive estimation are combined through a two-step adjustment strategy, more robust adaptive Kalman filtering is formed in engineering practice to solve the problem of measuring the noise statistical characteristic variation, and Kalman filtering is constructed by taking an attitude quaternion and gyroscope zero offset as state quantities, so that the technical problem of real-time calculation of carrier attitude and heading in the presence of environmental magnetic interference can be solved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a flow chart of the process of the present invention.
Fig. 2 is a graph of magnetometer measurement values containing magnetic interference and recovered geomagnetic field measurement values acquired in an actual flight test according to embodiment 3 of the present invention.
Fig. 3 is an attitude and heading information diagram calculated by an actual flight test provided in embodiment 3 of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the 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, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
Referring to fig. 1, the present invention provides an improved adaptive kalman filter-based attitude and heading reference system algorithm, which includes the following steps:
s101: recovering the measured value of the interference magnetic field according to the reference value provided by the international geomagnetic reference field, and taking the residual magnetic field error as the measurement noise of the noise statistical characteristic change;
if the attitude at the k-1 moment can be accurately obtained, the magnetic field interference value at the k-1 moment
Figure BDA0002634234540000081
Comprises the following steps:
Figure BDA0002634234540000082
in the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000083
for magnetometer measurements containing magnetic interference,
Figure BDA0002634234540000084
as a magnetometer measurement at time k-1, m, containing magnetic interferencenA magnetic field reference value obtained for an international geomagnetic reference field,
Figure BDA0002634234540000091
a magnetic field reference value obtained for the international geomagnetic reference field at the time k-1,
Figure BDA0002634234540000092
in the form of a matrix of poses,
Figure BDA0002634234540000093
the attitude matrix at the moment of k-1, and n represents a navigation coordinate system;
the measured value at the k moment is corrected by the magnetic field interference value at the k-1 moment and the measured value of the geomagnetic field at the k moment is recovered, namely the measured value at the k moment is recovered by setting the magnetic field interference change between the k-1 moment and the k moment to be small
Figure BDA0002634234540000094
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000095
removing the geomagnetic estimation value of the external magnetic field interference for the k moment; measurement noise with residual magnetic field error as change in noise statistical properties
Figure BDA0002634234540000096
And further processed by an adaptive kalman filter.
S102: forming a two-step adjustment adaptive Kalman filtering algorithm according to the innovation covariance and the residual covariance to process the observed quantity of the noise statistical characteristic change;
calculating innovation d of Kalman filtering at moment kkEstimate of innovation covariance
Figure BDA0002634234540000097
And obtaining an innovation-based measurement noise estimation covariance matrix by using variance matching variances
Figure BDA0002634234540000098
Namely, it is
Figure BDA0002634234540000099
Figure BDA00026342345400000910
Figure BDA00026342345400000911
In the formula, ZkIs an observed quantity at time k, HkIs a measurement matrix for the time k,
Figure BDA00026342345400000912
for the predicted state at time k obtained by one-step recursion, N is the length of the innovation sequence window used for estimating the innovation covariance, the superscript T is transposition, P isk/k-1A prediction state error covariance matrix in Kalman filtering;
by
Figure BDA00026342345400000913
Obtaining the first adjusted k-time adaptive noise covariance matrix for Kalman update, i.e.
Figure BDA00026342345400000914
In the formula, Radapt,k-1Obtaining an adaptive noise covariance matrix based on the estimated residual covariance matrix at the time k-1;
calculating residual v of k momentkEstimate of residual covariance
Figure BDA0002634234540000101
And obtaining a residual-based measurement noise estimation covariance matrix by using variance matching variance
Figure BDA0002634234540000102
Namely, it is
Figure BDA0002634234540000103
Figure BDA0002634234540000104
Figure BDA0002634234540000105
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000106
for the optimum filter state at time k, Pk/kIs a state error covariance matrix in Kalman filtering, vk-iResidual error at the k-i moment is obtained;
by using
Figure BDA0002634234540000107
Adaptive noise covariance matrix for first adjustment
Figure BDA0002634234540000108
Making a second correction, i.e.
Figure BDA0002634234540000109
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400001010
for a theoretical residual covariance matrix, the symbol trace (·) represents the trace of the sampling matrix; the result of the quadratic correction will be used in the filtering solution for the next cycle.
S103: and establishing a dynamic equation and a measurement equation of the Kalman filter according to the attitude quaternion updating model and the sensor error model, and realizing the estimation of the attitude quaternion by adopting a two-step adjustment self-adaptive Kalman filtering algorithm.
Selecting an attitude quaternion q ═ q0 q1 q2 q3]TZero bias with gyroscopebIs a state quantity in which the differential equation of a quaternion is
Figure BDA00026342345400001011
In the formula, symbol
Figure BDA00026342345400001012
Which represents the multiplication of a quaternion,
Figure BDA00026342345400001013
is composed of
Figure BDA00026342345400001014
The formed zero-scalar quaternion is used for a low-precision micro-mechanical inertia device which cannot sense the rotational angular velocity of the earth
Figure BDA00026342345400001015
Figure BDA00026342345400001016
In order to take the measurements of the gyroscope,
Figure BDA00026342345400001017
is the true value of the angular velocity;bfor a first-order Markov process, i.e. the presence of differential equations
Figure BDA00026342345400001018
In the formula, wgyroIs white gaussian noise;
discretizing the formula (11) and the formula (12) to obtain
Figure BDA0002634234540000111
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000112
is a state recurrence function, T is a time interval, qk-1Is the posture four element at the moment of k-1,
Figure BDA0002634234540000113
is the gyroscope zero offset at time k-1,
Figure BDA0002634234540000114
the gyroscope sample value at time k-1,
Figure BDA0002634234540000115
is a noise coefficient matrix, 04×3Is a 4 × 3 zero matrix, I3Is a 3-dimensional unit array, Wk-1＝wgyro,k-1Is process noise;
selecting accelerometer output values
Figure BDA0002634234540000116
And recovered magnetometer output values
Figure BDA0002634234540000117
Constructing a measurement equation of
Figure BDA0002634234540000118
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000119
for the purpose of the measurement function,
Figure BDA00026342345400001110
is in a state qkThe formed attitude matrix is used for forming the attitude matrix,
Figure BDA00026342345400001111
and
Figure BDA00026342345400001112
the gravity and magnetic field values under the navigation system are respectively,
Figure BDA00026342345400001113
in order to measure the noise, the noise is measured,
Figure BDA00026342345400001114
and
Figure BDA00026342345400001115
respectively the measurement noise of the accelerometer and the magnetometer under the carrier system,
Figure BDA00026342345400001116
is an additional acceleration in addition to gravity;
linearizing the nonlinear equation, and applying two-step adjustment adaptive Kalman filtering to obtain an adaptive extended Kalman filtering algorithm of
Figure BDA00026342345400001117
Figure BDA00026342345400001118
Figure BDA00026342345400001119
Figure BDA00026342345400001120
Pk＝(I-KkHk)Pk/k-1 (19)
In the formula, Qk-1Is a process noise covariance matrix, phik/k-1And HkAre respectively as
Figure BDA00026342345400001121
And h (X)k) In that
Figure BDA00026342345400001122
And
Figure BDA00026342345400001123
at a first order Taylor expansion, i.e.
Figure BDA0002634234540000121
Figure BDA0002634234540000122
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000123
the gyroscope sample value at time k-1,
Figure BDA0002634234540000124
is a filtered estimate of the gyroscope zero bias at time k-1,
Figure BDA0002634234540000125
the filtered predictor of the quaternion vector portion at time k-1,
Figure BDA0002634234540000126
the filter prediction value of the real part of the quaternion at the moment of k-1,
Figure BDA0002634234540000127
the filtered value of the quaternion vector portion at time k,
Figure BDA0002634234540000128
filtered values of the real part of the quaternion at time k, I7Is a 7-dimensional identity matrix, I3Is a 3-dimensional identity matrix, 03×1Is a zero vector of 3 × 1 dimensions, 03×3Zero matrix, symbol of 3 x 3 dimensions
Figure BDA0002634234540000129
Represents the vector r ═ r1 r2 r3]TForming an anti-symmetric matrix.
In order to further optimize the above technical solution, the present invention provides an interference geomagnetic field recovery algorithm based on the attitude and heading reference system algorithm based on the improved adaptive kalman filter in embodiment 1, including:
the method comprises the steps of obtaining a geomagnetic field estimation value under a carrier system at the previous moment by utilizing a geomagnetic field reference value under a navigation system provided by an international geomagnetic reference field and an attitude matrix at the previous moment, comparing the geomagnetic field estimation value with a geomagnetic field measurement value to obtain a magnetic field interference value at the previous moment, and correcting the geomagnetic field measurement value at the current moment by utilizing the magnetic field interference value at the previous moment under the premise that the magnetic field interference values at the previous and next measurement moments are not greatly changed to form an interference magnetic field recovery algorithm.
In order to further optimize the above technical solution, the present invention provides a two-step adaptive kalman filter algorithm based on the attitude and heading reference system algorithm based on the improved adaptive kalman filter described in embodiment 1, including:
the method comprises the steps of utilizing an innovation-based and residual-based noise parameter online estimation method, combining an innovation-based adaptive estimation method and a residual-based adaptive estimation method through a two-step adjustment strategy, carrying out measurement updating on Kalman filtering by using an estimated noise covariance matrix provided by the innovation-based adaptive estimation in the first step, and modifying the noise covariance matrix obtained in the first step by using the estimated noise covariance matrix provided by the residual-based adaptive estimation in the second step for updating and resolving in the next filtering period to form a two-step adjustment adaptive Kalman filtering algorithm.
In order to further optimize the above technical solution, on the basis of the improved adaptive kalman filter-based attitude and heading reference system algorithm described in embodiment 1, an adaptive extended kalman filter-based attitude and heading estimation algorithm is provided, which includes:
the attitude quaternion and the gyroscope zero offset are estimated by using a Kalman filtering technology, and an attitude heading reference system algorithm of the adaptive Kalman filtering is formed.
According to the method, an international geomagnetic reference field is introduced, geomagnetic field measurement interfered by an external magnetic field can be roughly recovered under the condition that the variation of external magnetic field interference value is slow, a geomagnetic field observation value with noise statistical characteristic variation is formed, innovation-based adaptive estimation and residual-based adaptive estimation are combined through a two-step adjustment strategy, more robust adaptive Kalman filtering is formed in engineering practice to solve the problem of measuring the noise statistical characteristic variation, and Kalman filtering is constructed by taking an attitude quaternion and gyroscope zero offset as state quantities, so that the technical problem of real-time calculation of carrier attitude and heading in the presence of environmental magnetic interference can be solved.
Example 2
In this embodiment, on the basis of the improved adaptive kalman filter-based attitude and heading reference system algorithm described in embodiment 1, an improved adaptive kalman filter-based attitude and heading reference system is provided, where the system in this embodiment includes:
the method is characterized in that an interference magnetic field recovery algorithm, a two-step adjustment adaptive Kalman filtering algorithm and an adaptive Kalman filtering attitude estimation algorithm are programmed to be realized, calculation of attitude and heading is realized by utilizing a calculation processor, an attitude and heading reference system is formed, the technical problem of real-time calculation of carrier attitude and heading when environmental magnetic interference exists is solved, and the calculation processor can be a common computer, can also be an embedded processor, and can also be transplanted into a front embedded system to form the embedded attitude and heading reference system.
Example 3
To further illustrate the implementation of the embodiments of the present invention, the following detailed description of the invention is provided by way of an actual scenario.
In this embodiment, taking four-rotor flight data as an example, the specific implementation steps of the present invention are as follows:
(1) according to a reference value provided by an international geomagnetic reference field, recovering an interference magnetic field measurement value, and regarding a residual magnetic field error as measurement noise with noise statistical characteristic change:
if the attitude at the moment k-1 can be accurately obtained, the magnetic field interference value at the previous moment
Figure BDA0002634234540000141
Is composed of
Figure BDA0002634234540000142
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000143
as measured by a magnetometer containing magnetic interference, mnFor the values of the earth magnetic field obtained by IGRF,
Figure BDA0002634234540000144
b represents a carrier coordinate system, and n represents a navigation coordinate system.
The measured value at the k moment is corrected by the magnetic field interference value at the k-1 moment and the measured value of the geomagnetic field at the k moment is recovered, namely the measured value at the k moment is recovered by setting the magnetic field interference change between the k-1 moment and the k moment to be small
Figure BDA0002634234540000145
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000146
and removing the geomagnetic estimation value of the external magnetic field interference for the k moment. Measurement noise with residual geomagnetic error as noise statistical characteristic change
Figure BDA0002634234540000147
And further processed by adaptive extended kalman filtering. The geomagnetic triaxial measurement values recovered in this embodiment are shown in fig. 2.
(2) According to the innovation covariance and the residual covariance, a two-step adjustment self-adaptive Kalman filtering algorithm is provided to process the observed quantity of noise statistical characteristic change:
calculating innovation d of Kalman filtering at moment kkEstimate of innovation covariance
Figure BDA0002634234540000148
And obtaining an innovation-based measurement noise estimation covariance matrix by using variance matching variances
Figure BDA0002634234540000151
Namely, it is
Figure BDA0002634234540000152
Figure BDA0002634234540000153
Figure BDA0002634234540000154
In the formula, ZkIs an observed quantity at time k, HkIs a measurement matrix for the time k,
Figure BDA0002634234540000155
for the predicted state at time k obtained by one-step recursion, N is the length of the innovation sequence window used to estimate the innovation covariance, where N is 3, superscript T is transposition, and P isk/k-1Is a prediction state error covariance matrix in kalman filtering.
By
Figure BDA0002634234540000156
Obtaining the first adjusted k-time adaptive noise covariance matrix for Kalman update, i.e.
Figure BDA0002634234540000157
In the formula, Radapt,k-1An adaptive noise covariance matrix obtained based on the estimated residual covariance matrix for time k-1.
Calculating residual v of k momentkEstimate of residual covariance
Figure BDA0002634234540000158
And obtaining a residual-based measurement noise estimation covariance matrix by using variance matching variance
Figure BDA0002634234540000159
Namely, it is
Figure BDA00026342345400001510
Figure BDA00026342345400001511
Figure BDA00026342345400001512
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400001513
for the optimum filter state at time k, Pk/kIs a state error covariance matrix in kalman filtering.
By using
Figure BDA00026342345400001514
Adaptive noise covariance matrix for first adjustment
Figure BDA00026342345400001515
Making a second correction, i.e.
Figure BDA00026342345400001516
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400001517
for the theoretical residual covariance matrix, the notation trace (·) denotes the trace of the extraction matrix. The result of the quadratic correction will be used in the filtering solution for the next cycle.
(3) Establishing a dynamic equation and a measurement equation of an extended Kalman filter according to the attitude quaternion updating model and the sensor error model, and realizing estimation of the attitude quaternion by adopting a two-step adjustment self-adaptive extended Kalman filtering algorithm:
selecting an attitude quaternion q ═ q0 q1 q2 q3]TZero bias with gyroscopebIs a state quantity in which the differential equation of a quaternion is
Figure BDA0002634234540000161
In the formula, symbol
Figure BDA0002634234540000162
Which represents the multiplication of a quaternion,
Figure BDA0002634234540000163
is composed of
Figure BDA0002634234540000164
The formed zero-scalar quaternion is used for a low-precision micro-mechanical inertia device which cannot sense the rotational angular velocity of the earth
Figure BDA0002634234540000165
Figure BDA0002634234540000166
In order to take the measurements of the gyroscope,
Figure BDA0002634234540000167
is the true value of angular velocity.bCan be generally built as a first order Markov process, i.e. there are differential equations
Figure BDA0002634234540000168
In the formula, wgyroIs gaussian white noise.
Discretizing the formula (11) and the formula (12) to obtain
Figure BDA0002634234540000169
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400001610
for the state recursion function, T is the time interval, which is 0.02s, q in this embodimentk-1Is the posture four element at the moment of k-1,
Figure BDA00026342345400001611
is the gyroscope zero offset at time k-1,
Figure BDA00026342345400001612
the gyroscope sample value at time k-1,
Figure BDA00026342345400001613
is a noise coefficient matrix, 04×3Is a 4 × 3 zero matrix, I3Is a 3-dimensional unit array, Wk-1＝wgyro,k-1Is process noise.
Selecting accelerometer output values
Figure BDA00026342345400001614
And recovered magnetometer output values
Figure BDA00026342345400001615
Constructing a measurement equation of
Figure BDA00026342345400001616
In the formula (I), the compound is shown in the specification,
Figure BDA0002634234540000171
for the purpose of the measurement function,
Figure BDA0002634234540000172
is in a state qkThe formed attitude matrix is used for forming the attitude matrix,
Figure BDA0002634234540000173
and
Figure BDA0002634234540000174
the gravity and magnetic field values under the navigation system are taken as
Figure BDA0002634234540000175
Figure BDA0002634234540000176
Is provided in real time by the international geomagnetic reference field,
Figure BDA0002634234540000177
in order to measure the noise, the noise is measured,
Figure BDA0002634234540000178
and
Figure BDA0002634234540000179
respectively the measurement noise of the accelerometer and the magnetometer under the carrier system,
Figure BDA00026342345400001710
an additional acceleration in addition to gravity.
Linearizing the nonlinear equation, and applying two-step adjustment adaptive Kalman filtering to obtain an adaptive extended Kalman filtering algorithm of
Figure BDA00026342345400001711
Figure BDA00026342345400001712
Figure BDA00026342345400001713
Figure BDA00026342345400001714
Pk＝(I-KkHk)Pk/k-1 (19)
In the formula, Qk-1Taken as Q in this example as the process noise covariance matrixk-1＝(1°/h)2I3And normalizing the measured value, wherein the initial value of the covariance matrix of the measured noise is R equal to 0.12I6Followed by adaptive adjustment of phik/k-1And HkAre respectively as
Figure BDA00026342345400001715
And h (X)k) In that
Figure BDA00026342345400001716
And
Figure BDA00026342345400001717
at a first order Taylor expansion, i.e.
Figure BDA00026342345400001718
Figure BDA00026342345400001719
In the formula (I), the compound is shown in the specification,
Figure BDA00026342345400001720
the gyroscope sample value at time k-1,
Figure BDA00026342345400001721
is a filtered estimate of the gyroscope zero bias at time k-1,
Figure BDA00026342345400001722
the filtered predictor of the quaternion vector portion at time k-1,
Figure BDA00026342345400001723
the filter prediction value of the real part of the quaternion at the moment of k-1,
Figure BDA00026342345400001724
the filtered value of the quaternion vector portion at time k,
Figure BDA00026342345400001725
filtered values of the real part of the quaternion at time k, I7Is a 7-dimensional identity matrix, I3Is a 3-dimensional identity matrix, 03×1Is a zero vector of 3 × 1 dimensions, 03×3Zero matrix, symbol of 3 x 3 dimensions
Figure BDA0002634234540000181
Represents the vector r ═ r1 r2 r3]TForming an anti-symmetric matrix. The attitude and heading calculation results obtained in this embodiment are shown in fig. 3.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims (8)

1. An improved adaptive Kalman filtering-based attitude and heading reference system algorithm, comprising:
recovering the measured value of the interference magnetic field according to the reference value provided by the international geomagnetic reference field, and taking the residual magnetic field error as the measurement noise of the noise statistical characteristic change;
forming a two-step adjustment adaptive Kalman filtering algorithm according to the innovation covariance and the residual covariance to process the observed quantity of the noise statistical characteristic change;
and establishing a dynamic equation and a measurement equation of the Kalman filter according to the attitude quaternion updating model and the sensor error model, and realizing the estimation of the attitude quaternion by adopting a two-step adjustment self-adaptive Kalman filtering algorithm.
2. The algorithm of claim 1, wherein the estimated geomagnetic field value under the carrier system at the previous time is obtained by using a magnetic field reference value under a navigation system provided by an international geomagnetic reference field and an attitude matrix at the previous time, the estimated geomagnetic field value is compared with the magnetic field measurement value to obtain a magnetic field interference value at the previous time, and the magnetic field measurement value at the current time is corrected by using the magnetic field interference value at the previous time under the premise that the magnetic field interference values at the previous and subsequent measurement times do not change much to form an interference magnetic field recovery algorithm to recover the interference magnetic field measurement value.
3. The algorithm of claim 2, wherein the residual magnetic field error is considered as the measurement noise of the statistical characteristic variation of the noise, and the carrier system b is the magnetic field interference value at the time k-1
Figure FDA0002634234530000011
From the attitude matrix calculated at the time k-1 and the geomagnetic field reference value, i.e.
Figure FDA0002634234530000012
In the formula (I), the compound is shown in the specification,
Figure FDA0002634234530000013
for magnetometer measurements containing magnetic interference,
Figure FDA0002634234530000014
as a magnetometer measurement at time k-1, m, containing magnetic interferencenA magnetic field reference value obtained for an international geomagnetic reference field,
Figure FDA0002634234530000015
a magnetic field reference value obtained for the international geomagnetic reference field at the time k-1,
Figure FDA0002634234530000016
in the form of a matrix of poses,
Figure FDA0002634234530000017
the attitude matrix at the moment of k-1, and n represents a navigation coordinate system;
the measured value at the k moment is corrected by the magnetic field interference value at the k-1 moment and the measured value of the geomagnetic field at the k moment is recovered, namely the measured value at the k moment is recovered by setting the magnetic field interference change between the k-1 moment and the k moment to be small
Figure FDA0002634234530000021
In the formula (I), the compound is shown in the specification,
Figure FDA0002634234530000022
removing the geomagnetic estimation value of the external magnetic field interference for the k moment; measurement noise with residual magnetic field error as change in noise statistical properties
Figure FDA0002634234530000023
And further processed by an adaptive kalman filter.
4. The attitude and heading reference system algorithm based on the improved adaptive Kalman filtering according to claim 3, characterized in that an innovation-based and residual-based noise parameter online estimation method is utilized, the innovation-based adaptive estimation method and the residual-based adaptive estimation method are combined through a two-step adjustment strategy, in the first step, the Kalman filtering measurement updating is performed through an estimated noise covariance matrix provided by the innovation-based adaptive estimation, and in the second step, the noise covariance matrix obtained in the first step is corrected through the estimated noise covariance matrix provided by the residual-based adaptive estimation and is used for updating and resolving in the next filtering period to form a two-step adjustment adaptive Kalman filtering algorithm.
5. An improved adaptive Kalman filter based attitude and heading reference system algorithm according to claim 4, characterized in that the two-step adaptive Kalman filter algorithm formed according to innovation covariance and residual covariance to process the observed quantity of noise statistic characteristic change comprises:
calculating innovation d of Kalman filtering at moment kkEstimate of innovation covariance
Figure FDA0002634234530000024
And obtaining an innovation-based measurement noise estimation covariance matrix by using variance matching variances
Figure FDA0002634234530000025
Namely, it is
Figure FDA0002634234530000026
Figure FDA0002634234530000027
Figure FDA0002634234530000028
In the formula, ZkIs an observed quantity at time k, HkIs the amount of time kThe measurement matrix is used for measuring the matrix,
Figure FDA0002634234530000029
for the predicted state at time k obtained by one-step recursion, N is the length of the innovation sequence window used for estimating the innovation covariance, the superscript T is transposition, P isk/k-1A prediction state error covariance matrix in Kalman filtering;
by
Figure FDA0002634234530000031
Obtaining the first adjusted k-time adaptive noise covariance matrix for Kalman update, i.e.
Figure FDA0002634234530000032
In the formula, Radapt,k-1Obtaining an adaptive noise covariance matrix based on the estimated residual covariance matrix at the time k-1;
calculating residual v of k momentkEstimate of residual covariance
Figure FDA0002634234530000033
And obtaining a residual-based measurement noise estimation covariance matrix by using variance matching variance
Figure FDA0002634234530000034
Namely, it is
Figure FDA0002634234530000035
Figure FDA0002634234530000036
Figure FDA0002634234530000037
In the formula (I), the compound is shown in the specification,
Figure FDA0002634234530000038
for the optimum filter state at time k, Pk/kIs a state error covariance matrix in Kalman filtering, vk-iResidual error at the k-i moment is obtained;
by using
Figure FDA0002634234530000039
Adaptive noise covariance matrix for first adjustment
Figure FDA00026342345300000310
Making a second correction, i.e.
Figure FDA00026342345300000311
In the formula (I), the compound is shown in the specification,
Figure FDA00026342345300000312
for a theoretical residual covariance matrix, the symbol trace (·) represents the trace of the sampling matrix; the result of the quadratic correction will be used in the filtering solution for the next cycle.
6. The algorithm of claim 4, wherein the attitude and heading reference system algorithm based on the improved adaptive Kalman filtering is characterized in that a nonlinear state equation is constructed by taking attitude quaternion and gyroscope zero offset as states, a nonlinear observation equation is constructed by taking accelerometer measurement values and recovered magnetometer measurement values, measurement noise parameters are adaptively adjusted by taking residual geomagnetic errors as measurement noise through two-step adaptive Kalman filtering, and the attitude and heading reference system algorithm based on the adaptive Kalman filtering is formed by estimating the attitude quaternion and the gyroscope zero offset through a Kalman filtering technology.
7. The attitude and heading reference system algorithm based on the improved adaptive kalman filter according to claim 6, wherein the dynamic equation and the measurement equation of the extended kalman filter are established according to the attitude quaternion updating model and the sensor error model, and the estimation of the attitude quaternion is realized by adopting a two-step adjustment adaptive extended kalman filter algorithm, which comprises:
selecting an attitude quaternion q ═ q0 q1 q2 q3]TZero bias with gyroscopebIs a state quantity in which the differential equation of a quaternion is
Figure FDA0002634234530000041
In the formula, symbol
Figure FDA0002634234530000042
Which represents the multiplication of a quaternion,
Figure FDA0002634234530000043
is composed of
Figure FDA0002634234530000044
The formed zero-scalar quaternion is used for a low-precision micro-mechanical inertia device which cannot sense the rotational angular velocity of the earth
Figure FDA0002634234530000045
Figure FDA0002634234530000046
In order to take the measurements of the gyroscope,
Figure FDA0002634234530000047
is the true value of the angular velocity;bfor a first-order Markov process, i.e. the presence of differential equations
Figure FDA0002634234530000048
In the formula, wgyroIs white gaussian noise;
discretizing the formula (11) and the formula (12) to obtain
Figure FDA0002634234530000049
In the formula (I), the compound is shown in the specification,
Figure FDA00026342345300000410
is a state recurrence function, T is a time interval, qk-1Is the posture four element at the moment of k-1,
Figure FDA00026342345300000411
is the gyroscope zero offset at time k-1,
Figure FDA00026342345300000412
the gyroscope sample value at time k-1,
Figure FDA00026342345300000413
is a noise coefficient matrix, 04×3Is a 4 × 3 zero matrix, I3Is a 3-dimensional unit array, Wk-1＝wgyro,k-1Is process noise;
selecting accelerometer output values
Figure FDA00026342345300000414
And recovered magnetometer output values
Figure FDA00026342345300000415
Constructing a measurement equation of
Figure FDA00026342345300000416
In the formula (I), the compound is shown in the specification,
Figure FDA0002634234530000051
for the purpose of the measurement function,
Figure FDA0002634234530000052
is in a state qkThe formed attitude matrix is used for forming the attitude matrix,
Figure FDA0002634234530000053
and
Figure FDA0002634234530000054
the gravity and magnetic field values under the navigation system are respectively,
Figure FDA0002634234530000055
in order to measure the noise, the noise is measured,
Figure FDA0002634234530000056
and
Figure FDA0002634234530000057
respectively the measurement noise of the accelerometer and the magnetometer under the carrier system,
Figure FDA0002634234530000058
is an additional acceleration in addition to gravity;
linearizing the nonlinear equation, and applying two-step adjustment adaptive Kalman filtering to obtain an adaptive extended Kalman filtering algorithm of
Figure FDA0002634234530000059
Figure FDA00026342345300000510
Figure FDA00026342345300000511
Figure FDA00026342345300000512
Pk＝(I-KkHk)Pk/k-1 (19)
In the formula, Qk-1Is a process noise covariance matrix, phik/k-1And HkAre respectively as
Figure FDA00026342345300000513
And h (X)k) In that
Figure FDA00026342345300000514
And
Figure FDA00026342345300000515
at a first order Taylor expansion, i.e.
Figure FDA00026342345300000516
Figure FDA00026342345300000517
In the formula (I), the compound is shown in the specification,
Figure FDA00026342345300000518
the gyroscope sample value at time k-1,
Figure FDA00026342345300000519
is a filtered estimate of the gyroscope zero bias at time k-1,
Figure FDA00026342345300000520
the filtered predictor of the quaternion vector portion at time k-1,
Figure FDA00026342345300000521
the filter prediction value of the real part of the quaternion at the moment of k-1,
Figure FDA00026342345300000522
the filtered value of the quaternion vector portion at time k,
Figure FDA00026342345300000523
filtered values of the real part of the quaternion at time k, I7Is a 7-dimensional identity matrix, I3Is a 3-dimensional identity matrix, 03×1Is a zero vector of 3 × 1 dimensions, 03×3Zero matrix, symbol of 3 x 3 dimensions
Figure FDA00026342345300000524
Represents the vector r ═ r1 r2 r3]TForming an anti-symmetric matrix.
8. An attitude heading reference system based on improved adaptive Kalman filtering, comprising:
the method is characterized in that a disturbing magnetic field recovery algorithm, a two-step adjustment adaptive Kalman filtering algorithm and an adaptive Kalman filtering attitude estimation algorithm are programmed to be realized, calculation of attitude and heading is realized by utilizing a calculation processor, and an attitude and heading reference system is formed, wherein the calculation processor is a common computer or an embedded processor, or is transplanted into a front embedded system to form the embedded attitude and heading reference system.
CN202010820409.3A 2020-08-14 2020-08-14 Attitude heading reference system algorithm based on improved adaptive Kalman filtering Active CN112013836B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010820409.3A CN112013836B (en) 2020-08-14 2020-08-14 Attitude heading reference system algorithm based on improved adaptive Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010820409.3A CN112013836B (en) 2020-08-14 2020-08-14 Attitude heading reference system algorithm based on improved adaptive Kalman filtering

Publications (2)

Publication Number Publication Date
CN112013836A true CN112013836A (en) 2020-12-01
CN112013836B CN112013836B (en) 2022-02-08

Family

ID=73504699

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010820409.3A Active CN112013836B (en) 2020-08-14 2020-08-14 Attitude heading reference system algorithm based on improved adaptive Kalman filtering

Country Status (1)

Country Link
CN (1) CN112013836B (en)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112660144A (en) * 2020-12-04 2021-04-16 上汽通用五菱汽车股份有限公司 Yaw rate filtering method, control terminal, vehicle and storage medium
CN112729348A (en) * 2021-01-10 2021-04-30 河南理工大学 Attitude self-adaptive correction method for IMU system
CN112946641A (en) * 2021-01-27 2021-06-11 北京航空航天大学 Data filtering method based on relevance of Kalman filtering innovation and residual error
CN113109848A (en) * 2021-04-14 2021-07-13 长沙学院 Navigation positioning method under low-speed rotation condition of carrier and application system thereof
CN113237478A (en) * 2021-05-27 2021-08-10 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle
CN113359809A (en) * 2021-07-23 2021-09-07 西北工业大学 Bridge detection unmanned aerial vehicle autonomous positioning method based on RBFNN assistance
CN113607167A (en) * 2021-03-15 2021-11-05 南京航空航天大学 Self-adaptive attitude estimation method for attitude heading system and smooth switching algorithm thereof
CN113670314A (en) * 2021-08-20 2021-11-19 西南科技大学 Unmanned aerial vehicle attitude estimation method based on PI self-adaptive two-stage Kalman filtering
CN114018250A (en) * 2021-10-18 2022-02-08 杭州鸿泉物联网技术股份有限公司 Inertial navigation method, electronic device, storage medium, and computer program product
CN114323008A (en) * 2021-12-31 2022-04-12 杭州电子科技大学 Fusion course angle estimation method and system based on machine learning classification
CN114459466A (en) * 2021-12-29 2022-05-10 宜昌测试技术研究所 MEMS multi-sensor data fusion processing method based on fuzzy control
CN114578439A (en) * 2021-12-27 2022-06-03 北京自动化控制设备研究所 Magnetic compensation coefficient self-adaptive optimization method and system
CN114771886A (en) * 2022-02-24 2022-07-22 哈尔滨工业大学 Space rotating mechanism gravity unloading device and method based on Kalman filtering
CN116147624A (en) * 2022-12-21 2023-05-23 广东智能无人系统研究院(南沙) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN116659487A (en) * 2022-11-02 2023-08-29 丰疆智能(深圳)有限公司 Pose adjustment method, pose adjustment device, electronic equipment and readable storage medium
CN116817896A (en) * 2023-04-03 2023-09-29 盐城数智科技有限公司 Gesture resolving method based on extended Kalman filtering

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102654404A (en) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 Method for improving resolving precision and anti-jamming capability of attitude heading reference system
CN105606096A (en) * 2016-01-28 2016-05-25 北京航空航天大学 Attitude and heading calculation method and system assisted by carrier movement state information
CN105716610A (en) * 2016-01-28 2016-06-29 北京航空航天大学 Carrier attitude and heading calculation method assisted by geomagnetic field model and system
CN106500693A (en) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 A kind of AHRS algorithms based on adaptive extended kalman filtering
CN107860382A (en) * 2017-11-07 2018-03-30 吉林大学 A kind of method for measuring posture using AHRS in the case of magnetic anomaly
CN110779532A (en) * 2019-11-18 2020-02-11 河南工业大学 Geomagnetic navigation system and method applied to near-earth orbit satellite
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102654404A (en) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 Method for improving resolving precision and anti-jamming capability of attitude heading reference system
CN105606096A (en) * 2016-01-28 2016-05-25 北京航空航天大学 Attitude and heading calculation method and system assisted by carrier movement state information
CN105716610A (en) * 2016-01-28 2016-06-29 北京航空航天大学 Carrier attitude and heading calculation method assisted by geomagnetic field model and system
CN106500693A (en) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 A kind of AHRS algorithms based on adaptive extended kalman filtering
CN107860382A (en) * 2017-11-07 2018-03-30 吉林大学 A kind of method for measuring posture using AHRS in the case of magnetic anomaly
CN110779532A (en) * 2019-11-18 2020-02-11 河南工业大学 Geomagnetic navigation system and method applied to near-earth orbit satellite
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
N.GAO等: "A novel robust Kalman filter on AHRS in the magnetic distortion environment", 《ADVANCES IN SPACE RESEARCH》 *
周卫东 等: "基于新息和残差的自适应 UKF 算法", 《宇航学报》 *
王录等: "基于地磁场自适应修正的航姿系统姿态解算研究", 《西北工业大学学报》 *
田易等: "航姿参考系统中一种自适应卡尔曼滤波算法", 《西安电子科技大学学报》 *

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112660144A (en) * 2020-12-04 2021-04-16 上汽通用五菱汽车股份有限公司 Yaw rate filtering method, control terminal, vehicle and storage medium
CN112660144B (en) * 2020-12-04 2022-06-24 上汽通用五菱汽车股份有限公司 Yaw rate filtering method, control terminal, vehicle and storage medium
CN112729348A (en) * 2021-01-10 2021-04-30 河南理工大学 Attitude self-adaptive correction method for IMU system
CN112729348B (en) * 2021-01-10 2023-11-28 河南理工大学 Gesture self-adaptive correction method for IMU system
CN112946641A (en) * 2021-01-27 2021-06-11 北京航空航天大学 Data filtering method based on relevance of Kalman filtering innovation and residual error
CN113607167A (en) * 2021-03-15 2021-11-05 南京航空航天大学 Self-adaptive attitude estimation method for attitude heading system and smooth switching algorithm thereof
CN113109848A (en) * 2021-04-14 2021-07-13 长沙学院 Navigation positioning method under low-speed rotation condition of carrier and application system thereof
CN113109848B (en) * 2021-04-14 2023-03-14 长沙学院 Navigation positioning method under low-speed rotation condition of carrier
CN113237478A (en) * 2021-05-27 2021-08-10 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle
CN113237478B (en) * 2021-05-27 2022-10-14 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle
CN113359809A (en) * 2021-07-23 2021-09-07 西北工业大学 Bridge detection unmanned aerial vehicle autonomous positioning method based on RBFNN assistance
CN113359809B (en) * 2021-07-23 2022-11-11 西北工业大学 Bridge detection unmanned aerial vehicle autonomous positioning method based on RBFNN assistance
CN113670314B (en) * 2021-08-20 2023-05-09 西南科技大学 Unmanned aerial vehicle attitude estimation method based on PI self-adaptive two-stage Kalman filtering
CN113670314A (en) * 2021-08-20 2021-11-19 西南科技大学 Unmanned aerial vehicle attitude estimation method based on PI self-adaptive two-stage Kalman filtering
CN114018250A (en) * 2021-10-18 2022-02-08 杭州鸿泉物联网技术股份有限公司 Inertial navigation method, electronic device, storage medium, and computer program product
CN114018250B (en) * 2021-10-18 2024-05-03 杭州鸿泉物联网技术股份有限公司 Inertial navigation method, electronic device, storage medium and computer program product
CN114578439A (en) * 2021-12-27 2022-06-03 北京自动化控制设备研究所 Magnetic compensation coefficient self-adaptive optimization method and system
CN114459466A (en) * 2021-12-29 2022-05-10 宜昌测试技术研究所 MEMS multi-sensor data fusion processing method based on fuzzy control
CN114323008A (en) * 2021-12-31 2022-04-12 杭州电子科技大学 Fusion course angle estimation method and system based on machine learning classification
CN114771886A (en) * 2022-02-24 2022-07-22 哈尔滨工业大学 Space rotating mechanism gravity unloading device and method based on Kalman filtering
CN116659487A (en) * 2022-11-02 2023-08-29 丰疆智能(深圳)有限公司 Pose adjustment method, pose adjustment device, electronic equipment and readable storage medium
CN116147624A (en) * 2022-12-21 2023-05-23 广东智能无人系统研究院(南沙) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN116147624B (en) * 2022-12-21 2023-08-01 广东智能无人系统研究院(南沙) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN116817896A (en) * 2023-04-03 2023-09-29 盐城数智科技有限公司 Gesture resolving method based on extended Kalman filtering
CN116817896B (en) * 2023-04-03 2024-04-16 盐城数智科技有限公司 Gesture resolving method based on extended Kalman filtering

Also Published As

Publication number Publication date
CN112013836B (en) 2022-02-08

Similar Documents

Publication Publication Date Title
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
CN109459019B (en) Vehicle navigation calculation method based on cascade adaptive robust federal filtering
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN111076722B (en) Attitude estimation method and device based on self-adaptive quaternion
CN109945859B (en) Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering
Eckenhoff et al. High-accuracy preintegration for visual-inertial navigation
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN112798021B (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
CN108562290B (en) Navigation data filtering method and device, computer equipment and storage medium
CN116067370B (en) IMU gesture resolving method, IMU gesture resolving equipment and storage medium
CN112683267B (en) Vehicle-mounted attitude estimation method with GNSS velocity vector assistance
CN111649747A (en) IMU-based adaptive EKF attitude measurement improvement method
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN114459466A (en) MEMS multi-sensor data fusion processing method based on fuzzy control
CN116718153B (en) Deformation monitoring method and system based on GNSS and INS
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN110160530B (en) Spacecraft attitude filtering method based on quaternion
CN110849364A (en) Adaptive Kalman attitude estimation method based on communication-in-motion
CN114858166B (en) IMU attitude resolving method based on maximum correlation entropy Kalman filter
CN110672127A (en) Real-time calibration method for array type MEMS magnetic sensor
CN115143954A (en) Unmanned vehicle navigation method based on multi-source information fusion
CN114812546A (en) Shielded space individual soldier navigation pose correction method and device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant