CN108225308A - A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number - Google Patents

A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number Download PDF

Info

Publication number
CN108225308A
CN108225308A CN201711179949.2A CN201711179949A CN108225308A CN 108225308 A CN108225308 A CN 108225308A CN 201711179949 A CN201711179949 A CN 201711179949A CN 108225308 A CN108225308 A CN 108225308A
Authority
CN
China
Prior art keywords
axis
sampling instant
matrix
formula
represent
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
CN201711179949.2A
Other languages
Chinese (zh)
Other versions
CN108225308B (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

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 a kind of attitude algorithm methods of the expanded Kalman filtration algorithm based on quaternary number, include the following steps：Obtain multi-sensor data under carrier fixed coordinate system；The data acquired to accelerometer and magnetometer are filtered, and do normalized to the data of the two sensors acquisition；According to quaternion differential equation and the state equation of attitude matrix carrier construction system, and determine the process noise covariance matrix of system；Systematic observation model is built, and determine system measurements noise variance matrix using quick gauss-newton method；Kalman filtering recurrence equation is established according to the system state equation of foundation and observation model；Three attitude angles of carrier are calculated using the best quaternary number that recursion obtains.The present invention can greatly simplify calculation amount, and it is unknown to solve the problems, such as that existing parameter calculates.

Description

A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number
Technical field
The present invention relates to the multisensor Data Fusion technology fields of motion carrier navigation, especially a kind of to be based on quaternary number Expanded Kalman filtration algorithm attitude algorithm method.
Background technology
In the Fusion field of the motion carriers such as mans motion simulation, aircraft navigation navigation, to carrier Posture carry out accurately real-time estimation have a wide range of applications.The rapid development of the MEMS of low cost causes smaller more Cheap inertial sensor is widely used.But the data that the Inertial Measurement Unit of low cost measures are easily by height The influence of the biasing of frequency noise and time-varying, so needing to be smoothed in Data Fusion of Sensor algorithm and estimate without biasing Meter.In order to obtain more accurately posture, usually by the data of accelerometer and magnetometer and the angular velocity data of gyroscope output Fusion.And technology used in most common nonlinear attitude method of estimation is exactly complementary filter and Extended Kalman filter.
Complementary filter coefficient in complementary filter method needs to correct in real time in a dynamic system, but how to correct not There is specific model equation, the method often used is joined using adaptive adjust.Huang He et al. is proposed " a kind of based on adaptive mutual Mend the pose control system for unmanned plane and method of fusion " method that the second attitude angle is obtained using gradient descent method is proposed, But the use of different sensors can cause adaptively to adjust the method difference of ginseng, the estimation of parameter can make in real process Into very big trouble, it is inaccurate to even result in Attitude estimation.Lee state friend et al. is proposed " a kind of to be calculated based on complementary Kalman filtering The method that method calculates fusion attitude angle " proposes the method that complementary filter is combined with Kalman filtering, but the system of foundation Model is only applicable to one-axis system, is not suitable for practical three-axis gyroscope and 3-axis acceleration meter systems.What Chen Yang et al. was proposed " the quadrotor calculation method that conjugate gradient is combined with Extended Kalman filter ", which proposes to establish using conjugate gradient method, to be extended Kalman filtering observation model, but system state equation is established inaccurate, does not provide process noise covariance matrix and survey Measure computational methods of the noise variance matrix in real process.
Invention content
The technical problems to be solved by the invention are, provide a kind of expanded Kalman filtration algorithm based on quaternary number Attitude algorithm method can greatly simplify calculation amount, and it is unknown to solve the problems, such as that existing parameter calculates.
In order to solve the above technical problems, the present invention provides a kind of posture of the expanded Kalman filtration algorithm based on quaternary number Calculation method includes the following steps：
(1) posture estimation system is built, obtaining carrier fixed reference system coordinate system, (i.e. b systems, coordinate origin are located at inertia survey Measure " right-preceding-on " rectangular coordinate system at component center) under multi-axial sensor data, three axis angular rate data of gyroscope acquisition, The 3-axis acceleration data of accelerometer acquisition, three axis magnetic induction intensity data of magnetometer acquisition；
(2) filtering process is done to the acceleration information of acquisition and magnetic induction intensity data, and the two sensors are acquired Data do normalized；The data of three-axis gyroscope acquisition are w=[wx wy wz]T, wherein wx、wy、wzIt is illustrated respectively in The collected three axis angular rates data of x-axis, y-axis and z-axis in carrier fixed coordinate system；T represents transposition；Three axis after normalization add The data of speedometer acquisition are a=[ax ay az]T, wherein ax、ay、azIt is illustrated respectively in x-axis, y-axis in carrier fixed coordinate system With the collected 3-axis acceleration data of z-axis；The data of three axle magnetometer acquisition after normalization are m=[mx my mz]T, Middle mx、my、mzIt is illustrated respectively in the collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis in carrier fixed coordinate system；
(3) according to quaternion differential equation and the state equation of attitude matrix carrier construction system, and the mistake of system is obtained Journey noise variance matrix；
(4) using the observation model of quick Gauss-Newton method structure system, and the measurement noise variance square of system is obtained Battle array；
(5) Extended Kalman filter recurrence equation is established according to the system state equation of foundation and observation model；
(6) three attitude angles of carrier are resolved using the best quaternary number that each recursion obtains：Course angle is Ψ, pitch angle For θ, roll angle γ.
Preferably, in step (3), according to quaternion differential equation and the state equation of attitude matrix structure system, and Detailed process to systematic procedure noise variance matrix is as follows：
Quaternion differential equation is as follows：
In formula, qkRepresent k sampling instant quaternary number, wherein qk=[qk1 qk2 qk3 qk0]T, [qk1 qk2 qk3]TIt is four First number qkVector section, useIt represents, qk0It is scalar component；Represent qkDifferential；wkRepresent the top of k sampling instant The data of spiral shell instrument acquisition；Wherein Ω (wk) be defined as follows：
In formula, wk=[wkx wky wkz]T, wherein wkx、wky、wkzRespectively k sampling instant gyroscope is fixed in carrier and is sat Angular velocity data on the x-axis, y-axis and z-axis direction of mark system acquisition；T represents transposition；
It is according to the discrete time model that quaternion differential equation obtains：
In formula, qk+1Represent k+1 sampling instant quaternary number；ΩkIt is the Ω (w of k sampling instantk)；qkDuring for k sampling The quaternary number at quarter, q (0) are the quaternary number that primary condition obtains；TsFor the sampling period；
According to quaternary number qk=[qk1 qk2 qk3 qk0]TWith attitude matrix(n systems：" east-north-day " geographical coordinate System) relationship to obtain attitude matrix as follows：
Using attitude quaternion as the state of system, i.e.,：
Xk=qk
In formula, XkRepresent k sampling instant state matrix；qkRepresent k sampling instant quaternary number；
The state equation of the system of foundation such as following formula：
Xk+1=ΦkXk+VK
In formula, Xk+1Represent k+1 sampling instant state matrix；ΦkRepresent k sampling instant state-transition matrix；XkTable Show k sampling instant quaternary number；VKFor k sampling instant process noise battle array；
Φk=exp (ΩkTs)
In formula, ΩkΩ (w for k sampling instantk)；TsFor the sampling period；
In formula,For white Gaussian noise, covariance matrix Σk=| | ek| | I, I are 3 rank unit matrixs；ΞkRepresent k The error coefficient matrix of secondary sampling instant；
In formula,Acceleration a for k sampling instant actual measurementk=[akx aky akz]TWith the acceleration v of predictionk=[vkx vky vkz]TBetween rotation error, akx、aky、akzFor x-axis, y-axis and the z surveyed under k sampling instant carrier fixed coordinate system The collected 3-axis acceleration data of axis, vkx、vky、vkzFor x-axis, the y-axis predicted under k sampling instant carrier fixed coordinate system With the collected 3-axis acceleration data of z-axis；Magnetic induction intensity m for k sampling instant actual measurementk=[mkx mky mkz]TWith The magnetic induction intensity h of predictionk=[hkx hky hkz]TBetween rotation error, mkx、mky、mkzIt is fixed for k sampling instant carrier The collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis surveyed under coordinate system, hkx、hky、hkzIt is carried for k sampling instant The collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis predicted under body fixed coordinate system；Acceleration predicted vector vk= [vkx vky vkz]TWith magnetic induction intensity predicted vector hk=[hkx hky hkz]TRespectively ideally in navigational coordinate system Projection of the terrestrial gravitation vector sum geomagnetic field intensity vector in carrier coordinate system, while projection can be converted by posture It arrives；
Systematic procedure noise variance matrix calculation formula is as follows：
Qk=(Ts/2)2ΞkΣkΞk T。
Preferably, in step (4), using the observation model of quick Gauss-Newton method structure system, and systematic survey is obtained The detailed process of noise variance matrix is as follows：
Utilize the acceleration a of actual measurementk=[akx aky akz]TSubtract the acceleration v of predictionk=[vkx vky vkz]TAnd actual measurement Magnetic induction intensity mk=[mkx mky mkz]TSubtract the magnetic induction intensity h of predictionk=[hkx hky hkz]TObtain error function ∈ (qk), error function calculation formula is as follows：
Satisfaction is obtained using quick Gauss-Newton methodQuaternary numberOptimal solution, quick Gauss-Newton method formula is as follows：
In formula,Represent the optimal quaternary number solution of k+1 sampling instant；A step for k sampling instant is pre- Survey quaternary numerical value；λkOptimal value can be all changed into each iterative process；J (0) isCorresponding Jacobian matrix, Jacobian matrix J (k) calculation formula are as follows：
Optimal solution that quick gauss-newton method is obtained is chosen as observation, i.e.,：
It enablesIt is as follows to obtain system measurements noise variance matrix：
In formula, I3×3For 3 rank unit matrixs.
Preferably, in step (5), according to the following expansion card of the system state equation of foundation and systematic observation model foundation Kalman Filtering recurrence equation：
State one-step prediction equation calculation formula is as follows：
Xk,k-1=ΦkXk-1
In formula, Xk-1Represent k-1 sampling instant state matrix；Xk,k-1Represent k sampling instant predicted state matrix；
One-step prediction variance matrix calculation formula is as follows：
In formula,For matrix ΦkTransposition；Qk-1Systematic procedure noise variance matrix for k-1 sampling instant；Pk-1Table Show the estimation error variance matrix of k-1 sampling instant；Pk,k-1Represent the one-step prediction varivance matrix of k sampling instant；
Filtering gain matrix calculation formula is as follows：
In formula, RkRepresent k sampling instant system measurements noise variance matrix；T represents transposition；HkFor 4 rank unit matrixs, And it is k sampling instant observing matrix；KkRepresent the gain matrix of k sampling instant；
State estimation calculation formula is as follows：
In formula, ZkRepresent the observation calculated by quick gauss-newton method of k sampling instant；
Estimation error variance Matrix Computation Formulas is as follows：
Pk=[I4×4-KkHk]Pk,k-1
In formula, I4×4Represent quadravalence unit matrix；PkRepresent the estimation error variance matrix of k sampling instant.
Preferably, in step (6), three attitude angles of carrier are resolved using the best quaternary number that each recursion obtains：Boat It is Ψ to angle, pitch angle θ, roll angle is that the detailed process of γ is as follows：
It can be obtained according to the relationship between direction cosine matrix and quaternary number：
γk=arctan [- (qk1qk3-qk2qk0)/0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/0.5-qk1 2-qk3 2)]
In formula, θk、γk、ΨkCourse angle, pitch angle and the roll angle of k sampling instant are represented respectively.
Beneficial effects of the present invention are：(1) appearance of a kind of expanded Kalman filtration algorithm based on quaternary number of the invention State calculation method is a kind of quaternary number estimation technique, and systematic procedure equation is linear model, has the advantages that estimated accuracy is high, together When calculate it is uncomplicated；(2) present invention is proposed establishes systematic observation model using quick Gauss-Newton method, relative to other Extended Kalman filter method, it greatly simplifies calculation amount, while also has good effect；(3) The present invention gives The computational methods of process noise covariance matrix and measurement noise variance matrix in real process solve existing parameter and calculate not The problems such as detailed.
Description of the drawings
Fig. 1 is the method flow schematic diagram of the present invention.
Fig. 2 is the calibration gyroscope of slave AHRS equipment record, accelerometer and the magnetometer data signal that the present invention uses Figure.
Fig. 3 is the Eulerian angles schematic diagram data that the present invention is obtained by attitude algorithm.
Specific embodiment
As shown in Figure 1, a kind of attitude algorithm method of expanded Kalman filtration algorithm based on quaternary number of the present invention, packet Include following steps：
Step 1：Posture estimation system is built, obtains multi-axial sensor data under carrier fixed reference system coordinate system；
Step 2：Acceleration information and magnetic induction intensity data to acquisition do filtering process, and the two sensors are adopted The data of collection do normalized；
The data of three-axis gyroscope acquisition are w=[wx wy wz]T, the data of the three axis accelerometer acquisition after normalization For a=[ax ay az]T, the data of the three axle magnetometer acquisition after normalization are m=[mx my mz]T；
Step 3：According to quaternion differential equation and the state equation of attitude matrix carrier construction system, and obtain system Process noise covariance matrix；
Step 4：Using the observation model of quick Gauss-Newton method structure system, and obtain the measurement noise variance of system Matrix；
Step 5：Extended Kalman filter recurrence equation is established according to the system state equation of foundation and observation model；
Step 6：Three attitude angles of carrier are resolved using the best quaternary number that each recursion obtains：Course angle is Ψ, is bowed The elevation angle is θ, roll angle γ；
In above-mentioned, the state equation of system is built, and the detailed process that the process noise covariance matrix of system is obtained is as follows：
Quaternion differential equation is as follows：
In formula, qkRepresent k sampling instant quaternary number, wherein qk=[qk1 qk2 qk3 qk0]T, [qk1 qk2 qk3]TIt is four First number qkVector section, useIt represents, qk0It is scalar component；Represent qkDifferential；wkGyro for k sampling instant The data of instrument acquisition；Wherein Ω (wk) be defined as follows：
In formula, wk=[wkx wky wkz]T, wherein wkx、wky、wkzThe x-axis of respectively k times sampling instant gyroscope acquisition, y Angular velocity data on axis and z-axis direction；T represents transposition；
It is as follows according to the time discretization model that quaternion differential equation obtains：
In formula, qk+1Represent k+1 sampling instant quaternary number；ΩkIt is the Ω (w of k sampling instantk)；qkDuring for k sampling The quaternary number at quarter, q (0) are the quaternary number that primary condition obtains；TsFor the sampling period；
Pass through quaternary number qk=[qk1 qk2 qk3 qk0]TIt can obtain attitude matrixN systems (navigational coordinate system)： " east-north-day " geographic coordinate system；
Using attitude quaternion as the state of system, i.e.,：
Xk=qk
In formula, XkRepresent k sampling instant state matrix；qkRepresent k sampling instant quaternary number；
Establish the state equation such as following formula of system
Xk+1=ΦkXk+VK
In formula, Xk+1Represent k+1 sampling instant state matrix；ΦkRepresent k sampling instant state-transition matrix；XkTable Show k sampling instant quaternary number；VKFor k sampling instant process noise battle array；
Φk=exp (ΩkTs)
In formula, ΩkΩ (w for k sampling instantk)；TsFor the sampling period；
In formula,For white Gaussian noise, covariance matrix Σk=| | ek| | I, I are 3 rank unit matrixs；ΞkRepresent k The error coefficient matrix of secondary sampling instant；
In formula,Vector acceleration a for k sampling instant actual measurementk=[akx aky akz]TWith acceleration predicted vector vk =[vkx vky vkz]TBetween rotation error, akx、aky、akzFor surveyed under k sampling instant carrier fixed coordinate system x-axis, Y-axis and the collected 3-axis acceleration data of z-axis, vkx、vky、vkzFor the x predicted under k sampling instant carrier fixed coordinate system Axis, y-axis and the collected 3-axis acceleration data of z-axis；Magnetic induction intensity vector m for k sampling instant actual measurementk=[mkx mky mkz]TWith magnetic induction intensity predicted vector hk=[hkx hky hkz]TBetween rotation error, mkx、mky、mkzFor k sampling The collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis surveyed under moment carrier fixed coordinate system, hkx、hky、hkzFor k The collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis predicted under secondary sampling instant carrier fixed coordinate system；
Acceleration predicted vector vk=[vkx vky vkz]TWith magnetic induction intensity predicted vector hk=[hkx hky hkz]TRespectively For projection of the terrestrial gravitation vector sum geomagnetic field intensity vector in carrier coordinate system in ideally navigational coordinate system, throw Shadow process is converted to by posture, and calculation formula is as follows：
bkz=dkz
In formula, [d in formulakx dky dkz]TIt is output of the magnetometer in n systems；[bkx 0 bkz]TThe practical earth magnetic assumed that The size and Orientation of field；It isTransposition；
Systematic procedure noise variance matrix calculation formula is as follows：
Qk=(Ts/2)2ΞkΣkΞk T。
In above-mentioned technical proposal, step 4 obtains observation and the detailed process of the measurement noise variance matrix of system is obtained It is as follows：Utilize the acceleration a of actual measurementk=[akx aky akz]TSubtract the acceleration v of predictionk=[vkx vky vkz]TWith actual measurement Magnetic induction intensity mk=[mkx mky mkz]TSubtract the magnetic induction intensity h of predictionk=[hkx hky hkz]TObtain error function ∈ (qk), error function calculation formula is as follows：
Satisfaction is obtained using quick Gauss-Newton methodQuaternary number Optimal solution, quick Gauss-Newton method formula is as follows：
In formula,Represent the optimal quaternary number solution of k+1 sampling instant；A step for k sampling instant is pre- Survey quaternary numerical value；λkOptimal value can be all changed into each iterative process；J (0) isCorresponding Jacobian matrix, Jacobian matrix J (k) calculation formula are as follows：
The optimal solution that quick gauss-newton method is obtained is selected as observation, i.e.,
It enablesIt is as follows to obtain system measurements noise variance matrix：
In formula, I3×3For 3 rank unit matrixs；
In above-mentioned technical proposal, the detailed process of Extended Kalman filter recurrence equation that step 5 obtains is as follows：State one It is as follows to walk predictive equation calculation formula：
Xk,k-1=ΦkXk-1
In formula, Xk-1Represent k-1 sampling instant state matrix；Xk,k-1Represent k sampling instant predicted state matrix；
One-step prediction variance matrix calculation formula is as follows：
In formula,For matrix ΦkTransposition, Qk-1Systematic procedure noise variance matrix for k-1 sampling instant；Pk-1Table Show the estimation error variance matrix of k-1 sampling instant；Pk,k-1Represent the one-step prediction varivance matrix of k sampling instant；
Filtering gain matrix calculation formula is as follows：
In formula, RkRepresent k sampling instant system measurements noise variance matrix；T represents transposition；HkFor 4 rank unit matrixs, And it is k sampling instant observing matrix；KkRepresent the gain matrix of k sampling instant；
State estimation calculation formula is as follows：
In formula, ZkRepresent the observation calculated by quick gauss-newton method of k sampling instant；
Estimation error variance Matrix Computation Formulas is as follows：
Pk=[I4×4-KkHk]Pk,k-1
In formula, I4×4Represent quadravalence unit matrix；PkRepresent the estimation error variance matrix of k sampling instant；
In above-mentioned technical proposal, step 6 resolves three attitude angles of carrier using the best quaternary number that each recursion obtains： Course angle is Ψ, and pitch angle θ, roll angle is that the detailed process of γ is as follows：
It can be obtained according to the relationship between direction cosine matrix and quaternary number：
γk=arctan [- (qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/（0.5-qk1 2-qk3 2)]
In formula, θk、γk、ΨkCourse angle, pitch angle and the roll angle of k sampling instant are represented respectively.
Although the present invention is illustrated and has been described with regard to preferred embodiment, it is understood by those skilled in the art that Without departing from scope defined by the claims of the present invention, variations and modifications can be carried out to the present invention.

