CN110440756B - Attitude estimation method of inertial navigation system - Google Patents
Attitude estimation method of inertial navigation system Download PDFInfo
- Publication number
- CN110440756B CN110440756B CN201910571246.7A CN201910571246A CN110440756B CN 110440756 B CN110440756 B CN 110440756B CN 201910571246 A CN201910571246 A CN 201910571246A CN 110440756 B CN110440756 B CN 110440756B
- Authority
- CN
- China
- Prior art keywords
- measurement
- equation
- attitude
- accelerometer
- quaternion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention relates to an attitude estimation method of an inertial navigation system, which is technically characterized by comprising the following steps: the method comprises the following steps: step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation; step 2, establishing a quaternion pseudo Kalman wave measurement equation by taking the measurement value of the accelerometer as input; and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition. The invention effectively improves the estimation precision of the horizontal attitude under the dynamic condition.
Description
Technical Field
The invention belongs to the technical field of IMU attitude measurement, and particularly relates to an attitude estimation method of an inertial navigation system.
Background
An IMU-based attitude measurement system uses accelerometer and gyroscope outputs to optimally estimate horizontal attitude. When the carrier has no motion acceleration, the accelerometer can accurately sense the local gravity vector and is fused with the output of the gyroscope to obtain a high-precision horizontal posture. However, when the carrier has a motion acceleration, the accelerometer output includes a local gravity acceleration and a motion acceleration, and a large error is caused when the horizontal attitude is calculated by taking the gravity vector as a reference. That is, the acceleration of the carrier movement interferes with the measurement of the accelerometer on the gravity vector, so that the horizontal attitude estimation is inaccurate. Therefore, how to effectively weaken the interference of the motion acceleration of the carrier enables the system to still have higher attitude measurement accuracy under the dynamic condition, and has certain research significance.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides an inertial navigation system attitude estimation method which can effectively weaken the interference of motion acceleration, realize the optimal estimation of attitude information and improve the horizontal attitude estimation precision under the dynamic condition of a system.
The invention solves the practical problem by adopting the following technical scheme:
an inertial navigation system attitude estimation method comprises the following steps:
and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition.
Further, the specific steps of step 1 include:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωyωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filtering state equation, wherein the basic form of the known state equation is as follows:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrixWherein T is a filter period;
Wherein, deltag,kThe noise variance is output for the gyroscope.
Further, the specific steps of step 2 include:
(1) the basic form of the known measurement equation is:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
04×1=HkQk+Vk
measuring Z as 04×1;
(2) Measure noise matrixδa,kOutput noise variance for accelerometer, ap,kIs the variance of the acceleration of the carrier motion.
Further, the specific steps of step 3 include:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and a residual error queue r with the most recent number of N (taking N as 100) is recordedk-N+1,rk-N+2,……,rk;
The queues are sequenced to obtain new queues r' v which are arranged from small to largek-N+1,r′vk-N+2,……,rk'; wherein, r' vk-N+1≤r′vk-N+2≤…≤r′vk
After the maximum number and the minimum number are removed, the residual data are averaged, and then the measurement and update residual value is as follows:
then measure the noise variance matrix RkThe unbiased estimate of (d) may be expressed as:
i.e. the filter updates the residual error based on accelerometer measurementsDynamically adjust the measurement noise variance matrix RkThe adaptive adjustment of the filter parameters under different dynamic conditions is realized.
The invention has the advantages and beneficial effects that:
1. according to the method, the estimation algorithm based on the accelerometer measurement updating residual error is designed, the measurement variance matrix is adjusted in real time, the problem of horizontal attitude estimation of the inertial navigation system under the dynamic condition is well solved under the condition that the motion acceleration is unknown, the horizontal attitude estimation precision under the dynamic condition is effectively improved, and the method has a high engineering application value.
2. The invention can be used for an attitude Measurement system taking an Inertial Measurement Unit (IMU) as a core device. When the carrier has the motion acceleration, the filter can dynamically adjust the value of the measurement variance matrix according to the measurement update residual error of the accelerometer under the condition that the motion acceleration is unknown, so that the interference of the motion acceleration is effectively weakened, and the optimal estimation of the attitude information is realized.
3. Aiming at the problem that the motion acceleration is unpredictable, a pseudo measurement model based on the accelerometer output is designed, threshold judgment on a combined acceleration value output by the accelerometer is not needed, and the measurement noise variance is estimated in real time according to a residual value updated by the accelerometer, namely the measurement noise variance matrix is dynamically adjusted according to the motion acceleration, so that the attitude measurement precision of the system under a dynamic condition is ensured.
Drawings
FIG. 1 is a process flow diagram of the present invention.
Detailed Description
The embodiments of the invention will be described in further detail below with reference to the accompanying drawings:
an inertial navigation system attitude estimation method, as shown in fig. 1, includes the following steps:
the specific steps of the step 1 comprise:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωyωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filtering state equation, wherein the basic form of the known state equation is as follows:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrixWhere T is the filter period, F is the system continuous equation state matrix, WkIs the system noise;
Wherein, I3×3Is a unit matrix, δg,kThe variance of the noise output for the gyroscope depends on the characteristics of the gyroscope itself and can be obtained through measurement.
the specific steps of the step 2 comprise:
(1) the basic form of the measurement equation is known as:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
04×1=HkQk+Vk
Wherein the measuring matrixQkIs an attitude quaternion, VkG is a local gravity acceleration value for measuring noise;
measuring Z as 04×1;
(2) Measure noise matrixδa,kOutput noise variance for accelerometer, ap,kIs the variance of the motion acceleration of the carrier;
and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition.
The specific steps of the step 3 comprise:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and a residual error queue r with the most recent number of N (taking N as 100) is recordedk-N+1,rk-N+2,……,rk(ii) a The queues are sequenced to obtain new queues r' v which are arranged from small to largek-N+1,r′vk-N+2,……,rk'; wherein, r' vk-N+1≤r′vk-N+2≤…≤r′vk. The remaining data is averaged after the largest and smallest numbers are removed.
Then measure the noise variance matrix RkCan be expressed as
I.e. the filter updates the residual error based on accelerometer measurementsDynamically adjust the measurement noise variance matrix RkThe adaptive adjustment of the filter parameters under different dynamic conditions is realized.
It should be emphasized that the examples described herein are illustrative and not restrictive, and thus the present invention includes, but is not limited to, those examples described in this detailed description, as well as other embodiments that can be derived from the teachings of the present invention by those skilled in the art and that are within the scope of the present invention.
Claims (1)
1. An inertial navigation system attitude estimation method is characterized by comprising the following steps: the method comprises the following steps:
step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation;
step 2, establishing a quaternion pseudo Kalman filtering measurement equation by taking the measurement value of the accelerometer as input;
step 3, performing Kalman filtering measurement updating according to the Kalman filter state equation established in the step 1 and the quaternion pseudo Kalman filtering measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on a measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of a measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under a dynamic condition;
the specific steps of the step 1 comprise:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωy ωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filter state equation, wherein the known state equation has the basic form:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrixWhere T is the filter period, F is the system continuous equation state matrix, WkIs the system noise;
Wherein, I3×3Is a 3 × 3 unit array, δg,kOutputting a noise variance for the gyroscope;
the specific steps of the step 2 comprise:
(1) the basic form of the known measurement equation is:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
04×1=HkQk+Vk
wherein the measuring matrixQkIs an attitude quaternion, VkG is a local gravity acceleration value for measuring noise;
measuring Z as 04×1;
(2) Measure noise matrixδa,kOutput noise variance for accelerometer, ap,kIs the variance of the motion acceleration of the carrier;
the specific steps of the step 3 comprise:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and residual error queues r with the latest number of N are recordedk-N+1,rk-N+2,……,rk;
Sequencing the queues to obtain a new queue r 'arranged from small to large'k-N+1,r′k-N+2,……,r′k(ii) a Wherein, N is 100, r'k-N+1≤r′k-N+2≤…≤r′k
After the maximum number and the minimum number are removed, the residual data are averaged, and then the measurement and update residual value is as follows:
then measure the noise variance matrix RkThe unbiased estimate of (d) may be expressed as:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910571246.7A CN110440756B (en) | 2019-06-28 | 2019-06-28 | Attitude estimation method of inertial navigation system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910571246.7A CN110440756B (en) | 2019-06-28 | 2019-06-28 | Attitude estimation method of inertial navigation system |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110440756A CN110440756A (en) | 2019-11-12 |
CN110440756B true CN110440756B (en) | 2021-12-31 |
Family
ID=68428386
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910571246.7A Active CN110440756B (en) | 2019-06-28 | 2019-06-28 | Attitude estimation method of inertial navigation system |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110440756B (en) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112729348B (en) * | 2021-01-10 | 2023-11-28 | 河南理工大学 | Gesture self-adaptive correction method for IMU system |
CN113175926B (en) * | 2021-04-21 | 2022-06-21 | 哈尔滨工程大学 | Self-adaptive horizontal attitude measurement method based on motion state monitoring |
CN114264304B (en) * | 2021-12-23 | 2023-07-14 | 湖南航天机电设备与特种材料研究所 | High-precision horizontal attitude measurement method and system for complex dynamic environment |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101566477A (en) * | 2009-06-03 | 2009-10-28 | 哈尔滨工程大学 | Quick measurement method of initial attitude of ship local strap-down inertial navigation system |
CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
CN104154914A (en) * | 2014-07-25 | 2014-11-19 | 辽宁工程技术大学 | Initial attitude measurement method of space stabilization type strapdown inertial navigation system |
CN105698792A (en) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | Dynamic MEMS (micro-electromechanical systems) inertial attitude measuring system based on self-adaptive robust integration algorithm |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8626441B2 (en) * | 2008-06-17 | 2014-01-07 | Agco Corporation | Methods and apparatus for using position/attitude information to enhance a vehicle guidance system |
-
2019
- 2019-06-28 CN CN201910571246.7A patent/CN110440756B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101566477A (en) * | 2009-06-03 | 2009-10-28 | 哈尔滨工程大学 | Quick measurement method of initial attitude of ship local strap-down inertial navigation system |
CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
CN104154914A (en) * | 2014-07-25 | 2014-11-19 | 辽宁工程技术大学 | Initial attitude measurement method of space stabilization type strapdown inertial navigation system |
CN105698792A (en) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | Dynamic MEMS (micro-electromechanical systems) inertial attitude measuring system based on self-adaptive robust integration algorithm |
Also Published As
Publication number | Publication date |
---|---|
CN110440756A (en) | 2019-11-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110440756B (en) | Attitude estimation method of inertial navigation system | |
CN109459019B (en) | Vehicle navigation calculation method based on cascade adaptive robust federal filtering | |
CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
CN107014376B (en) | A kind of posture inclination angle estimation method suitable for the accurate operation of agricultural machinery | |
JP6083279B2 (en) | Movement status information calculation method and movement status information calculation device | |
CN110887481A (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
CN109945859B (en) | Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering | |
CN110887480A (en) | Flight attitude estimation method and system based on MEMS sensor | |
CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
CN112070170A (en) | Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method | |
CN114964222A (en) | Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device | |
CN112874529B (en) | Vehicle mass center slip angle estimation method and system based on event trigger state estimation | |
CN109506674B (en) | Acceleration correction method and device | |
CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
CN112284388B (en) | Unmanned aerial vehicle multisource information fusion navigation method | |
CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
CN110132271B (en) | Adaptive Kalman filtering attitude estimation algorithm | |
CN110849364B (en) | Adaptive Kalman attitude estimation method based on communication-in-motion | |
CN110471293B (en) | Z-axis gyroscope sliding mode control method for estimating time-varying angular velocity | |
CN110736459B (en) | Angular deformation measurement error evaluation method for inertial quantity matching alignment | |
CN117091592A (en) | Gesture resolving method, gesture resolving device, and computer storage medium | |
CN114111843B (en) | Initial alignment method for optimal movable base of strapdown inertial navigation system |
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 |