JP5747752B2 - Posture estimation device, posture estimation method, posture estimation program - Google Patents

Description

  The present invention relates to a posture estimation device, a posture estimation method, and a posture estimation program.

  As this type of technology, Patent Document 1 discloses a method for estimating the attitude of a tiltable object having a tilt detection unit and an angular velocity detection unit.

Special table 2003-521697 gazette

  By the way, the applicant of the present application has developed a technique for estimating the posture of a mobile robot. Specifically, the mobile robot includes a tilt sensor (for example, a three-axis acceleration sensor) that measures a posture angle, and a change in posture angle. And a gyro sensor for measuring the posture angular velocity. Then, the posture of the mobile robot is accurately estimated by combining the measurement result of the tilt sensor and the measurement result of the gyro sensor by filter processing. That is, the posture of the mobile robot is estimated by the following equation. The following formulas (1) to (3) show the filter processing for one axis by Euler angle expression.

The α ^dot _measured is a measurement result of the posture angular velocity by the gyro sensor.
α _measured is a measurement result of the posture angle by the tilt sensor.
α _predict is a predicted value of the posture angle of the mobile robot predicted using the measurement result of the posture angular velocity by the gyro sensor.
α _n is an estimated value of the posture angle of the mobile robot obtained by adding the measurement result of the posture angle by the tilt sensor to the predicted value of the posture angle of the mobile robot.
b _n is an estimated value of the attitude angular velocity bias of the gyro sensor.
b _n−1 is an estimated value of the attitude angular velocity bias of the previous gyro sensor.
dt is a sampling period.
K ₀ is a Kalman filter coefficient.
K ₁ is a Kalman filter coefficient.

  Since the above equation (2) shows the filtering process for one axis, the filtering process is repeated three times for three axes, and the calculation cost is high. On the other hand, the present applicant has succeeded in reducing the calculation cost of the above formula (2) by introducing the concept of quaternions. Details are as follows.

When the above equation (2) is modified, the following equation (4) is obtained. In the following formula (4), α _n corresponds to an angle that interpolates two angles α _predict and α _{measured at} a ratio of (1-K ₀ ): K ₀ . If α is a quaternion rather than an angle of one axis by Euler angles, the calculation of the following equation (4) can be regarded as the same form as spherical linear interpolation (slerp).

Here, the function slerp (q ₀ , q ₁ , t) for interpolating between the two quaternions of q ₀ and q ₁ with the variable t is theoretically defined by the following equation (5).

The above equation (5) can be replaced by the following equation (6) using ω which is the length of the arc between q ₀ and q ₁ .

  The trigonometric function between the two quaternions can be easily obtained by the following equation (7).

  Therefore, the following equation (8) is obtained.

  As described above, the above equation (2) is executed at low calculation cost by using spherical linear interpolation peculiar to a quaternion, assuming that α is not a posture angle of one axis by Euler angles but a quaternion. It becomes possible. The applicant of the present application has already filed a patent for an efficient calculation method for the above equation (2).

On the other hand, there was room for improvement in the calculation of the above formula (3). Specifically, unlike the above equation (2), the above equation (3) cannot be brought into the form of using the spherical linear interpolation peculiar to the quaternion. Therefore, when α _measured or the like is already a quaternion, it is necessary to perform calculation by returning to the Euler angle representation once and executing the calculation again. And, since a function that takes time such as a trigonometric function or an inverse function is frequently used for conversion between the quaternion and the Euler angle, as a result, much time is spent on the calculation of the above formula (3).

  An object of the present invention is to provide a technique for improving the efficiency of calculation for estimating a posture angular velocity bias.

According to a first aspect of the present invention, there is provided an attitude estimation device for estimating an attitude of a device body including an inclination angle sensor and a gyro sensor based on a measurement result of the inclination angle sensor and a measurement result of the gyro sensor. A first attitude angle quaternion generating means for generating a first attitude angle quaternion α _measured which is a quaternion representation of the attitude angle of the device main body based on a measurement result of the tilt angle sensor; and the gyro sensor a second attitude angle quaternion generating means for generating and predicting a second orientation angle quaternion alpha _predict on the basis of the measurement result is quaternion number representation of the attitude angle of the device body, the filter coefficient K ₁ There is provided an attitude estimation device including filter coefficient generation means for generating and attitude angular velocity bias quaternion estimation means for estimating an attitude angular velocity bias quaternion b _n by the following equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion. According to the above configuration, there is no need to convert the quaternion representation into the Euler angle representation or the Euler angle representation into the quaternion representation, and the posture angular velocity bias quaternion b _n remains in the quaternion representation. Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

