Attitude Estimation Version 1.0

Transcription

1 Attitude Estimation Version 1. Francesco Farina May 23, 216 Contents 1 Introduction 2 2 Mathematical background Reference frames and coordinate systems Euler angles Rotation matrices Quaternions Quaternion properties Quaternions and rotations Frame transformation Attitude perturbations Quaternion time derivatives The Kalman Filter Sensors Calibration MEMS underlying models Gyroscope Characterization Accelerometer Characterization Magnetometer Characterization Attitude quaternions from data Attitude through magnetometer and accelerometer Attitude from gyroscope Bibliography 12 1

2 1 Introduction NB. This material is still a work in progress. 2 Mathematical background 2.1 Reference frames and coordinate systems In this paper two reference frames are used. The first one is the Inertial Frame (I) that is a frame in which the first principle of dynamic holds. We assume it to be centered in the earth center. The second one is a non inertial frame and it is called Body Frame (B). It is assumed to be centered at the center of mass of the considered body (the smartphone in our case) and to be aligned with the principal axis of inertia of the body. In particular, regarding the coordinate system used to label the axis of the smartphone in this work, the standard North-East-Down (NED) is used. 2.2 Euler angles Euler angles are a means of representing the spatial orientation of a coordinate system with respect to any reference frame. In particular, for us (φ, θ, ψ) represent the angles between the the (x,y,z) axis of the body frame and the ones of the inertial frame. According to Euler s theorem, the motion of a rigid body with one point fixed, can also be defined as a rotation by an angle φ around some axis (a unit vector e). So, a rotation vector is defined as This is called axis-angle formulation. Problem: Gimbal lock. 2.3 Rotation matrices a = φe. (1) A rotation matrix is a matrix whose multiplication with a vector rotates the vector while preserving its lemgth. In particular, focusing on the orthogonal group of 3 3 rotation matrices (SO(3)), if R SO(3), then the following property holds: detr = 1 (2) R 1 = R T (3) Regarding the rotation of a vector x = [x 1 x 2 x 3 ] T it is performed as x rot = Rx (4) where x rot denotes the rotated vector. The inverse operation is straightforward x = R T x rot. (5) 2

3 In this work we define the rotation matrix that encodes the attitude of a rigid body to be the matrix that if multiply a vector expressed in the body frame yields the same vector expressed in the inertial frame, i.e. x I = Rx B (6) x B = R T x I (7) where the I and B subscripts denotes that vector x is expressed in the inertial or body frame respectively. If different reference frames are use, such as A and B they will be explicitly expressed as x B = R B Ax A = (R A B) T x A (8) x A = (R B A) T x B = R A Bx B. (9) We define a coordinate rotation to be a rotation about a single coordinate axis. Rotation of an angle α about axis x, y and z are define respectively as R 1 (α) = 1 cosα sinα (1) sinα cosα cosα sinα R 2 (α) = 1 (11) sinα cosα cosα sinα R 3 (α) = sinα cosα (12) 1 The conversion between Euler angles and rotation matrices is so unswerving The inverse operation is performed as R(φ,θ,ψ) = R 1 (φ)r 2 (θ)r 3 (ψ). (13) φ θ = ψ atan2(r 2,3,R 3,3 ) arcsin(r 1,3 ) atan2(r 1,2,R 1,1 ), (14) where R i,j denotes the element in row i and column j in R. Ifwe considerthe axisangleformulationinstead, given arotationvectora = φe, the corresponding rotation matrix is given by R(a) = e [a] (15) which through the Taylor expansion leads to the Rodrigues rotation formula R(a) = I 3 3 +sinφ[e] +(1 cosφ)[e] 2. (16) where [ ] is the skew operator, i.e. [a] = a 3 a 2 a 3 a 1 (17) a 2 a 1 3