Claims (5)

1. a kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number, which is characterized in that including walking as follows Suddenly：
(1) posture estimation system is built, obtains multi-axial sensor data, i.e. b systems under carrier fixed reference system coordinate system, coordinate is former Positioned at " right-preceding-on " rectangular coordinate system at inertial measurement cluster center, three axis angular rate data of gyroscope acquisition accelerate point The 3-axis acceleration data of degree meter acquisition, three axis magnetic induction intensity data of magnetometer acquisition；
(2) filtering process is done to the acceleration information of acquisition and magnetic induction intensity data, and to the number of the two sensors acquisition According to doing normalized；The data of three-axis gyroscope acquisition are w=[wx wy wz]T, wherein wx、wy、wzIt is illustrated respectively in carrier The collected three axis angular rates data of x-axis, y-axis and z-axis in fixed coordinate system；T represents transposition；3-axis acceleration after normalization The data of meter acquisition are a=[ax ay az]T, wherein ax、ay、azIt is illustrated respectively in x-axis, y-axis and z-axis in carrier fixed coordinate system Collected 3-axis acceleration data；The data of three axle magnetometer acquisition after normalization are m=[mx my mz]T, wherein mx、 my、mzIt is illustrated respectively in the collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis in carrier fixed coordinate system；
(3) according to quaternion differential equation and the state equation of attitude matrix carrier construction system, and the process for obtaining system is made an uproar Sound variance matrix；
(4) using the observation model of quick Gauss-Newton method structure system, and the measurement noise variance matrix of system is obtained；
(5) Extended Kalman filter recurrence equation is established according to the system state equation of foundation and observation model；
(6) three attitude angles of carrier are resolved using the best quaternary number that each recursion obtains：Course angle is Ψ, pitch angle θ, Roll angle is γ.
2. the attitude algorithm method of the expanded Kalman filtration algorithm as described in claim 1 based on quaternary number, feature exist In in step (3), according to quaternion differential equation and the state equation of attitude matrix structure system, and obtaining systematic procedure and make an uproar The detailed process of sound variance matrix is as follows：
Quaternion differential equation is as follows：
In formula, qkRepresent k sampling instant quaternary number, wherein qk=[qk1 qk2 qk3 qk0]T, [qk1 qk2 qk3]TIt is quaternary number qk Vector section, useIt represents, qk0It is scalar component；Represent qkDifferential；wkRepresent that the gyroscope of k sampling instant is adopted The data of collection；Wherein Ω (wk) be defined as follows：
In formula, wk=[wkx wky wkx]T, wherein wkx、wky、wkzRespectively k sampling instant gyroscope is in carrier fixed coordinate system Angular velocity data on the x-axis, y-axis and z-axis direction of acquisition；T represents transposition；
It is according to the discrete time model that quaternion differential equation obtains：
In formula, qk+1Represent k+1 sampling instant quaternary number；ΩkIt is the Ω (w of k sampling instantk)；qkFor k sampling instant Quaternary number, q (0) are the quaternary number that primary condition obtains；TsFor the sampling period；
According to quaternary number qk=[qk1 qk2 qk3 qk0]TWith attitude matrix(n systems：" east-north-day " geographic coordinate system) It is as follows that relationship obtains attitude matrix：
Using attitude quaternion as the state of system, i.e.,：
Xk=qk
In formula, XkRepresent k sampling instant state matrix；qkRepresent k sampling instant quaternary number；
The state equation of the system of foundation such as following formula：
Xk+1=ΦkXk+VK
In formula, Xk+1Represent k+1 sampling instant state matrix；ΦkRepresent k sampling instant state-transition matrix；XkIt represents k times Sampling instant quaternary number；VKFor k sampling instant process noise battle array；
Φk=exp (ΩkTs)
In formula, ΩkΩ (w for k sampling instantk)；TsFor the sampling period；
In formula,For white Gaussian noise, covariance matrix Σk=| | ek| | I, I are 3 rank unit matrixs；ΞkIt represents to adopt for k times The error coefficient matrix at sample moment；
In formula,Acceleration a for k sampling instant actual measurementk=[akx aky akz]TWith the acceleration v of predictionk=[vkx vky vkz]TBetween rotation error, akx、aky、akzX-axis, y-axis and z-axis to be surveyed under k sampling instant carrier fixed coordinate system are adopted The 3-axis acceleration data collected, vkx、vky、vkzFor the x-axis, y-axis and z-axis predicted under k sampling instant carrier fixed coordinate system Collected 3-axis acceleration data；Magnetic induction intensity m for k sampling instant actual measurementk=[mkx mky mkz]TWith prediction Magnetic induction intensity hk=[hkx hky hkz]TBetween rotation error, mkx、mky、mkzFor k sampling instant carrier fixed coordinates The lower collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis surveyed of system, hkx、hky、hkzConsolidate for k sampling instant carrier The collected three axis magnetic induction intensity data of x-axis, y-axis and z-axis predicted under position fixing system；Acceleration predicted vector vk=[vkx vky vkz]TWith magnetic induction intensity predicted vector hk=[hkx hky hkz]TThe respectively ideally earth in navigational coordinate system The projection of gravitational vectors and geomagnetic field intensity vector in carrier coordinate system, while projection can be converted to by posture；
Systematic procedure noise variance matrix calculation formula is as follows：
Qk=(Ts/2)2ΞkΣkΞk T。
3. the attitude algorithm method of the expanded Kalman filtration algorithm as described in claim 1 based on quaternary number, feature exist In in step (4), using the observation model of quick Gauss-Newton method structure system, and obtaining systematic survey noise variance matrix Detailed process it is as follows：
Utilize the acceleration a of actual measurementk=[akx aky akz]TSubtract the acceleration v of predictionk=[vkx vky vkz]TWith the magnetic of actual measurement Induction mk=[mkx mky mkz]TSubtract the magnetic induction intensity h of predictionk=[hkx hky hkz]TObtain error function ∈ (qk), Error function calculation formula is as follows：
Satisfaction is obtained using quick Gauss-Newton methodQuaternary numberOptimal solution, quick Gauss-Newton method formula is as follows：
In formula,Represent the optimal quaternary number solution of k+1 sampling instant；One-step prediction four for k sampling instant First numerical value；λkOptimal value can be all changed into each iterative process；J (0) isCorresponding Jacobian matrix, Jacobi Matrix J (k) calculation formula is as follows：
Optimal solution that quick gauss-newton method is obtained is chosen as observation, i.e.,：
It enablesIt is as follows to obtain system measurements noise variance matrix：
In formula, I3×3For 3 rank unit matrixs.
4. the attitude algorithm method of the expanded Kalman filtration algorithm as described in claim 1 based on quaternary number, feature exist In in step (5), according to the following Extended Kalman filter recursion of the system state equation of foundation and systematic observation model foundation Equation：
State one-step prediction equation calculation formula is as follows：
Xk,k-1=ΦkXk-1
In formula, Xk-1Represent k-1 sampling instant state matrix；Xk,k-1Represent k sampling instant predicted state matrix；
One-step prediction variance matrix calculation formula is as follows：
In formula,For matrix ΦkTransposition；Qk-1Systematic procedure noise variance matrix for k-1 sampling instant；Pk-1Represent k- The estimation error variance matrix of 1 sampling instant；Pk,k-1Represent the one-step prediction varivance matrix of k sampling instant；
Filtering gain matrix calculation formula is as follows：
In formula, RkRepresent k sampling instant system measurements noise variance matrix；T represents transposition；HkFor 4 rank unit matrixs, and it is k Secondary sampling instant observing matrix；KkRepresent the gain matrix of k sampling instant；
State estimation calculation formula is as follows：
In formula, ZkRepresent the observation calculated by quick gauss-newton method of k sampling instant；
Estimation error variance Matrix Computation Formulas is as follows：
Pk=[I4×4-KkHk]Pk,k-1
In formula, I4×4Represent quadravalence unit matrix；PkRepresent the estimation error variance matrix of k sampling instant.
5. the attitude algorithm method of the expanded Kalman filtration algorithm as described in claim 1 based on quaternary number, feature exist In in step (6), the best quaternary number obtained using each recursion resolves three attitude angles of carrier：Course angle is Ψ, pitching Angle is θ, and roll angle is that the detailed process of γ is as follows：
It can be obtained according to the relationship between direction cosine matrix and quaternary number：
γk=arctan [- (qk1qk3-qk2qk0)/(0.5-qk1 2-qk2 2)]
Ψk=[(qk1qk2-qk3qk0)/(0.5-qk1 2-qk3 2)]
In formula, θk、γk、ΨkCourse angle, pitch angle and the roll angle of k sampling instant are represented respectively.
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 true CN108225308A (en) 2018-06-29
CN108225308B 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)