Preferably, when the filter coefficient K ₁ is negative, the attitude angular velocity bias estimation unit inverts the positive / negative of the filter coefficient K ₁ and then calculates the attitude angular velocity bias quaternion b according to the equation (9). Estimate _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

Preferably, the posture angular velocity estimated bias means, said if the absolute value of the filter coefficient K ₁ is greater than 1, the absolute value of the filter coefficient K ₁ in terms of the 1, posture angular velocity bias by the formula (9) Estimate the quaternion b _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

According to a second aspect of the present invention, there is provided an attitude estimation method for estimating an attitude of a device main body including an inclination angle sensor and a gyro sensor based on a measurement result of the inclination angle sensor and a measurement result of the gyro sensor. A first posture angle quaternion generating step for generating a first posture angle quaternion α _measured which is a quaternion representation of the posture angle of the device main body based on a measurement result of the tilt angle sensor, and the gyro sensor A second attitude angle quaternion generation step that predicts and generates a second attitude angle quaternion α _predict , which is a quaternion representation of the attitude angle of the device body, based on the measurement result, and a filter coefficient K ₁ There is provided a posture estimation method including a filter coefficient generation step to be generated and a posture angular velocity bias quaternion estimation step for estimating the posture angular velocity bias quaternion b _n by the above equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion.

  Also provided is a posture estimation program for causing a computer to execute the posture estimation method described above.

According to the present invention, there is no need to convert the quaternion representation into the Euler angle representation or the Euler angle representation into the quaternion representation, and the posture angular velocity bias quaternion b _n is not changed from the quaternion representation. Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

FIG. 1 is a block diagram of an inverted motorcycle body. FIG. 2 is a block diagram of the posture estimation unit. FIG. 3 is a block diagram of the attitude angular velocity bias quaternion estimation unit. FIG. 4 is a control flow of the inverted motorcycle.

  Hereinafter, an embodiment of the present invention will be described with reference to FIGS.

  The inverted two-wheeled vehicle 1 (mobile robot, device) includes an inverted two-wheeled vehicle main body 2 (device main body), a cart, a handle, wheels, and the like (not shown).

  The inverted two-wheeled vehicle body 2 includes an inclination angle sensor 3, a gyro sensor 4, a CPU 5 (Central Processing Unit), a RAM 6 (Random Access Memory), and a ROM 7 (Read Only Memory). The ROM 7 stores an attitude estimation program and an attitude control program. The posture estimation program and the posture control program are read into the CPU 5 and executed on the CPU 5, thereby causing the hardware such as the CPU 5 to function as the posture estimation unit 8 (posture estimation device) and the posture control unit 9. It is like that.

  The posture estimation unit 8 estimates the posture of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3 and the measurement result of the gyro sensor 4. The “posture of the inverted motorcycle body 2” is a concept including “the attitude angle of the inverted motorcycle body 2” and “the attitude angular velocity of the inverted motorcycle body 2”.

  The attitude control unit 9 controls the attitude of the inverted motorcycle body 2 based on the attitude of the inverted motorcycle body 2 estimated by the attitude estimation unit 8.

  As shown in FIG. 2, the posture estimation unit 8 includes a first posture angle quaternion generation unit 20 (first posture angle quaternion generation unit) and a second posture angle quaternion generation unit 21 (second posture). Angle quaternion generation unit), filter coefficient generation unit 22 (filter coefficient generation unit), posture angle quaternion synthesis unit 23, posture angle quaternion normalization unit 24, Euler angle conversion unit 25, The posture angular velocity bias quaternion estimation unit 26 (posture angular velocity bias quaternion estimation means) and the posture angular velocity bias quaternion normalization unit 27 are configured.

  As shown in FIG. 3, the attitude angular velocity bias quaternion estimating unit 26 includes a difference calculating unit 40 (difference calculating unit), a filter calculating unit 41 (filter calculating unit), an integral calculating unit 42 (integral calculating unit), and , Is composed of.