4 which produces a skew-simmetric matrix, [a] T = [a] starting from any 3- tuple a. The skew operator is very useful in order to rewrite the cross product as a matrix-vector multiplication, [a] b = a b, a,b R 3. (18) Note that, since the cross product is anti-commutative it holds that, Problem: singularity in inversions. 2.4 Quaternions [a] b = [b] a (19) Due to the absence of singularity and Gimbal lock problems, unit quaternions are the best way to represent rotation in space. Following the Hamilton formulation, we represent a quaternion q as a 4-tuple [ q q q = q 1 q], (2) where q is reffered to as the real part, and q as the vector part. Notice that many authors use a different convention for quaternions representation, for example in the JPL formulation a quaternion is defined as q = [q, q ] T. These different choices can lead to the same results presented hereafter, but with differences in details; for example the quaternion multiplication is defined in a different way. Given that the results and the properties in the following hold only with the adopted convention for quaternion representation. q 2 q Quaternion properties The sum of two (or more) quaternions is straightforwardd defined as [ ] p ±q p±q = p±q (21) and it is commutative and associative by definition. The product of two quaternions, denoted by is defined as [ ] p p q q p T q. (22) p q+q p+p q The quaternion product is associative and distributive over the sum, however it must be stressed out that the quaternion product is in general not commutative (as it can be seen by the presence of the cross product), i.e. p q q p. (23) 4

5 The product of two quaternions is bi-linear and can be expressed equivalently with two different matrix products, i.e. where, p q = [p] L q (24) p q = [q] R p (25) [ ] q T [q] L = q I q [q] [ ] q T [q] R = q I q [q] (26) (27) The norm of a quaternion is defined as q = q 2 +q2 1 +q2 2 +q2 3. (28) Given a quaterion q = [q q] T, it can be defined its conjugate q as [ ] q q, (29) q which has the following properties: [ ] q q q = q 2 q = 3 1 The inverse of a quaternion q is defined as (3) (p q) = q p. (31) q 1 = q q. (32) It must be stressed out that for unit quaternions, i.e. q = 1 it holds that q = q 1 (33) Unit quaternions are used for attitude representation since, in general, a quaternion can not only change the orientation of a vector, but its length too. In order to perform rotations only, the norm of the quaternion must remain equat to 1. Unit quaternions can always be written in the form [ ] cosθ q = (34) esinθ where e is a unit vector and θ is a scalar. This property will be used in the following for converting Euler axis-angle representation to quaterion form. Finally, we define the exponential of a quaternion q as e q = e q+q = e q e q, (35) then, using the Euler formula we get [ ] cos q e q = e q q q sin q. (36) This will be useful in the following, when performing quaternion integration. Notice since now that the exponential of a pure quaternion, i.e. q =, is a unit quaternion. 5

6 2.4.2 Quaternions and rotations If we consider the rotation vector a = φe, the corresponding unit quaternion is defined as [ cos φ ] [ ] cos a q(a) = 2 asin φ = 2 a a. (37) 2 a sin 2 Conversely, given a quaternion q φ = atan2( q,q ) (38) e = q q. (39) In order to rotate a vector u by φ around e, the double quaternion product is used, u rot q = q u q q. (4) where u rot q, i.e. denotes the rotated vector, and q is the quaternion form of vector [ x q = x] (41) It can be easily seen unit quaternions and rotation matrices act in a similar way, in fact [ ] q u q q = (42) Rx where R = e [a] is calculated by (16). Given a quaternion q, the corresponding attitude matrix A(q) is defined as A(q) = (q 2 q 2 )I q [q] +2qq T (43) which has the following useful properties Frame transformation A(p)A(q) = A(p q) (44) A( q) = A(q) (45) A(q ) = A(q) T (46) According to the Hamilton formulation, we consider the representation of a vector x B in the body frame to be the mapping of its inertial representation x I by the quaternion q, i.e. x I = q x B q = A(q)x B. (47) The inverse operation is straightforward defined as x B = q x I q = A(q) T x I. (48) 6

7 2.5 Attitude perturbations The quaternion representing the true attitude of a body q can be expressed as the composition of the estimated attitude ˆq and a perturbation of an angle around an axis δq(a), where a is defined in (1), i.e. q = ˆq δq(a) (49) If the perturbation angle is small, then from (37) the perturbation quaternion can be approximated as [ ] [ ] 1 δq(a) a (5) 2 1 a 2 2 a 2 Similarly, the true attitude matrix can be expressed as A(q) = A(ˆq)A(δq(a)) (51) and if the small angle hypothesis holds we can approximate the perturbation matrix as A(δq(a)) I 3 3 +[a] ( a 2 I 3 3 aa T ) I 3 3 +[a] (52) Quaternion time derivatives In order to develop the time derivative of a quaternion q let us consider q = q(t) as the original state and ˆq = q(t+δt) = q δq as the perturbed state, q(t+δt) q(t) q δq q q lim = lim = (53) δt δt δt δt ([ ] [ 1 1 q a/2 ]) = lim (54) δt δt [ ] q a/2 = lim (55) δt δt Then we can note that since a is a local angular perturbation, its derivative corresponds to the angular velocity vector ω, and hence q = 1 2 q ω q (56) Finally we can define the time derivative of the quaternion product as 2.6 The Kalman Filter Work in progress. (p q) = ṗ q +p q (57) 7