Cited By (22)

* 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
CN109883451A (en) * 2019-04-15 2019-06-14 山东建筑大学 A kind of adaptive gain complementary filter method and system for the estimation of pedestrian orientation
CN109990776A (en) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 A kind of attitude measurement method and device
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
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
CN110207647A (en) * 2019-05-08 2019-09-06 诺百爱（杭州）科技有限责任公司 A kind of armlet attitude angle calculation method based on complementary Kalman filter
CN110307842A (en) * 2019-07-08 2019-10-08 常州大学 For inertia-earth magnetism combination Quick Extended Kalman Algorithm
CN110319840A (en) * 2019-07-05 2019-10-11 东北大学秦皇岛分校 Conjugate gradient attitude algorithm method towards abnormal gait identification
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111551174A (en) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN111625768A (en) * 2020-05-19 2020-09-04 西安因诺航空科技有限公司 Handheld holder attitude estimation method based on extended Kalman filtering
CN111896007A (en) * 2020-08-12 2020-11-06 智能移动机器人(中山)研究院 Quadruped robot attitude calculation method for compensating foot-ground impact
CN112181046A (en) * 2019-07-03 2021-01-05 深圳拓邦股份有限公司 Knob gear output method and device, knob device and storage device
CN112414364A (en) * 2020-11-04 2021-02-26 国网福建省电力有限公司建设分公司 Attitude monitoring device and method for suspension holding pole
CN113114105A (en) * 2021-03-10 2021-07-13 上海工程技术大学 Dynamic measurement method for output characteristics of photovoltaic cell assembly
CN113830220A (en) * 2021-11-04 2021-12-24 江苏科技大学 Electric vehicle power-assisted control method based on information fusion
CN114053679A (en) * 2021-11-19 2022-02-18 上海复动医疗管理有限公司 Exercise training method and system
CN114815583A (en) * 2022-03-28 2022-07-29 河南科技大学 Image stabilizing method for vehicle-mounted strawberry picking robot
CN114964226A (en) * 2022-04-29 2022-08-30 燕山大学 Noise adaptive strong tracking extended Kalman filter four-rotor attitude resolving method
CN115164875A (en) * 2021-04-06 2022-10-11 上海理工大学 Attitude fusion algorithm based on Kalman filtering and explicit complementary filtering
CN115855104A (en) * 2022-11-25 2023-03-28 哈尔滨工程大学 Optimal online evaluation method for combined navigation filtering result
CN116070066A (en) * 2023-02-20 2023-05-05 北京自动化控制设备研究所 Method for calculating rolling angle of guided projectile