(Inclination angle sensor 3)
Returning to FIG. 1, the tilt angle sensor 3 is for measuring the current posture angle of the inverted motorcycle body 2. In the present embodiment, the tilt angle sensor 3 is configured by a combination of a triaxial acceleration sensor and a geomagnetic sensor. The triaxial acceleration sensor can measure the roll angle and the pitch angle among the posture angles of the inverted two-wheeled vehicle body 2. The geomagnetic sensor can measure the yaw angle of the posture angle of the inverted motorcycle body 2. The tilt angle sensor 3 outputs the posture angle signal obtained by the measurement to the first posture angle quaternion generation unit 20 of the posture estimation unit 8.

(Gyro sensor 4)
The gyro sensor 4 is for measuring the current posture angular velocity of the inverted motorcycle body 2. In this embodiment, the gyro sensor 4 is configured by a MEMS gyro. The gyro sensor 4 outputs the posture angular velocity signal obtained by the measurement to the second posture angle quaternion generation unit 21 of the posture estimation unit 8.

(First posture angle quaternion generator 20)
The first posture angle quaternion generation unit 20 in FIG. 2 generates a first posture angle quaternion α _measured that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3. To do. Specifically, it is as follows.

(→ Filter processing)
First, the first posture angle quaternion generation unit 20 filters the posture angle signal received from the tilt angle sensor 3 and cuts an undesirable frequency band included in the posture angle signal.

(→ Mounting angle correction process)
Next, the first posture angle quaternion generation unit 20 corrects the posture angle signal based on the attachment angle of the three-axis acceleration sensor with respect to the inverted two-wheeled vehicle body 2. Thereby, the influence by the attachment angle of the triaxial acceleration sensor is canceled from the attitude angle signal.

(→ Self acceleration cancellation process)
Next, the first posture angle quaternion generation unit 20 corrects the posture angle signal based on the acceleration of the inverted two-wheeled vehicle body 2 obtained by differentiating the rotational speed of the wheels. Thereby, the influence by the self acceleration is canceled from the attitude angle signal.

(→ Attitude angle matrix conversion process)
Next, the first posture angle quaternion generation unit 20 converts rotation obtained by adding a yaw angle (azimuth angle) to a posture angle signal (acceleration sensor vector) into a posture angle matrix.

(→ Quaternion conversion process)
Next, the first posture angle quaternion generation unit 20 converts the posture angle matrix into a first posture angle quaternion α _measured which is a quaternion representation of the posture angle.

Then, the first posture angle quaternion generating unit 20 outputs the generated first posture angle quaternion α _measured to the posture angle quaternion synthesizing unit 23 and the posture angular velocity bias quaternion estimating unit 26.

(Second posture angle quaternion generator 21)
Second posture angle quaternion generator 21 is generated by predicting a second orientation angle quaternion alpha _predict which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the measurement result of the gyro sensor 4 Is. Specifically, it is as follows.

(→ Filter processing)
First, the second posture angle quaternion generation unit 21 filters the posture angular velocity signal received from the gyro sensor 4 and cuts an undesirable frequency band included in the posture angular velocity signal.

(→ prediction process)
Next, the second posture angle quaternion generator 21 obtains the previous posture angular velocity bias b _n−1 from the RAM ₆ and then obtains the previous posture angular velocity bias b _n−1 from the posture angular velocity signal α ^dot _measured. Subtract, multiply this by the sampling period dt, add to the previous attitude angle α _n−1 , and convert the result to a quaternion. Thereby, the second posture angle quaternion α _predict is obtained. This prediction process may be performed with a quaternion from the beginning, or may be performed with Euler angles for the time being and finally converted into a quaternion. When calculating with the Euler angle, the following equation (1) is used.

Then, the second posture angle quaternion generation unit 21 outputs the generated second posture angle quaternion α _predict to the posture angle quaternion synthesis unit 23 and the posture angular velocity bias quaternion estimation unit 26.