8 ωout ωout bω time (s) time (s) Figure 1: On the right the biased output of the gyroscope during a static phase, on the left the unbiased measurements. 3 Sensors Calibration 3.1 MEMS underlying models First of all the models of Micro Electro Mechanical Systems (MEMS) inertial sensors in the IMU are introduced. Supposing no axis misalignment, each sensor can be modeled as follow: x out = x+b x +w x, (58) ḃ x = β x +w bx. (59) where x out is the sensor s output, x is the true reading, b x is the sensor bias, β x is a parameter representing a linear drift of the bias and w x WN(,Σ x ) and w bx WN(,Σ bx ) are respectively the sensor and the bias noises. In the following we denote with ω, g and m the gyroscope, accelerometer and magnetometer readings respectively. 3.2 Gyroscope Characterization In order to characterize the gyroscope, parameters b ω, w ω, β ω and w bω must be identified. To do so, data have to be collected for a sufficiently long static period. Once the data have been collected the bias b ω and the linear bias drift (if present) β ω can be easilycomputed bylinearly interpolatingthe data. In Figure 1 is shown an example of raw and filtered gyroscope data. The measurement and bias noises can be characterized as Σ ω = var(ω out b ω ) (6) ( ) d Σ bω = var dt (ω out b ω ) (61) It must be noted that, by calculating the covariance matrices this way the one of the bias, Σ bω, is probably overestimated. A more accurate way to proceed in order to estimate the noise s variances is to use the Allan variance (see [1]). 8

9 3.3 Accelerometer Characterization The accelerometer calibration follows the same procedure as the one of the gyroscope. A more accurate characterization involves the axis misalignment too. 3.4 Magnetometer Characterization It must be noted that in general the measures of the magnetic field obtained from the magnetometer could be affected by Hard and Soft-iron interferences. In fact, while the magnetometers readings should be distributed on a sphere of radius B centered at the origin, the Hard-Iron interferences add an offset to the measurements and the Soft-Iron ones deform the sphere in an ellipsoid. Hard-iron is due to... Soft-iron is due to... Given that, let us define the magnetic and gravitational reference field in the inertial frame as G I = [ g] T, (62) M I = [Bcosδ Bsinδ] T, (63) where g is the gravity acceleration, B is the geomagnetic field and δ is the geomagnetic field inclination (note that B and δ vary over the Earth s surface). The magnetic measurements are distributed on an ellipsoid defined by (m b m ) T W(m b m ) = B 2 (64) where m are the magnetometer readings and b m and W S 3 + represent respectively the offsets (biases) due to the Hard-Iron and the deformation due to the Soft-Iron interferences. Due to the highly variability of the Soft-Iron interferences in a closed environment (for example inside a building) one can consider the Hard-Iron ones only and filter them out. In order to do it properly, the calibration must be done in an Soft-iron interferences free environment and the data must be collected while moving the body in many direction as possible in order to map the whole magnetic field. If we ignore the term W, we can rewrite (64) as (m b m ) T (m b m ) = B 2, (65) which can be easily solved a least square problem. Suppose we have n > 4 measurements; let us define m x m y m z 1 m x1 m y1 m z1 1 X =.... (66).... m xn m yn m zn 1 m 2 x +m 2 y +m 2 z m 2 x 1 +m 2 y 1 +m 2 z 1 Y =. m 2 x n +m 2 y n +m 2 z n (67) 9