Citations (8)

* 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
US20170003751A1 (en) * 2015-06-30 2017-01-05 Stmicroelectronics S.R.L. Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus
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

Patent Citations (8)

* 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
US20170003751A1 (en) * 2015-06-30 2017-01-05 Stmicroelectronics S.R.L. Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus
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
XU XIANG,ETC: "A Kalman Filter for SINS Self-Alignment Based on Vector Observation", 《SENSORS》 *
徐晓苏等: "基于四元数自适应卡尔曼滤波的快速对准算法", 《中国惯性技术学报》 *
赵文晔等: "Quaternion-EKF的多源传感器联合定向算法", 《测绘科学技术学报》 *

Cited By (31)

* 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
CN109990776A (en) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 A kind of 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
CN110207647A (en) * 2019-05-08 2019-09-06 诺百爱（杭州）科技有限责任公司 A kind of armlet attitude angle calculation method based on complementary Kalman filter
WO2020253854A1 (en) * 2019-06-21 2020-12-24 台州知通科技有限公司 Mobile robot posture angle calculation method
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
CN112181046A (en) * 2019-07-03 2021-01-05 深圳拓邦股份有限公司 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
CN110307842A (en) * 2019-07-08 2019-10-08 常州大学 For inertia-earth magnetism combination Quick Extended Kalman Algorithm
CN111551174A (en) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111426318B (en) * 2020-04-22 2024-01-26 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111625768A (en) * 2020-05-19 2020-09-04 西安因诺航空科技有限公司 Handheld holder attitude estimation method based on extended Kalman filtering
CN111625768B (en) * 2020-05-19 2023-05-30 西安因诺航空科技有限公司 Hand-held cradle head posture estimation method based on extended Kalman filtering
CN111896007A (en) * 2020-08-12 2020-11-06 智能移动机器人(中山)研究院 Quadruped robot attitude calculation method for compensating foot-ground impact
CN112414364A (en) * 2020-11-04 2021-02-26 国网福建省电力有限公司建设分公司 Attitude monitoring device and method for suspension holding pole
CN113114105A (en) * 2021-03-10 2021-07-13 上海工程技术大学 Dynamic measurement method for output characteristics of photovoltaic cell assembly
CN115164875A (en) * 2021-04-06 2022-10-11 上海理工大学 Attitude fusion algorithm based on Kalman filtering and explicit complementary filtering
CN113830220B (en) * 2021-11-04 2022-09-13 浙江欧飞电动车有限公司 Electric vehicle power-assisted control method based on information fusion
CN113830220A (en) * 2021-11-04 2021-12-24 江苏科技大学 Electric vehicle power-assisted control method based on information fusion
CN114053679A (en) * 2021-11-19 2022-02-18 上海复动医疗管理有限公司 Exercise training method and system
CN114815583A (en) * 2022-03-28 2022-07-29 河南科技大学 Image stabilizing method for vehicle-mounted strawberry picking robot
CN114815583B (en) * 2022-03-28 2024-08-20 河南科技大学 Image stabilizing method of vehicle-mounted strawberry picking robot
CN114964226A (en) * 2022-04-29 2022-08-30 燕山大学 Noise adaptive strong tracking extended Kalman filter four-rotor attitude resolving method
CN114964226B (en) * 2022-04-29 2024-09-20 燕山大学 Four-rotor gesture resolving method of noise self-adaptive strong tracking extended Kalman filter
CN115855104A (en) * 2022-11-25 2023-03-28 哈尔滨工程大学 Optimal online evaluation method for combined navigation filtering result
CN116070066A (en) * 2023-02-20 2023-05-05 北京自动化控制设备研究所 Method for calculating rolling angle of guided projectile
CN116070066B (en) * 2023-02-20 2024-03-15 北京自动化控制设备研究所 Method for calculating rolling angle of guided projectile