(Filter coefficient generator 22)
The filter coefficient generation unit 22 generates Kalman filter coefficients K ₀ and K ₁ as a kind of observer for realizing state feedback. The Kalman filter coefficient K ₀ is a coefficient for filtering posture angle measurement noise and feeding it back to the posture angle. The Kalman filter coefficient K ₁ is a coefficient for filtering posture angle measurement noise and feeding it back to the posture angular velocity bias. .

(Attitude angle quaternion synthesis unit 23)
The posture angle quaternion combining unit 23 _combines the first posture angle quaternion α _measured and the second posture angle quaternion α _predict by filter processing with the Kalman filter coefficient K ₀ , and A posture angle α _n (estimated value) is generated. It has been shown that the composition processing by the attitude angle quaternion composition unit 23 can be efficiently executed by using spherical linear interpolation as described above. If necessary, see Equation (2) expressed in Euler angles. The posture angle quaternion combining unit 23 outputs the generated current posture angle α _n (estimated value) of the inverted motorcycle body 2 to the posture angle quaternion normalizing unit 24.

(Attitude angle quaternion normalization unit 24)
The posture angle quaternion normalization unit 24 normalizes the current posture angle α _n of the inverted two-wheeled vehicle body 2 so as to avoid accumulation of errors in floating point arithmetic. The posture angle quaternion normalization unit 24 outputs the normalized posture angle α _n to the Euler angle conversion unit 25.

(Euler angle converter 25)
The Euler angle conversion unit 25 converts the posture angle α _n into Euler angle expression and outputs the converted value to the posture control unit 9.

(Attitude angular velocity bias quaternion estimation unit 26: FIG. 3)
The posture angular velocity bias quaternion estimation unit 26 estimates the angular velocity bias quaternion b _n by the following equation (9). Specifically, it is as follows.

The attitude angular velocity bias b _n is obtained by the following equation (3) as described above.

The above equation (3) cannot be transformed into a form in which spherical linear interpolation can be used like the above-described equation (2). Therefore, the attitude angular velocity bias quaternion estimation unit 26 introduces the concept of a quaternion power (power) and efficiently solves the above equation (3). More specifically, q ^t means the quaternion q to the power of t. When t = 0, q ⁰ = 1, that is, the identity quaternion (1,0,0,0). In addition, when t = 1, q ¹ = q, which behaves like a real number. It should be noted that when t <0 and t> 1, the quaternion cannot indicate a rotation of 360 degrees or more, so it is not possible to increase t to represent multiple rotations. Algebras about scalar powers such as (a ^s ) ^t = a ^st may not apply. However, in the case of this filter operation, the absolute value of K ₁ is always smaller than _{1 in} order to reduce noise by filtering, and the first attitude angle quaternion α _measured is also the second. Since the attitude angle quaternion α _predict also indicates one angle, the power multiplier can be used in the range of 0 to 1. In general, the power of the quaternion is used when the multiplier is in the range of 0 to 1.

Now, addition in Euler angle representation is a cross product (operator is x) in quaternion representation, and subtraction in Euler angle representation is a cross product with conjugate quaternion (operator is ^-1 ) in quaternion representation. It is. The multiplication of the rotation scalar corresponds to the power in the quaternion representation. Therefore, the above formula (3) can be replaced with the above formula (9). As the meaning of the above equation (9), the noise component of the posture angle quaternion α is filtered by the Kalman filter coefficient K ₁ and is used to rotate and correct the posture angular velocity bias quaternion b _n−1. It will be.

Therefore, in FIG. 3, the difference calculation unit 40 includes the first posture angle quaternion α _measured acquired from the first posture angle quaternion generation unit 20 and the second posture angle quaternion generation unit 21 acquired from the second posture angle quaternion generation unit 21. The difference between the two attitude angle quaternions α _predict is obtained, and the noise component of the attitude angle quaternion α is calculated.

Next, the filter calculation unit 41 filters the noise component of the attitude angle quaternion α calculated by the difference calculation unit 40 with the Kalman filter coefficient K ₁ . At this time, if the Kalman filter coefficient K ₁ is negative, the filter calculation unit 41 inverts the sign of the Kalman filter coefficient K ₁ and filters the noise component of the attitude angle quaternion α. That is, the following formula (10) is used instead of the above formula (9).