10 mz mzf m y -4-2 m x m yf m xf 2 4 Figure 2: Raw magnetic field readings (in red) and filtered magnetic filed (in blue). 2b mx χ = 2b my 2b mz (68) B 2 b 2 m x b 2 m y b 2 m z. then b m and B from (65) can be computed as the solution of χ = (X T X) 1 X T Y, (69) and the filtered magnetometers reading are can be defined as m f = m b m. (7) An example of raw and filtered magnetometer readings is shown in Figure 2. It is obviously possible to estimate the matrix W in (64) too (see for example [2] or [3] for a general least square ellipsoid fitting algorithm), however it is almost useless in an indoor environment because of the highly variability of the Soft-iron disturbances. Regarding the magnetometer noise characterization, a procedure similar to the one used for the gyroscope can be performed. While it has no sense to identify the bias drift and noise for the magnetometer because of its highly variable nature. 4 Attitude quaternions from data In this section two methods to determine the attitude of a rigid body are presented. The first one involves both the magnetometer and the accelerometer and provides an instantaneous measurement of the body pose, while the second one involves the gyroscope and calculates the current body attitude by integrating the previous one through the measured angular rates. 1

11 4.1 Attitude through magnetometer and accelerometer The accelerometer and magnetometer reading in the body frame can be expressed as g = R 1 (φ)r 2 (θ)r 3 (ψ)g I (71) m f = R 1 (φ)r 2 (θ)r 3 (ψ)m I (72) where φ, θ and ψ are respectively the roll, pitch and yaw angles. The measurements from the accelerometer are used to determine φ and θ, which are used to de-rotate the magnetometer readings in order to obtain the ψ (the angle to the north). Equation (71) can be rewritten as R 2 ( θ)r 1 ( φ)g = R 3 (ψ)g I. (73) Since R 3 (ψ)g I = [ g] T, we obtain cosθ sinθ 1 1 cosφ sinφ G x G y = (74) sinθ cosθ sinφ cosφ G z g which gives: φ = arctan ( gy g z ) ( ) g x θ = arctan g y sinφ+g z cosφ Then, in a similar way we obtain from (72) ( ) m fz sinφ m fy cosφ ψ = arctan m fz cosθ +m fy sinθsinφ+m fx sinθcosφ (75) (76) (77) As a standard convention, in order to have a unique solution, θ [ π/2,π/2] and φ,ψ [ π,π]. An example of the attitude quaternion computed using magnetometer and accelerometer in a Soft-iron free environment is shown in Figure Attitude from gyroscope While using the magnetometer and the accelerometer the attitude can be computed at each time instant, using the gyroscope the attitude is obtained as integration from the initial condition. Let us denote ω = [ω x ω y ω z ] T as the angular rate of the gyroscope, the derivative of the unit quaternion q representing the attitude has been defined in (56) as q = 1 2 q ω q In order to perform a discrete time integration, many method have been proposed, however, in order to preserve the unit length of the quaternion the direct multiplication method is used. 11

12 q q time (s) time (s) Figure 3: Attitude quaternion components. On the left the one obtained using the gyroscope and on the right the one from magnetometer and accelerometer. The unit quaternion at time t+δt can be expressed using the Taylor expansion as q t+δt = q t + q t δt+ 1 2 q tδt (78) where, by using (56) and (57) q t = 1 2 q t ω qt (79) q t = 1 4 q t ωq 2 t q tω q (8) If we suppose ω to be constant over a sampling interval δt, we get ω = and then ( q t+δt = q t ω q t δt+ 1 ( ) ω q t δt +...) (81) Finally, by identifying the Taylor expansion of e ωq t δt/2 in (81) and using (36), we get q t+δt = q t q(ωδt), (82) where q(ωδt) is defined in (37) and δt represents the sampling interval. An example of the computed attitude quaternion is shown in Figure 3. References [1] N. El-Sheimy, H. Hou, and X. Niu, Analysis and modeling of inertial sensors using allan variance, Instrumentation and Measurement, IEEE Transactions on, vol. 57, no. 1, pp , 28. [2] V. Renaudin, M. H. Afzal, and G. Lachapelle, New method for magnetometers based orientation estimation, in Position Location and Navigation Symposium (PLANS), 21 IEEE/ION. IEEE, 21, pp [3] A. Fitzgibbon, M. Pilu, and R. B. Fisher, Direct least square fitting of ellipses, Pattern Analysis and Machine Intelligence, IEEE Transactions on, vol. 21, no. 5, pp ,

13 [4] F. L. Markley, Attitude error representations for kalman filtering, Journal of guidance, control, and dynamics, vol. 26, no. 2, pp ,