Also Published As

Publication number Publication date
CN108225308B (en) 2021-06-25

Similar Documents

Publication Publication Date Title
CN108225308A (en) A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number
Ludwig et al. Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data
CN107655476B (en) Pedestrian high-precision foot navigation method based on multi-information fusion compensation
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
CN106225784B (en) Based on inexpensive Multi-sensor Fusion pedestrian dead reckoning method
CN102692225B (en) Attitude heading reference system for low-cost small unmanned aerial vehicle
CN104698486B (en) A kind of distribution POS data processing computer system real-time navigation methods
CN104969030B (en) Inertial device, methods and procedures
Grip et al. Nonlinear observer for GNSS-aided inertial navigation with quaternion-based attitude estimation
CN104698485B (en) Integrated navigation system and air navigation aid based on BD, GPS and MEMS
CN108458714B (en) Euler angle solving method without gravity acceleration in attitude detection system
CN109696183A (en) The scaling method and device of Inertial Measurement Unit
CN107478223A (en) A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN107014371A (en) UAV integrated navigation method and apparatus based on the adaptive interval Kalman of extension
CN102445200A (en) Microminiature personal combined navigation system as well as navigating and positioning method thereof
CN105043415A (en) Inertial system self-aligning method based on quaternion model
CN108318038A (en) A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN105953795B (en) A kind of navigation device and method for the tour of spacecraft surface
CN107063262A (en) A kind of complementary filter method resolved for UAV Attitude
CN107728182A (en) Flexible more base line measurement method and apparatus based on camera auxiliary
Wahdan et al. Three-dimensional magnetometer calibration with small space coverage for pedestrians
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
Carratù et al. Energy characterization of attitude algorithms
Ludwig Optimization of control parameter for filter algorithms for attitude and heading reference systems

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