In addition, as a filter method, there is a type in which coefficients such as a Kalman filter change. For example, in the sequential Kalman filter, the absolute values of K ₀ and K ₁ may temporarily be larger than 1. In this case, the filter calculation unit 41 may set the absolute value of the Kalman filter coefficient K ₁ to 1 and estimate the posture angular velocity bias quaternion b _n by the above equation (9) or (10). The same applies to the Kalman filter coefficient K _0.

Then, the integral calculating section 42, the value after filtering, by integrating the previous posture angular velocity bias quaternion b _n-1, obtains the current posture angular velocity bias quaternion b _n. The integral calculation unit 42 outputs the obtained posture angular velocity bias quaternion b _n to the posture angular velocity bias quaternion normalization unit 27.

(Attitude angular velocity bias quaternion normalization unit 27)
The attitude angular velocity bias quaternion normalization unit 27 normalizes the attitude angular velocity bias quaternion b _n in order to avoid accumulation of errors in floating point arithmetic. Then, the posture angular velocity bias quaternion normalization unit 27 stores the normalized posture angular velocity bias quaternion b _n in the RAM 6.

  Next, the operation of the posture estimation unit 8 will be mainly described with reference to FIG.

(Measurement step by tilt angle sensor)
When the power of the inverted motorcycle 1 is turned on (S300), the tilt angle sensor 3 measures the current posture angle of the inverted motorcycle body 2 and outputs a posture angle signal to the first posture angle quaternion generator 20 ( S310). Next, the gyro sensor 4 measures the current posture angular velocity of the inverted motorcycle body 2 and outputs a posture angular velocity signal to the second posture angle quaternion generation unit 21 (S320). Next, the first posture angle quaternion generator 20 generates a first posture angle quaternion α that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the posture angle signal received from the tilt angle sensor 3. Generate _measured (S330). Next, the second posture angle quaternion generator 21, the second posture angle quaternion which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the attitude angular rate signal received from the gyro sensor 4 alpha _predict Is predicted and generated (S340). Next, the filter coefficient generation unit 22 generates Kalman filter coefficients K ₀ and K ₁ (S350). Note that the processes of S310 and S320 are interchangeable, and the processes of S330 to S350 may be interchanged as appropriate. Next, the posture angular velocity bias quaternion estimation unit 26 estimates the posture angular velocity bias quaternion b _n by the following equation (9) (S360). Then, the process returns to S310 to prepare for the next cycle.

  Although the preferred embodiment of the present invention has been described above, the above embodiment has the following features in short.

The posture estimation unit 8 (posture estimation device) determines the posture of the inverted two-wheeled vehicle body 2 (equipment main body) including the tilt angle sensor 3 and the gyro sensor 4 based on the measurement result of the tilt angle sensor 3 and the measurement result of the gyro sensor 4. To be estimated. The posture estimation unit 8 generates a first posture angle quaternion that generates a first posture angle quaternion α _measured that is a quaternion representation of the posture angle of the inverted motorcycle body 2 based on the measurement result of the tilt angle sensor 3. and part 20 (first posture angle quaternion generating means), predicts a second orientation angle quaternion alpha _predict which quaternion number representation of the attitude angle of the inverted two-wheel vehicle body 2 based on the measurement result of the gyro sensor 4 A second attitude angle quaternion generator 21 (second attitude angle quaternion generator) and a filter coefficient generator 22 (filter coefficient generator) for generating a Kalman filter coefficient K ₁ (filter coefficient K ₁ ). And a posture angular velocity bias quaternion estimating unit 26 (posture angular velocity bias quaternion estimating means) for estimating a posture angular velocity bias quaternion b _n by the following equation (9). However, b _n−1 is the previous attitude angular velocity bias quaternion.

According to the above configuration, to convert quaternary number representation Euler angles representing the Euler angles representation there is no need to convert the quaternion representation, the left posture angular velocity bias quaternion b _n quaternary number representation Can be estimated. Therefore, the angular velocity bias quaternion b _n can be estimated with high calculation efficiency.

