CN108225308B - Quaternion-based attitude calculation method for extended Kalman filtering algorithm - Google Patents

Quaternion-based attitude calculation method for extended Kalman filtering algorithm Download PDF

Info

Publication number
CN108225308B
CN108225308B CN201711179949.2A CN201711179949A CN108225308B CN 108225308 B CN108225308 B CN 108225308B CN 201711179949 A CN201711179949 A CN 201711179949A CN 108225308 B CN108225308 B CN 108225308B
Authority
CN
China
Prior art keywords
axis
matrix
quaternion
sampling
formula
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
Application number
CN201711179949.2A
Other languages
Chinese (zh)
Other versions
CN108225308A (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN201711179949.2A priority Critical patent/CN108225308B/en
Publication of CN108225308A publication Critical patent/CN108225308A/en
Application granted granted Critical
Publication of CN108225308B publication Critical patent/CN108225308B/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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

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

Abstract

The invention discloses an attitude resolving method of an extended Kalman filtering algorithm based on quaternion, which comprises the following steps: acquiring multi-sensor data under a carrier fixed coordinate system; filtering data collected by an accelerometer and a magnetometer, and normalizing the data collected by the two sensors; constructing a state equation of the carrier system according to the quaternion differential equation and the attitude matrix, and determining a process noise variance matrix of the system; constructing a system observation model by using a fast Gaussian-Newton method, and determining a system measurement noise variance matrix; establishing a Kalman filtering recursion equation according to the established system state equation and the observation model; and solving the three attitude angles of the carrier by using the optimal quaternion obtained by recursion. The invention can greatly simplify the calculation amount and solve the problem of unrefined calculation of the existing parameters.

Description

Quaternion-based attitude calculation method for extended Kalman filtering algorithm
Technical Field
The invention relates to the technical field of multi-sensor data fusion of motion carrier navigation, in particular to an attitude calculation method based on a quaternion extended Kalman filtering algorithm.
Background
The method has wide application in the field of multi-sensor data fusion of motion carrier navigation such as human motion tracking, aircraft navigation and the like, and has the function of accurately estimating the posture of the carrier in real time. The rapid development of low cost micro-electromechanical systems has led to the widespread use of smaller and cheaper inertial sensors. However, data measured by a low-cost inertial measurement unit is easily affected by high-frequency noise and time-varying offset, so smoothing and offset-free estimation are required in a sensor data fusion algorithm. To obtain more accurate poses, the accelerometer and magnetometer data are often fused with the angular velocity data output by the gyroscope. The most common techniques used in the nonlinear attitude estimation method are complementary filtering and extended kalman filtering.
Complementary filter coefficients in the complementary filter method need to be corrected in real time in a dynamic system, but no specific model equation exists for correction, and a commonly used method is adaptive adaptation parameters. The unmanned aerial vehicle attitude control system and method based on adaptive complementary fusion proposed by the yellow crane and the like propose a method for obtaining a second attitude angle by using a gradient descent method, but the use of different sensors can cause different adaptive parameter methods, and the estimation of parameters can cause great trouble in the actual process and even cause inaccurate attitude estimation. The 'method for calculating the fusion attitude angle based on the complementary Kalman filtering algorithm' proposed by Li GuoPeng et al proposes a method for combining complementary filtering and Kalman filtering, but the established system model is only suitable for a single-axis system and is not suitable for actual three-axis gyroscopes and three-axis accelerometer systems. The method for solving the quadrotor by combining conjugate gradient and extended Kalman filtering, which is proposed by Chenyang et al, proposes that an extended Kalman filtering observation model is established by using a conjugate gradient method, but the establishment of a system state equation is not accurate enough, and a calculation method of a process noise variance matrix and a measurement noise variance matrix in an actual process is not provided.
Disclosure of Invention
The invention aims to provide an attitude calculation method based on a quaternion extended Kalman filtering algorithm, which can greatly simplify the calculation amount and solve the problem of unrefined parameter calculation in the prior art.
In order to solve the technical problems, the invention provides an attitude calculation method based on a quaternion extended Kalman filtering algorithm, which comprises the following steps:
(1) establishing an attitude estimation system, and acquiring multi-axis sensor data under a carrier fixed reference system coordinate system (namely a system b, a right-front-upper rectangular coordinate system with a coordinate origin positioned at the center of an inertial measurement component), three-axis angular velocity data acquired by a gyroscope, three-axis acceleration data acquired by an accelerometer and three-axis magnetic induction intensity data acquired by a magnetometer;
(2) filtering the acquired acceleration data and the acquired magnetic induction intensity data, and normalizing the data acquired by the two sensors; the data collected by the three-axis gyroscope is w ═ wx wy wz]TWherein w isx、wy、wzRespectively representing three-axis angular velocity data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system; t represents transposition; the data collected by the normalized triaxial accelerometer is a ═ ax ay az]TWherein a isx、ay、azRespectively representing three-axis acceleration data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system; the data collected by the normalized three-axis magnetometer is m ═ mx my mz]TWherein m isx、my、mzRespectively representing three-axis magnetic induction intensity data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system;
(3) constructing a state equation of the carrier system according to the quaternion differential equation and the attitude matrix, and obtaining a process noise variance matrix of the system;
(4) constructing an observation model of the system by using a fast Gaussian-Newton method, and obtaining a measurement noise variance matrix of the system;
(5) establishing an extended Kalman filtering recursion equation according to the established system state equation and the observation model;
(6) and resolving three attitude angles of the carrier by using the optimal quaternion obtained by each recursion: the heading angle is psi, the pitch angle is theta, and the roll angle is gamma.
Preferably, in the step (3), a state equation of the system is constructed according to the quaternion differential equation and the attitude matrix, and a specific process of obtaining the noise variance matrix of the system process is as follows:
the quaternion differential equation is as follows:
Figure BDA0001478975730000021
in the formula, qkRepresenting a quaternion of k sampling instants, where qk＝[qk1 qk2 qk3 qk0]T，[qk1 qk2 qk3]TIs a quaternion qkThe vector portion of (1) with
Figure BDA0001478975730000022
Is represented by qk0Is a scalar section;
Figure BDA0001478975730000023
denotes qkDifferentiation of (1); w is akData collected by the gyroscope representing k sampling moments; wherein omega (w)k) Is defined as follows:
Figure BDA0001478975730000024
Figure BDA0001478975730000025
in the formula, wk＝[wkx wky wkz]TWherein w iskx、wky、wkzAngular velocity data of the gyroscope in the x-axis direction, the y-axis direction and the z-axis direction are respectively acquired by a carrier fixed coordinate system at the sampling moment of k times; t represents transposition;
the discrete-time model obtained from the quaternion differential equation is:
Figure BDA0001478975730000031
in the formula, qk+1Expressing quaternion of k +1 sampling time; omegakIs Ω (w) of the sampling instants k timesk)；qkThe quaternion is a quaternion of the sampling moment of k times, and q (0) is a quaternion obtained under the initial condition; t issIs a sampling period;
from quaternion qk＝[qk1 qk2 qk3 qk0]TAnd attitude matrix
Figure BDA0001478975730000032
(n is the geographical coordinate system of east-north-sky) the attitude matrix is obtained as follows:
Figure BDA0001478975730000033
taking the attitude quaternion as the state of the system, namely:
Xk＝qk
in the formula, XkRepresenting a state matrix at k sampling moments; q. q.skRepresenting a quaternion of k sampling instants;
the state equation of the established system is as follows:
Xk+1＝ΦkXk+VK
in the formula, Xk+1Representing a state matrix at k +1 sampling moments; phikRepresenting a state transition matrix at k sampling moments; xkRepresenting a quaternion of k sampling instants; vKSampling a process noise array at the moment of k times;
Φk＝exp(ΩkTs)
in the formula, omegakIs Ω (w) of k sampling instantsk)；TsIs a sampling period;
Figure BDA0001478975730000034
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000035
is Gaussian white noise, and has a covariance matrix of ∑k＝||ekI is a 3-order identity matrix; xikAn error coefficient matrix representing k sampling instants;
Figure BDA0001478975730000036
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000037
acceleration a measured for k sampling momentsk＝[akx aky akz]TWith predicted acceleration vk＝[vkxvky vkz]TRotational error between akx、aky、akzThree-axis acceleration data v acquired for the x-axis, the y-axis and the z-axis actually measured under the carrier fixed coordinate system at the sampling time k timeskx、vky、vkzAcquiring triaxial acceleration data of an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling moment k times;
Figure BDA0001478975730000038
magnetic induction m actually measured at k sampling momentsk＝[mkx mky mkz]TAnd predicted magnetic induction hk＝[hkx hky hkz]TRotational error between mkx、mky、mkzThree-axis magnetic induction intensity data h acquired by x-axis, y-axis and z-axis measured under a carrier fixed coordinate system at the sampling time k timeskx、hky、hkzThree-axis magnetic induction intensity data collected for an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling time k times; acceleration prediction vector vk＝[vkx vky vkz]TAnd the magnetic induction intensity prediction vector hk＝[hkx hky hkz]TThe projections of the earth gravity vector and the earth magnetic field intensity vector in the navigation coordinate system in an ideal state in a carrier coordinate system are respectively obtained, and meanwhile, the projections can be obtained through attitude conversion;
the system process noise variance matrix calculation formula is as follows:
Qk＝(Ts/2)2ΞkΣkΞk T。
preferably, in the step (4), an observation model of the system is constructed by using a fast gaussian-newton method, and a specific process of obtaining a system measurement noise variance matrix is as follows:
using measured acceleration ak＝[akx aky akz]TSubtracting the predicted acceleration vk＝[vkx vky vkz]TAnd the measured magnetic induction mk＝[mkx mky mkz]TSubtracting the predicted magnetic induction hk＝[hkx hky hkz]TObtaining an error function e (q)k) The error function is calculated as follows:
Figure BDA0001478975730000041
finding the satisfied form by fast Gauss-Newton method
Figure BDA0001478975730000042
Quaternion of (2)
Figure BDA0001478975730000043
The fast gauss-newton method formula is as follows:
Figure BDA0001478975730000044
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000045
representing an optimal quaternion solution for the k +1 sampling instants;
Figure BDA0001478975730000046
predicting a quaternion value for one step at the sampling moment of k times; lambda [ alpha ]kChanging to an optimal value during each iteration; j (0) is
Figure BDA0001478975730000047
Corresponding toThe jacobian matrix, jacobian matrix J (k) is calculated as follows:
Figure BDA0001478975730000051
selecting the optimal solution obtained by the rapid Gauss-Newton method as an observed value, namely:
Figure BDA0001478975730000052
order to
Figure BDA0001478975730000053
The system measurement noise variance matrix is obtained as follows:
Figure BDA0001478975730000054
in the formula I3×3Is a 3 rd order identity matrix.
Preferably, in step (5), the following extended kalman filter recursion equation is established according to the established system state equation and the system observation model:
the state one-step prediction equation is calculated as follows:
Xk,k-1＝ΦkXk-1
in the formula, Xk-1Representing a state matrix at k-1 sampling moments; xk,k-1Representing a prediction state matrix at k sampling moments;
the one-step prediction variance matrix calculation formula is as follows:
Figure BDA0001478975730000055
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000056
is a matrix phikTransposing; qk-1A system process noise variance matrix at the sampling moment of k-1 times; pk-1Representing an estimated error variance matrix at the sampling time of k-1 times; pk,k-1A one-step prediction error variance matrix representing k sampling moments;
the filter gain matrix calculation formula is as follows:
Figure BDA0001478975730000057
in the formula, RkRepresenting a system measurement noise variance matrix at the sampling moment of k times; t represents transposition; hkIs a 4-order identity matrix and is an observation matrix at k sampling moments; kkA gain matrix representing k sampling instants;
the state estimation calculation formula is as follows:
Figure BDA0001478975730000061
in the formula, ZkAn observed value calculated by a fast gauss-newton method representing the sampling time of k times;
the estimation error variance matrix calculation formula is as follows:
Pk＝[I4×4-KkHk]Pk,k-1
in the formula I4×4Representing a fourth order identity matrix; pkRepresenting the estimated error variance matrix at k sample instants.
Preferably, in step (6), the optimal quaternion obtained in each recursion is used to solve three attitude angles of the carrier: the specific process that the heading angle is psi, the pitch angle is theta and the roll angle is gamma is as follows:
from the relationship between the directional cosine matrix and the quaternion, we can obtain:
Figure BDA0001478975730000062
γk＝arctan[-(qk1qk3-qk2qk0)/0.5-qk1 2-qk2 2)]
Ψk＝[(qk1qk2-qk3qk0)/0.5-qk1 2-qk3 2)]
in the formula, thetak、γk、ΨkRespectively representing a course angle, a pitch angle and a roll angle at k sampling moments.
The invention has the beneficial effects that: (1) the attitude calculation method based on the quaternion extended Kalman filtering algorithm is a quaternion estimation method, a system process equation is a linear model, and the method has the advantages of high estimation precision and uncomplicated calculation; (2) the invention provides a method for establishing a system observation model by using a fast Gaussian-Newton method, which greatly simplifies the calculated amount and has good effect compared with other extended Kalman filtering methods; (3) the invention provides a method for calculating a process noise variance matrix and a measurement noise variance matrix in an actual process, and solves the problems of unrefined parameter calculation and the like in the prior art.
Drawings
FIG. 1 is a schematic flow chart of the method of the present invention.
Figure 2 is a schematic of calibration gyroscope, accelerometer and magnetometer data recorded from an AHRS device as employed in the present invention.
Fig. 3 is a schematic diagram of euler angle data obtained by attitude calculation according to the present invention.
Detailed Description
As shown in fig. 1, the attitude solution method based on the quaternion extended kalman filter algorithm of the present invention includes the following steps:
step 1: constructing a posture estimation system, and acquiring multi-axis sensor data under a carrier fixed reference system coordinate system;
step 2: filtering the acquired acceleration data and the acquired magnetic induction intensity data, and normalizing the data acquired by the two sensors;
the data collected by the three-axis gyroscope is w ═ wx wy wz]TAnd the data collected by the normalized triaxial accelerometer is a ═ ax ay az]TAnd the data collected by the normalized three-axis magnetometer is m ═ mx my mz]T；
And step 3: constructing a state equation of the carrier system according to the quaternion differential equation and the attitude matrix, and obtaining a process noise variance matrix of the system;
and 4, step 4: constructing an observation model of the system by using a fast Gaussian-Newton method, and obtaining a measurement noise variance matrix of the system;
and 5: establishing an extended Kalman filtering recursion equation according to the established system state equation and the observation model;
step 6: and resolving three attitude angles of the carrier by using the optimal quaternion obtained by each recursion: the course angle is psi, the pitch angle is theta, and the roll angle is gamma;
in the above, a specific process of constructing a state equation of the system and calculating a process noise variance matrix of the system is as follows:
the quaternion differential equation is as follows:
Figure BDA0001478975730000071
in the formula, qkRepresenting a quaternion of k sampling instants, where qk＝[qk1 qk2 qk3 qk0]T，[qk1 qk2 qk3]TIs a quaternion qkThe vector portion of (1) with
Figure BDA0001478975730000072
Is represented by qk0Is a scalar section;
Figure BDA0001478975730000073
denotes qkDifferentiation of (1); w is akData collected by the gyroscope at the sampling time k times; wherein omega (w)k) Is defined as follows:
Figure BDA0001478975730000074
Figure BDA0001478975730000075
in the formula, wk＝[wkx wky wkz]TWherein w iskx、wky、wkzAngular velocity data in the directions of an x axis, a y axis and a z axis are respectively collected by the gyroscope at the sampling time of k times; t represents transposition;
the discretized time model obtained from the quaternion differential equation is as follows:
Figure BDA0001478975730000076
in the formula, qk+1Expressing quaternion of k +1 sampling time; omegakIs Ω (w) of the sampling instants k timesk)；qkThe quaternion is a quaternion of the sampling moment of k times, and q (0) is a quaternion obtained under the initial condition; t issIs a sampling period;
by quaternion qk＝[qk1 qk2 qk3 qk0]TAn attitude matrix can be obtained
Figure BDA0001478975730000081
n system (navigation coordinate system): an "east-north-sky" geographic coordinate system;
Figure BDA0001478975730000082
taking the attitude quaternion as the state of the system, namely:
Xk＝qk
in the formula, XkRepresenting a state matrix at k sampling moments; q. q.skRepresenting a quaternion of k sampling instants;
the equation of state of the system is established as follows
Xk+1＝ΦkXk+VK
In the formula, Xk+1Representing a state matrix at k +1 sampling moments; phikRepresenting a state transition matrix at k sampling moments; xkRepresenting a quaternion of k sampling instants; vKSampling a process noise array at the moment of k times;
Φk＝exp(ΩkTs)
in the formula, omegakIs Ω (w) of k sampling instantsk)；TsIs a sampling period;
Figure BDA0001478975730000083
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000084
is Gaussian white noise, and has a covariance matrix of ∑k＝||ekI is a 3-order identity matrix; xikAn error coefficient matrix representing k sampling instants;
Figure BDA0001478975730000085
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000086
acceleration vector a actually measured at k sampling momentsk＝[akx aky akz]TAnd an acceleration prediction vector vk＝[vkx vky vkz]TRotational error between akx、aky、akzThree-axis acceleration data v acquired for the x-axis, the y-axis and the z-axis actually measured under the carrier fixed coordinate system at the sampling time k timeskx、vky、vkzAcquiring triaxial acceleration data of an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling moment k times;
Figure BDA0001478975730000087
is sampled k timesMagnetic induction intensity vector m actually measured at momentk＝[mkxmky mkz]TAnd magnetic induction prediction vector hk＝[hkx hky hkz]TRotational error between mkx、mky、mkzThree-axis magnetic induction intensity data h acquired by x-axis, y-axis and z-axis measured under a carrier fixed coordinate system at the sampling time k timeskx、hky、hkzThree-axis magnetic induction intensity data collected for an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling time k times;
acceleration prediction vector vk＝[vkx vky vkz]TAnd the magnetic induction intensity prediction vector hk＝[hkx hky hkz]TThe projection process is obtained by attitude conversion, and the calculation formula is as follows:
Figure BDA0001478975730000091
Figure BDA0001478975730000092
Figure BDA0001478975730000093
Figure BDA0001478975730000094
bkz＝dkz
Figure BDA0001478975730000095
Figure BDA0001478975730000096
in the formula, in the formula [ dkx dky dkz]TIs the output of the magnetometer in the n series; [ b ] akx 0 bkz]TIs the magnitude and direction of the assumed actual earth magnetic field;
Figure BDA0001478975730000097
is that
Figure BDA0001478975730000098
Transposing;
the system process noise variance matrix calculation formula is as follows:
Qk＝(Ts/2)2ΞkΣkΞk T。
in the above technical solution, the specific process of obtaining the observation value and solving the measurement noise variance matrix of the system in step 4 is as follows: using measured acceleration ak＝[akx aky akz]TSubtracting the predicted acceleration vk＝[vkx vky vkz]TAnd the measured magnetic induction mk＝[mkx mky mkz]TSubtracting the predicted magnetic induction hk＝[hkx hky hkz]TObtaining an error function e (q)k) The error function is calculated as follows:
Figure BDA0001478975730000101
finding the satisfied form by fast Gauss-Newton method
Figure BDA0001478975730000102
Quaternion of (2)
Figure BDA0001478975730000103
Figure BDA0001478975730000104
The fast gauss-newton method formula is as follows:
Figure BDA0001478975730000105
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000106
representing an optimal quaternion solution for the k +1 sampling instants;
Figure BDA0001478975730000107
predicting a quaternion value for one step at the sampling moment of k times; lambda [ alpha ]kChanging to an optimal value during each iteration; j (0) is
Figure BDA00014789757300001012
The corresponding Jacobian matrix, Jacobian matrix J (k), the computational formula is as follows:
Figure BDA0001478975730000108
the optimal solution obtained by the fast Gauss-Newton method is selected as the observed value, namely
Figure BDA0001478975730000109
Order to
Figure BDA00014789757300001010
The system measurement noise variance matrix is obtained as follows:
Figure BDA00014789757300001011
in the formula I3×3Is a 3-order identity matrix;
in the above technical solution, the specific process of the extended kalman filter recursion equation obtained in step 5 is as follows: the state one-step prediction equation is calculated as follows:
Xk,k-1＝ΦkXk-1
in the formula, Xk-1Representing a state matrix at k-1 sampling moments; xk,k-1Representing a prediction state matrix at k sampling moments;
the one-step prediction variance matrix calculation formula is as follows:
Figure BDA0001478975730000111
in the formula (I), the compound is shown in the specification,
Figure BDA0001478975730000112
is a matrix phikTranspose of (Q)k-1A system process noise variance matrix at the sampling moment of k-1 times; pk-1Representing an estimated error variance matrix at the sampling time of k-1 times; pk,k-1A one-step prediction error variance matrix representing k sampling moments;
the filter gain matrix calculation formula is as follows:
Figure BDA0001478975730000113
in the formula, RkRepresenting a system measurement noise variance matrix at the sampling moment of k times; t represents transposition; hkIs a 4-order identity matrix and is an observation matrix at k sampling moments; kkA gain matrix representing k sampling instants;
the state estimation calculation formula is as follows:
Figure BDA0001478975730000114
in the formula, ZkAn observed value calculated by a fast gauss-newton method representing the sampling time of k times;
the estimation error variance matrix calculation formula is as follows:
Pk＝[I4×4-KkHk]Pk,k-1
in the formula I4×4Representing a fourth order identity matrix; pkAn estimation error variance matrix representing k sampling instants;
in the above technical solution, step 6 uses the optimal quaternion obtained by recursion each time to resolve the three attitude angles of the carrier: the specific process that the heading angle is psi, the pitch angle is theta and the roll angle is gamma is as follows:
from the relationship between the directional cosine matrix and the quaternion, we can obtain:
Figure BDA0001478975730000115
γk＝arctan[-(qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk＝[(qk1qk2-qk3qk0)/（0.5-qk1 2-qk3 2)]
in the formula, thetak、γk、ΨkRespectively representing a course angle, a pitch angle and a roll angle at k sampling moments.
While the invention has been shown and described with respect to the preferred embodiments, it will be understood by those skilled in the art that various changes and modifications may be made without departing from the scope of the invention as defined in the following claims.

Claims (3)

1. An attitude calculation method based on a quaternion extended Kalman filtering algorithm is characterized by comprising the following steps:
(1) establishing an attitude estimation system, acquiring multi-axis sensor data under a carrier fixed coordinate system, namely a system b, a right-front-upper rectangular coordinate system with a coordinate origin positioned at the center of an inertial measurement component, triaxial angular velocity data acquired by a gyroscope, triaxial acceleration data acquired by an accelerometer and triaxial magnetic induction intensity data acquired by a magnetometer; taking the east-north-sky geographic coordinate system as a navigation coordinate system, namely an n system;
(2) filtering the acquired acceleration data and the acquired magnetic induction intensity data, and normalizing the data acquired by the two sensors; the data collected by the three-axis gyroscope is w ═ wx wy wz]TWherein w isx、wy、wzRespectively representing three-axis angular velocity data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system; t represents transposition; the data collected by the normalized triaxial accelerometer is a ═ ax ay az]TWherein a isx、ay、azRespectively representing three-axis acceleration data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system; the data collected by the normalized three-axis magnetometer is m ═ mx my mz]TWherein m isx、my、mzRespectively representing three-axis magnetic induction intensity data acquired by an x axis, a y axis and a z axis in a carrier fixed coordinate system;
(3) constructing a state equation of the carrier system according to the quaternion differential equation and the attitude matrix, and obtaining a process noise variance matrix of the system; the specific process is as follows:
the quaternion differential equation is as follows:
Figure FDA0002989783560000011
in the formula, qkRepresenting a quaternion of k sampling instants, where qk＝[qk1 qk2 qk3 qk0]T，[qk1 qk2 qk3]TIs a quaternion qkThe vector portion of (1) with
Figure FDA0002989783560000012
Is represented by qk0Is a scalar section;
Figure FDA0002989783560000013
denotes qkDifferentiation of (1); w is akData collected by the gyroscope representing k sampling moments; wherein omega (w)k) Is defined as follows:
Figure FDA0002989783560000014
Figure FDA0002989783560000015
in the formula, wk＝[wkx wky wkz]TWherein w iskx、wky、wkzAngular velocity data of the gyroscope in the x-axis direction, the y-axis direction and the z-axis direction are respectively acquired by a carrier fixed coordinate system at the sampling moment of k times; t represents transposition;
the discrete-time model obtained from the quaternion differential equation is:
Figure FDA0002989783560000021
in the formula, qk+1Expressing quaternion of k +1 sampling time; omegakIs Ω (w) of the sampling instants k timesk)；qkThe quaternion is a quaternion of the sampling moment of k times, and q (0) is a quaternion obtained under the initial condition; t issIs a sampling period;
from quaternion qk＝[qk1 qk2 qk3 qk0]TAnd attitude matrix
Figure FDA0002989783560000022
The relationship of (a) yields the attitude matrix as follows:
Figure FDA0002989783560000023
taking the attitude quaternion as the state of the system, namely:
Xk＝qk
in the formula, XkRepresenting a state matrix at k sampling moments; q. q.skRepresenting a quaternion of k sampling instants;
the state equation of the established system is as follows:
Xk+1＝ΦkXk+VK
in the formula, Xk+1Representing a state matrix at k +1 sampling moments; phikRepresenting a state transition matrix at k sampling moments; xkRepresenting a quaternion of k sampling instants; vKSampling a process noise array at the moment of k times;
Φk＝exp(ΩkTs)
in the formula, omegakIs Ω (w) of k sampling instantsk)；TsIs a sampling period;
Figure FDA0002989783560000024
in the formula (I), the compound is shown in the specification,
Figure FDA0002989783560000025
is Gaussian white noise, and has a covariance matrix of Σk＝||ekI is a 3-order identity matrix; xikAn error coefficient matrix representing k sampling instants;
Figure FDA0002989783560000026
in the formula (I), the compound is shown in the specification,
Figure FDA0002989783560000027
acceleration a measured for k sampling momentsk＝[akx aky akz]TWith predicted acceleration vk＝[vkx vkyvkz]TRotational error between akx、aky、akzIs k timesThree-axis acceleration data v acquired by an x axis, a y axis and a z axis actually measured under a carrier fixed coordinate system at sampling momentkx、vky、vkzAcquiring triaxial acceleration data of an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling moment k times;
Figure FDA0002989783560000028
magnetic induction m actually measured at k sampling momentsk＝[mkx mky mkz]TAnd predicted magnetic induction hk＝[hkx hky hkz]TRotational error between mkx、mky、mkzThree-axis magnetic induction intensity data h acquired by x-axis, y-axis and z-axis measured under a carrier fixed coordinate system at the sampling time k timeskx、hky、hkzThree-axis magnetic induction intensity data collected for an x axis, a y axis and a z axis predicted under a carrier fixed coordinate system at the sampling time k times; acceleration prediction vector vk＝[vkxvky vkz]TAnd the magnetic induction intensity prediction vector hk＝[hkx hky hkz]TRespectively projecting the earth gravity vector and the geomagnetic field intensity vector in a navigation coordinate system in an ideal state in a carrier coordinate system, and simultaneously obtaining the projections through attitude conversion;
the system process noise variance matrix calculation formula is as follows:
Qk＝(Ts/2)2Ξk∑kΞk T；
(4) constructing an observation model of the system by using a fast Gaussian-Newton method, and obtaining a measurement noise variance matrix of the system; the specific process is as follows:
using measured acceleration ak＝[akx aky akz]TSubtracting the predicted acceleration vk＝[vkx vky vkz]TAnd the measured magnetic induction mk＝[mkx mky mkz]TSubtracting the predicted magnetismStrength of induction hk＝[hkx hky hkz]TObtaining an error function e (q)k) The error function is calculated as follows:
Figure FDA0002989783560000031
finding the satisfied form by fast Gauss-Newton method
Figure FDA0002989783560000032
Quaternion of (2)
Figure FDA0002989783560000033
Of [ b ] is determinedkx 0 bkz]TIs the magnitude and direction of the assumed actual earth magnetic field, the fast gauss-newton method formula is as follows:
Figure FDA0002989783560000034
in the formula (I), the compound is shown in the specification,
Figure FDA0002989783560000035
representing an optimal quaternion solution for the k +1 sampling instants;
Figure FDA0002989783560000036
predicting a quaternion value for one step at the sampling moment of k times; lambda [ alpha ]kChanging to an optimal value during each iteration; j (0) is
Figure FDA0002989783560000037
The corresponding Jacobian matrix, Jacobian matrix J (k), the computational formula is as follows:
Figure FDA0002989783560000041
selecting the optimal solution obtained by the rapid Gauss-Newton method as an observed value, namely:
Figure FDA0002989783560000042
order to
Figure FDA0002989783560000043
The system measurement noise variance matrix is obtained as follows:
Figure FDA0002989783560000044
in the formula I3×3Is a 3 rd order identity matrix.
2. The quaternion-based attitude solution method for extended kalman filter algorithm according to claim 1, wherein in the step (5), the following extended kalman filter recursion equation is established according to the established system state equation and the system observation model:
the state one-step prediction equation is calculated as follows:
Xk，k-1＝ΦkXk-1
in the formula, Xk-1Representing a state matrix at k-1 sampling moments; xk，k-1Representing a prediction state matrix at k sampling moments;
the one-step prediction variance matrix calculation formula is as follows:
Figure FDA0002989783560000045
in the formula (I), the compound is shown in the specification,
Figure FDA0002989783560000046
is a matrix phikTransposing; qk-1A system process noise variance matrix at the sampling moment of k-1 times; pk-1Represents k-1 timesAn estimated error variance matrix at the sampling moment; pk，k-1A one-step prediction variance matrix representing k sampling instants;
the filter gain matrix calculation formula is as follows:
Figure FDA0002989783560000047
in the formula, RkRepresenting a system measurement noise variance matrix at k sampling moments; t represents transposition; hkIs a 4-order identity matrix and is an observation matrix at k sampling moments; kkA gain matrix representing k sampling instants;
the state estimation calculation formula is as follows:
Figure FDA0002989783560000051
in the formula, ZkAn observed value calculated by a fast Gauss-Newton method and representing k sampling moments;
the estimation error variance matrix calculation formula is as follows:
Pk＝[I4×4-KkHk]Pk，k-1
in the formula I4×4Representing a fourth order identity matrix; pkAn estimation error variance matrix representing k sampling instants;
(5) establishing an extended Kalman filtering recursion equation according to the established system state equation and the observation model;
(6) and resolving three attitude angles of the carrier by using the optimal quaternion obtained by each recursion: the heading angle is psi, the pitch angle is theta, and the roll angle is gamma.
3. The quaternion-based attitude solution method for extended kalman filter algorithm according to claim 1, characterized in that in step (6), the best quaternion obtained from each recursion is used to solve the three attitude angles of the carrier: the specific process that the heading angle is psi, the pitch angle is theta and the roll angle is gamma is as follows:
from the relationship between the directional cosine matrix and the quaternion, we can obtain:
Figure FDA0002989783560000052
γk＝arctan[-(qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk＝[(qk1qk2-qk3qk0)/(0.5-qk1 2-qk3 2)]
in the formula, thetak、γk、ΨkRespectively representing a course angle, a pitch angle and a roll angle at k sampling moments.
CN201711179949.2A 2017-11-23 2017-11-23 Quaternion-based attitude calculation method for extended Kalman filtering algorithm Active CN108225308B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711179949.2A CN108225308B (en) 2017-11-23 2017-11-23 Quaternion-based attitude calculation method for extended Kalman filtering algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711179949.2A CN108225308B (en) 2017-11-23 2017-11-23 Quaternion-based attitude calculation method for extended Kalman filtering algorithm

Publications (2)

Publication Number Publication Date
CN108225308A CN108225308A (en) 2018-06-29
CN108225308B true CN108225308B (en) 2021-06-25

Family

ID=62653564

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711179949.2A Active CN108225308B (en) 2017-11-23 2017-11-23 Quaternion-based attitude calculation method for extended Kalman filtering algorithm

Country Status (1)

Country Link
CN (1) CN108225308B (en)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827313A (en) * 2018-08-10 2018-11-16 哈尔滨工业大学 Multi-mode rotor craft Attitude estimation method based on extended Kalman filter
CN109990776B (en) * 2019-04-12 2021-09-24 武汉深海蓝科技有限公司 Attitude measurement method and device
CN109883451A (en) * 2019-04-15 2019-06-14 山东建筑大学 A kind of adaptive gain complementary filter method and system for the estimation of pedestrian orientation
CN110174121A (en) * 2019-04-30 2019-08-27 西北工业大学 A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction
CN110207647B (en) * 2019-05-08 2021-11-09 诺百爱（杭州）科技有限责任公司 Arm ring attitude angle calculation method based on complementary Kalman filter
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
CN112181046B (en) * 2019-07-03 2022-09-30 深圳拓邦股份有限公司 Knob gear output method and device, knob device and storage device
CN110319840A (en) * 2019-07-05 2019-10-11 东北大学秦皇岛分校 Conjugate gradient attitude algorithm method towards abnormal gait identification
CN110307842B (en) * 2019-07-08 2022-03-25 常州大学 Fast extended Kalman method for inertia-geomagnetic combination
CN111551174A (en) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN111426318B (en) * 2020-04-22 2024-01-26 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111625768B (en) * 2020-05-19 2023-05-30 西安因诺航空科技有限公司 Hand-held cradle head posture estimation method based on extended Kalman filtering
CN111896007B (en) * 2020-08-12 2022-06-21 智能移动机器人(中山)研究院 Attitude calculation method for quadruped robot for compensating foot-ground impact
CN112414364B (en) * 2020-11-04 2022-10-04 国网福建省电力有限公司建设分公司 Attitude monitoring device and method for suspension holding pole
CN113114105B (en) * 2021-03-10 2022-08-09 上海工程技术大学 Dynamic measurement method for output characteristics of photovoltaic cell assembly
CN113830220B (en) * 2021-11-04 2022-09-13 浙江欧飞电动车有限公司 Electric vehicle power-assisted control method based on information fusion
CN114053679A (en) * 2021-11-19 2022-02-18 上海复动医疗管理有限公司 Exercise training method and system
CN115855104A (en) * 2022-11-25 2023-03-28 哈尔滨工程大学 Optimal online evaluation method for combined navigation filtering result
CN116070066B (en) * 2023-02-20 2024-03-15 北京自动化控制设备研究所 Method for calculating rolling angle of guided projectile

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6453239B1 (en) * 1999-06-08 2002-09-17 Schlumberger Technology Corporation Method and apparatus for borehole surveying
CN101915580A (en) * 2010-07-14 2010-12-15 中国科学院自动化研究所 Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology
CN106500693A (en) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 A kind of AHRS algorithms based on adaptive extended kalman filtering
CN106595711A (en) * 2016-12-21 2017-04-26 东南大学 Strapdown inertial navigation system coarse alignment method based on recursive quaternion
CN106767837A (en) * 2017-02-23 2017-05-31 哈尔滨工业大学 Based on the spacecraft attitude method of estimation that volume quaternary number is estimated
WO2017089236A1 (en) * 2015-11-24 2017-06-01 Vinati S.R.L. Method of estimating an attitude of a control device for controlling operating machines
CN106979780A (en) * 2017-05-22 2017-07-25 深圳市靖洲科技有限公司 A kind of unmanned vehicle real-time attitude measuring method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10114464B2 (en) * 2015-06-30 2018-10-30 Stmicroelectronics S.R.L. Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6453239B1 (en) * 1999-06-08 2002-09-17 Schlumberger Technology Corporation Method and apparatus for borehole surveying
CN101915580A (en) * 2010-07-14 2010-12-15 中国科学院自动化研究所 Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology
WO2017089236A1 (en) * 2015-11-24 2017-06-01 Vinati S.R.L. Method of estimating an attitude of a control device for controlling operating machines
CN106500693A (en) * 2016-12-07 2017-03-15 中国电子科技集团公司第五十四研究所 A kind of AHRS algorithms based on adaptive extended kalman filtering
CN106595711A (en) * 2016-12-21 2017-04-26 东南大学 Strapdown inertial navigation system coarse alignment method based on recursive quaternion
CN106767837A (en) * 2017-02-23 2017-05-31 哈尔滨工业大学 Based on the spacecraft attitude method of estimation that volume quaternary number is estimated
CN106979780A (en) * 2017-05-22 2017-07-25 深圳市靖洲科技有限公司 A kind of unmanned vehicle real-time attitude measuring method

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A Kalman Filter for SINS Self-Alignment Based on Vector Observation;Xu Xiang,etc;《SENSORS》;20170228;第17卷(第2期);第1-19页 *
Quaternion-EKF的多源传感器联合定向算法;赵文晔等;《测绘科学技术学报》;20161231;第33卷(第3期);第247-251页 *
基于四元数自适应卡尔曼滤波的快速对准算法;徐晓苏等;《中国惯性技术学报》;20160831;第24卷(第4期);第454-459页 *

Also Published As

Publication number Publication date
CN108225308A (en) 2018-06-29

Similar Documents

Publication Publication Date Title
CN108225308B (en) Quaternion-based attitude calculation method for extended Kalman filtering algorithm
WO2020253854A1 (en) Mobile robot posture angle calculation method
WO2017063387A1 (en) Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN106052685B (en) A kind of posture and course estimation method of two-stage separation fusion
Cheviron et al. Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV
WO2018214227A1 (en) Unmanned vehicle real-time posture measurement method
CN109696183A (en) The scaling method and device of Inertial Measurement Unit
JP5704561B2 (en) Traveling direction estimation device, portable terminal, control program, computer-readable recording medium, and traveling direction estimation method
CN111551174A (en) High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN107478223A (en) A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN108458714B (en) Euler angle solving method without gravity acceleration in attitude detection system
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN104197927A (en) Real-time navigation system and real-time navigation method for underwater structure detection robot
CN108318038A (en) A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN109682377A (en) A kind of Attitude estimation method based on the decline of dynamic step length gradient
CN112504275B (en) Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
CN112683269B (en) MARG attitude calculation method with motion acceleration compensation
CN108627152B (en) Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN110793515A (en) Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition
CN111189474A (en) Autonomous calibration method of MARG sensor based on MEMS
CN112665574A (en) Underwater robot attitude acquisition method based on momentum gradient descent method
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction

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