Further, when the Kalman filter coefficient K ₁ is negative, the attitude angular velocity bias attitude estimation unit 8 inverts the sign of the Kalman filter coefficient K ₁ and then calculates the attitude angular velocity bias quaternion b _{n according} to the above equation (9). presume. According to the above configuration, the power of the quaternion can be calculated without any problem.

Further, the posture angular velocity bias posture estimation unit 8, if the absolute value of the Kalman filter coefficient K ₁ is greater than 1, after the absolute value of the Kalman filter coefficient K ₁ to 1, the formula (9) by the posture angular velocity bias quaternion Estimate the number b _n . According to the above configuration, the power of the quaternion can be calculated without any problem.

When the estimation of the angular velocity bias b _n is started, it is conceivable that the initial value of the angular velocity bias b _n is the identity quaternion (1,0,0,0) representing “not rotating”. In this case, it will eventually converge to a certain value depending on the state of the inverted motorcycle 1 (temperature and the bias eigenvalue of each angular velocity sensor). Preferably, instead of starting the initial value of the attitude angular velocity bias b _n from the identity quaternion when starting the inverted motorcycle 1 and starting the attitude estimation filter calculation, the value converged after the previous activation is calculated. If adopted as the initial value, the posture angular velocity bias b _n can be estimated more stably.

DESCRIPTION OF SYMBOLS 1 Inverted motorcycle 2 Inverted motorcycle main body 3 Inclination angle sensor 4 Gyro sensor 8 Attitude estimation part 9 Attitude control part 26 Attitude angular velocity bias quaternion estimation part

Claims (5)

In a device body having an inclination angle sensor and a gyro sensor, a bias quaternion estimation device that estimates an angular velocity bias quaternion that is a quaternion expression of an output value in a stationary state of the gyro sensor,
First posture angle quaternion generation means for generating a first posture angle quaternion αmeasured which is a quaternion representation of the posture angle of the device main body based on the measurement result of the tilt angle sensor;
Second attitude angle quaternion generation means for predicting and generating a second attitude angle quaternion αpredict, which is a quaternion representation of the attitude angle of the device body, based on the measurement result of the gyro sensor;
Filter coefficient generation means for generating the filter coefficient K1,
Angular velocity bias quaternion estimating means for estimating angular velocity bias quaternion bn by the following equation;
Bias quaternion estimation device.

However, bn-1 is the previous angular velocity bias quaternion. The bias quaternion estimation device according to claim 1,
The second attitude angle quaternion generating means subtracts the previous angular velocity bias from the measurement result of the gyro sensor, multiplies the subtracted result by a sampling period, and adds the result to the previous attitude angle of the device body. Generating the second attitude angle quaternion αpredict,
Bias quaternion estimation device. The posture estimation apparatus according to claim 1 or 2 ,
The angular velocity bias quaternion estimating means estimates the angular velocity bias quaternion bn according to the equation of claim 1 after inverting the sign of the filter coefficient K1 when the filter coefficient K1 is negative. ,
Bias quaternion estimation device . The posture estimation apparatus according to any one of claims 1 to 3 ,
The angular velocity bias quaternion estimating means sets the absolute value of the filter coefficient K1 to 1 when the absolute value of the filter coefficient K1 exceeds 1, and then calculates the angular velocity bias quaternion according to the equation of claim 1. estimate bn ,
Bias quaternion estimation device . In a device body having an inclination angle sensor and a gyro sensor, a bias quaternion estimation method for estimating an angular velocity bias quaternion that is a quaternion expression of an output value in a stationary state of the gyro sensor,
A first posture angle quaternion generating step for generating a first posture angle quaternion αmeasured which is a quaternion representation of the posture angle of the device main body based on the measurement result of the tilt angle sensor;
A second attitude angle quaternion generation step that predicts and generates a second attitude angle quaternion αpredict that is a quaternion representation of the attitude angle of the device body based on the measurement result of the gyro sensor;
A filter coefficient generation step for generating the filter coefficient K1,
Angular velocity bias quaternion estimation step for estimating the angular velocity bias quaternion bn by the following equation;
Including a bias quaternion estimation method.

However, bn-1 is the previous angular velocity bias quaternion.

Images