This article is a continuation of my IMU Guide, covering additional orientation kinematics topics. I will go through some theory first and then I will present a practical example with code build around an Arduino and a 6DOF IMU sensor (acc_gyro_6dof). The scope of this experiment is to create an algorithm for fusing gyroscope and accelerometer data in order to create an estimation of the device orientation in space. Such an algorithm was already presented in part 3 of my “IMU Guide” and a practical Arduino experiment with code was presented in the “Using a 5DOF IMU” article and was nicknamed “Simplified Kalman Filter”, providing a simple alternative to the well known Kalman Filter algorithm. In this article we’ll use another approach utilizing the DCM (Direction Cosine Matrix). For the reader that is unfamiliar with MEMS sensors it is recommended to read Part 1 and 2 of the IMU Guide article. Also for following the experiments presented in this text it is recommended to acquire an Arduino board and an acc_gyro_6dof sensor.

Prerequisites

No really advanced math is necessary. Find a good book on matrix operations, that’s all you might need above school math course. If you would like to refresh your knowledge below are some quick articles:
Cartesian Coordinate System – http://en.wikipedia.org/wiki/Cartesian_coordinate_system
Rotation – http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
Vector scalar product – http://en.wikipedia.org/wiki/Dot_product
Vector cross product – http://en.wikipedia.org/wiki/Cross_product
Matrix Multiplication – http://en.wikipedia.org/wiki/Matrix_multiplication
Block Matrix – http://en.wikipedia.org/wiki/Block_matrix
Transpose Matrix – http://en.wikipedia.org/wiki/Transpose
Triple Product – http://en.wikipedia.org/wiki/Triple_product

Notations

Vectors are marked in bold text - so for example “v” is a vector and “v” is a scalar (if you can’t distinguish the two there’s problem with the text formatting wherever you’re reading this).

Part 1. The DCM Matrix

Generally speaking orientation kinematics deals with calculating the relative orientation of a body relative to a global coordinate system. It is useful to attach a coordinate system to our body frame and call it Oxyz, and another one to our global frame and call it OXYZ. Both the global and the body frames have the same fixed origin O (see Fig. 1). Let’s also define i, j, k to be unity vectors co-directional with the body frame’s x, y, and z axes – in other words they are versors of Oxyz and let I, J, K be the versors of global frame OXYZ.

Figure 1

Thus, by definition, expressed in terms of global coordinates vectors I, J, K can be written as:

I^G = {1,0,0} ^T, J^G={0,1,0} ^T , K^G = {0,0,1} ^T

Note: we use {…} ^T notation to denote a column vector, in other words a column vector is a translated row vector. The orientation of vectors (row/column) will become relevant once we start multiplying them by a matrix later on in this text.

And similarly, in terms of body coordinates vectors i, j, k can be written as:

i^B = {1,0,0} ^T, j^B={0,1,0} ^T , k^B = {0,0,1} ^T

Now let’s see if we can write vectors i, j, k in terms of global coordinates. Let’s take vector i as an example and write its global coordinates:

i^G = {i_x^G , i_y^G , i_z^G} ^T

Again, by example let’s analyze the X coordinate i_x^G, it’s calculated as the length of projection of the i vector onto the global X axis.

i_x^G = |i| cos(X,i) = cos(I,i)

Where |i| is the norm (length) of the i unity vector and cos(I,i) is the cosine of the angle formed by the vectors I and i. Using the fact that |I| = 1 and |i| = 1 (they are unit vectors by definition). We can write:

i_x^G = cos(I,i) = |I||i| cos(I,i) = I.i

Where I.i. is the scalar (dot) product of vectors I and i. For the purpose of calculating scalar product I.i it doesn’t matter in which coordinate system these vectors are measured as long as they are both expressed in the same system, since a rotation does not modify the angle between vectors so: I.i = I^B.i^B = I^G.i^G = cos(I^B.i^B) = cos(I^G.i^G) , so for simplicity we’ll skip the superscript in scalar products I.i , J.j , K.k and in cosines cos(I,i), cos(J,j), cos(K,k).

Similarly we can show that:

i_y^G = J.i , i_z^G=K.i , so now we can write vector i in terms of global coordinate system as:

i^G= { I.i, J.i, K.i}^T

Furthermore, similarly it can be shown that j^G= { I.j, J.j, K.j} ^T , k^G= { I.k, J.k, K.k} ^T.

We now have a complete set of global coordinates for our body’s versors i, j, k and we can organize these values in a convenient matrix form:

(Eq. 1.1)

This matrix is called Direction Cosine Matrix for now obvious reasons – it consists of cosines of angles of all possible combinations of body and global versors.

The task of expressing the global frame versors I^G, J^G, K^G in body frame coordinates is symmetrical in nature and can be achieved by simply swapping the notations I, J, K with i, j, k, the results being:

I^B= { I.i, I.j, I.k}^T , J^B= { J.i, J.j, J.k}^T , K^B= { K.i, K.j, K.k}^T

and organized in a matrix form:

(Eq. 1.2)

It is now easy to notice that DCM^B = (DCM^G)^T or DCM^G = (DCM^B)^T , in other words the two matrices are translates of each other, we’ll use this important property later on.

Also notice that DCM^B. DCM^G = (DCM^G)^T .DCM^G = DCM^B. (DCM^B)^T = I₃ , where I₃ is the 3×3 identity matrix. In other words the DCM matrices are orthogonal.

This can be proven by simply expanding the matrix multiplication in block matrix form:

(Eq. 1.3)

To prove this we use such properties as for example: i^GT. i^G = | i^G|| i^G|cos(0) = 1 and i^GT. j^G = 0 because (i and j are orthogonal) and so forth.

The DCM matrix (also often called the rotation matrix) has a great importance in orientation kinematics since it defines the rotation of one frame relative to another. It can also be used to determine the global coordinates of an arbitrary vector if we know its coordinates in the body frame (and vice versa).

Let’s consider such a vector with body coordinates:

r^B= { r_x^B, r_y^B, r_z^B} ^T and let’s try to determine its coordinates in the global frame, by using a known rotation matrix DCM^G.

We start by doing following notation:

r^G = { r_x^G , r_y^G , r_z^G } ^T.

Now let’s tackle the first coordinate r_x^G:

r_x^G = | r^G| cos(I^G,r^G) , because r_x^G is the projection of r^G onto X axis that is co-directional with I^G.

Next let’s note that by definition a rotation is such a transformation that does not change the scale of a vector and does not change the angle between two vectors that are subject to the same rotation, so if we express some vectors in a different rotated coordinate system the norm and angle between vectors will not change:

| r^G| = | r^B| , | I^G| = | I^B| = 1 and cos(I^G,r^G) = cos(I^B,r^B), so we can use this property to write

r_x^G = | r^G| cos(I^G,r^G) = | I^B || r^B| cos(I^B,r^B) = I^B. r^B = I^B. { r_x^B, r_y^B, r_z^B} ^T , by using one the two definition of the scalar product.

Now recall that I^B= { I.i, I.j, I.k}^T and by using the other definition of scalar product:

r_x^G = I^B. r^B = { I.i, I.j, I.k}^T . { r_x^B, r_y^B, r_z^B} ^T = r_x^B I.i + r_y^B I.j + r_z^B I.k

In same fashion it can be shown that:

r_y^G = r_x^B J.i + r_y^B J.j + r_z^B J.k
r_z^G = r_x^B K.i + r_y^B K.j + r_z^B K.k

Finally let’s write this in a more compact matrix form:

(Eq. 1.4)

Thus the DCM matrix can be used to covert an arbitrary vector r^B expressed in one coordinate system B, to a rotated coordinate system G.

We can use similar logic to prove the reverse process:

(Eq. 1.5)

Or we can arrive at the same conclusion by multiplying both parts in (Eq. 1.4) by DCM^B which equals to DCM^GT, and using the property that DCM^GT.DCM^G = I₃ , see (Eq. 1.3):

DCM^B r^G = DCM^B DCM^G r^B = DCM^GT DCM^G r^B = I₃ r^B = r^B

Part 2. Angular Velocity

So far we have a way to characterize the orientation of one frame relative to another rotated frame, it is the DCM matrix and it allows us to easily convert the global and body coordinates back and forth using (Eq. 1.4) and (Eq. 1.5). In this section we’ll analyze the rotation as a function of time that will help us establish the rules of updating the DCM matrix based on a characteristic called angular velocity. Let’s consider an arbitrary rotating vector r and define it’s coordinates at time t to be r(t). Now let’s consider a small time interval dt and make the following notations: r = r (t) , r’= r (t+dt) and dr = r’ – r:

Figure 2

Let’s say that during a very small time interval dt → 0 the vector r has rotated about an axis co-directional with a unity vector u by an angle dθ and ended up in the position r’. Since u is our axis of rotation it is perpendicular to the plane in which the rotation took place (the plane formed by r and r’) so u is orthogonal to both r and r’. There are two unity vectors that are orthogonal to the plane formed by r and r’, they are shown on the picture as u and u’ since we’re still defining things we’ll choose the one that is co-directional with the cross product r x r’, following the rule of right-handed coordinate system. Thus because u is a unity vector |u| = 1 and is co-directional with r x r’ we can deduct it as follows:

u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|² sin(dθ)) (Eq. 2.1)

Since a rotation does not alter the length of a vector we used the property that| r’| = |r|.

The linear velocity of the vector r can be defined as the vector:

v = dr / dt = ( r’ – r) / dt (Eq. 2.2)

Please note that since our dt approaches 0 so does dθ → 0, hence the angle between vectors r and dr (let’s call it α) can be found from the isosceles triangle contoured by r , r’ and dr:

α = (π – dθ) / 2 and because dθ → 0 , then α → π/2

What this tells us is that r is perpendicular to dr when dt → 0 and hence r is perpendicular to v since v and dr are co-directional from (Eq. 2.2):

v ⊥ r (Eq. 2.21)

We are now ready to define the angular velocity vector. Ideally such a vector should define the rate of change of the angle θ and the axis of the rotation, so we define it as follows:

w = (dθ/dt ) u (Eq. 2.3)

Indeed the norm of the w is |w| = dθ/dt and the direction of w coincides with the axis of rotation u. Let’s expand (Eq. 2.3) and try to establish a relationship with the linear velocity v:

Using (Eq. 2.3) and (Eq. 2.1):

w = (dθ/dt ) u = (dθ/dt ) (r x r’) / (|r|² sin(dθ))

Now note that when dt → 0, so does dθ → 0 and hence for small dθ, sin(dθ) ≈ dθ , we end up with:

w = (r x r’) / (|r|² dt) (Eq. 2.4)

Now because r’ = r + dr , dr/dt = v , r x r = 0 and using the distributive property of cross product over addition:

w = (r x (r + dr)) / (|r|² dt) = (r x r + r x dr)) / (|r|² dt) = r x (dr/dt) / |r|²

And finally:

w = r x v / |r|²­ (Eq. 2.5)

This equation establishes a way to calculate angular velocity from a known linear velocity v.

We can easily prove the reverse equation that lets us deduct linear velocity from angular velocity:

v = w x r ­ (Eq. 2.6)

This can be proven simply by expanding w from (Eq. 2.5) and using vector triple product rule (a x b) x c = (a.c)b – (b.c)a. Also we’ll use the fact that v and r are perpendicular (Eq. 2.21) and thus v.r = 0

w x r = (r x v / |r|²­) x r = (r x v) x r / |r|²­ = ((r.r) v + (v.r)r) / |r|²­ = ( |r|²­ v + 0) |r|² = v

So we just proved that (Eq. 2.6) is true. Just to check (Eq. 2.6) intuitively – from Figure 2 indeed v has the direction of w x r using the right hand rule and indeed v ⊥ r and v ⊥ w because it is in the same plane with r and r’.

Part 3. Gyroscopes and angular velocity vector

A 3-axis MEMS gyroscope is a device that senses rotation about 3 axes attached to the device itself (body frame). If we adopt the device’s coordinate system (body’s frame), and analyze some vectors attached to the earth (global frame), for example vector K pointing to the zenith or vector I pointing North – then it would appear to an observer inside the device that these vector rotate about the device center. Let w_x , w_y , w_z be the outputs of a gyroscope expressed in rad/s – the measured rotation about axes x, y , z respectively. Converting from the raw output of the gyroscope to physical values is discussed for example here: http://www.starlino.com/imu_guide.html . If we query the gyroscope at regular, small time intervals dt, then what gyroscope output tells us is that during this time interval the earth rotated about gyroscope’s x axis by an angle of dθ_x = w_xdt, about y axis by an angle of dθ_y = w_ydt and about z axis by an angle of dθ_z = w_zdt. These rotations can be characterized by the angular velocity vectors: w_x = w_x i = {w_x , 0 , 0 }^T , w_y = w_y j = { 0 , w_y , 0 }^T , w_z = w_z k = { 0 , 0, w_z }^T , where i,j,k are versors of the local coordinate frame (they are co-directional with body’s axes x,y,z respectively). Each of these three rotations will cause a linear displacement which can be expressed by using (Eq. 2.6):

dr₁ = dt v₁ = dt (w_x x r) ; dr₂ = dt v₂ = dt (w_y x r) ; dr₃ = dt v₃ = dt (w_z x r) .

The combined effect of these three displacements will be:

dr = dr₁ + dr₂ + dr₃ = dt (w_x x r + w_y x r + w_z x r) = dt (w_x + w_y + w_z) x r (cross product is distributive over addition)

Thus the equivalent linear velocity resulting from these 3 transformations can be expressed as:

v = dr/dt = (w_x + w_y + w_z) x r = w x r , where we introduce w = w_x + w_y + w_z = {w_x , w_y , w_z }

Which looks exactly like (Eq. 2.6) and suggests that the combination of three small rotations about axes x,y,z characterized by angular rotation vectors w_x , w_y , w_z is equivalent to one small rotation characterized by angular rotation vector w = w_x + w_y + w_z = {w_x , w_y , w_z }. Please note that we’re stressing out that these are small rotations, since in general when you combine large rotations the order in which rotations are performed become important and you cannot simply sum them up. Our main assumption that let us go from a linear displacement to a rotation by using (Eq. 2.6) was that dt is really small, and thus the rotations dθ and linear displacement dr are small as well. In practice this means that the larger the dt interval between gyro queries the larger will be our accumulated error, we’ll deal with this error later on. Now, since w_x , w_y , w_z are the output of the gyroscope, then we arrive at the conclusion that in fact a 3 axis gyroscope measures the instantaneous angular velocity of the world rotating about the device’s center.

Part 4. DCM complimentary filter algorithm using 6DOF or 9DOF IMU sensors

In the context of this text a 6DOF device is an IMU device consisting of a 3 axis gyroscope and a 3 axis accelerometer. A 9DOF device is an IMU device of a 3 axis gyroscope, a 3 axis accelerometer and a 3 axis magnetometer. Let’s attach a global right-handed coordinate system to the Earth’s frame such that the I versor points North, K versor points to the Zenith and thus, with these two versors fixed, the J versor will be constrained to point West.

Figure 3

Also let’s consider the body coordinate system to be attached to our IMU device (acc_gyro used as an example),

Figure 4

We already established the fact that gyroscopes can measure the angular velocity vector. Let’s see how accelerometer and magnetometer measurements will fall into our model.

Accelerometers are devices that can sense gravitation. Gravitation vector is pointing towards the center of the earth and is opposite to the vector pointing to Zenith K^B. If the 3 axis accelerometer output is A = {A_x , Ay , A_z } and we assume that there are no external accelerations or we have corrected them then we can estimate that K^B = -A. (See this IMU Guide for more clarifications http://www.starlino.com/imu_guide.html).

Magnetometers are devices that are really similar to accelerometers, except that instead of gravitation they can sense the Earth’s magnetic North. Just like accelerometers they are not perfect and often need corrections and initial calibration. If the corrected 3-axis magnetometer output is M = {M_x , My , M_z }, then according to our model I^B is pointing North , thus I^B = M.

Knowing I^B and K^B allows us calculate J^B = K^B x I^B.

Thus an accelerometer and a magnetometer alone can give us the DCM matrix , expressed either as DCM^B or DCM^G

DCM^G = DCM^BT = [I^B, J^B, K^B]^T

The DCM matrix can be used to convert any vector from body’s(devices) coordinate system to the global coordinate system. Thus for example if we know that the nose of the plane has some fixed coordinates expressed in body’s coordinate system as r^B = {1,0,0}, the we can find where the device is heading in other words the coordinates of the nose in global coordinate systems using (Eq. 1.4):

r^G = DCM^G r^B

So far you’re asking yourself if an accelerometer and a magnetometer gives us the DCM matrix at any point in time, why do we need the gyroscope ? The gyroscope is actually a more precise device than the accelerometer and magnetomer are , it is used to “fine-tune” the DCM matrix returned by the accelerometer and magnetometer.

Gyroscopes have no sense of absolute orientation of the device , i.e. they don’t know where north is and where zenith is (things that we can find out using the accelerometer and magnetometer), instead if we know the orientation of the device at time t, expressed as a DCM matrix DCM(t) , we can find a more precise orientation DCM(t+dt) using the gyroscope , then the one estimated directly from the accelerometer and magnetometer direct readings which are subject to a lot of noise in form of external (non-gravitational) inertial forces (i.e. acceleration) or magnetically forces that are not caused by the earth’s magnetic field.

These facts call for an algorithm that would combine the readings from all three devices (accelerometer, magnetometer and gyroscope) in order to create our best guess or estimate regarding the device orientation in space (or space’s orientation in device’s coordinate systems), the two orientations are related since they are simply expressed using two DCM matrices that are transpose of one another (DCM^G = DCM^BT ).

We’ll now go ahead and introduce such an algorithm.

We’ll work with the DCM matrix that consists of the versors of the global (earth’s) coordinate system aligned on each row:

If we read the rows of DCM^G we get the vectors I^B, J^B, K^B. We’ll work mostly with vectors K^B (that can be directly estimated by accelerometer) and vector I^B (that can be directly estimated by the magnetometer). The vector J^B is simply calculated as J^B = K^B x I^B , since it’s orthogonal to the other two vectors (remember versors are unity vectors with same direction as coordinate axes).

Let’s say we know the zenith vector expressed in body frame coordinates at time t₀ and we note it as K^B_0. Also let’s say we measured our gyro output and we have determined that our angular velocity is w = {w_x , w_y , w_z }. Using our gyro we want to know the position of our zenith vector after a small period of time dt has passed we’ll note it as K^B_1G . And we find it using (Eq. 2.6):

K^B_1G ≈ K^B₀ + dt v = K^B₀ + dt (w_g x K^B₀) = K^B₀ + ( dθ_g x K^B₀)

Where we noted dθ_g = dt w_g. Because w_g is angular velocity as measured by the gyroscope. We’ll call dθ_g angular displacement. In other words it tells us by what small angle (given for all 3 axis in form of a vector) has the orientation of a vector K^B changed during this small period of time dt.

Obviously, another way to estimate K^B is by making another reading from accelerometer so we can get a reading that we note as K^B_1A .

In practice the values K^B_1G will be different from from K^B_1A. One was estimated using our gyroscope and the other was estimated using our accelerometer.

Now it turns out we can go the reverse way and estimate the angular velocity w_a or angular displacement dθ_{a ­}= dt w_a , from the new accelerometer reading K^B_1A­ , we’ll use (Eq. 2.5):

w­_a = K^B₀ x v_a / | K^B₀|²­

Now v_a = (K^B_1A­ - K^B₀) / dt , and is basically the linear velocity of the vector K^B₀. And | K^B₀|²­­ = 1 , since K^B₀ is a unity vector. So we can calculate:

dθ_{a ­}= dt w_a = K^B₀ x (K^B_1A­ - K^B₀)

The idea of calculating a new estimate K^B_{1 ­} that combines both K^B_1A and K^B_1G is to first estimate dθ as a weighted average of dθ_a and dθ_g :

dθ = (s_a dθ_a + s_g dθ_g) / (s_a + s_g­), we’ll discuss about the weights later on , but shortly they are determined and tuned experimentally in order to achieve a desired response rate and noise rejection.

And then K^B_{1 ­} is calculated similar to how we calculated K^B_1G:

K^B₁ ≈ K^B₀ + ( dθ x K^B₀)

Why we went all the way to calculate dθ and did not apply the weighted average formula directly to K^B_1A and K^B_1G ? Because dθ can be used to calculate the other elements of our DCM matrix in the same way:

I^B₁ ≈ I^B₀ + ( dθ x I^B₀)

J^B₁ ≈ J^B₀ + ( dθ x J^B₀)

The idea is that all three versors I^B, J^B, K^B are attached to each other and will follow the same angular displacement dθ during our small interval dt. So in a nutshell this is the algorithm that allows us to calculate the DCM₁ matrix at time t₁ from our previous estimated DCM₀ matrix at time t­­₀. It is applied recursively at regular small time intervals dt and gives us an updated DCM matrix at any point in time. The matrix will not drift too much because it is fixed to the absolute position dictated by the accelerometer and will not be too noisy from external accelerations because we also use the gyroscope data to update it.

So far we didn’t mention a word about our magnetometer. One reasons being that it is not available on all IMU units (6DOF) and we can go away without using it, but our resulting orientation will then have a drifting heading (i.e. it will not show if we’re heading north, south, west or east), or we can introduce a virtual magnetometer that is always pointing North, to introduce stability in our model. This situation is demonstrated in the accompanying source code that used a 6DOF IMU.

Now we’ll show how to integrate magnetometer readings into our algorithm. As it turns out it is really simple since magnetometer is really similar to accelerometer (they even use similar calibration algorithms), the only difference being that instead of estimating the Zenith vector K^B vector it estimates the vector pointing North I^B. Following the same logic as we did for our accelerometer we can determine the angular displacement according to the updated magnetometer reading as being:

dθ_{m ­}= dt w_m = I^B₀ x (I^B_1M­ - I^B₀)

Now let’s incorporate it into our weighted average:

dθ = (s_a dθ_a + s_g dθ_g + s_m dθ_m) / (s_a + s_g +_­ s_m)

From here we go the same path to calculate the updated DCM_1­

I^B₁ ≈ I^B₀ + ( dθ x I^B₀) , K^B₁ ≈ K^B₀ + ( dθ x K^B₀) and J^B₁ ≈ J^B₀ + ( dθ x J^B₀),

In practice we’ll calculate J^B₁ = K^B₁ x I^B_1, after correcting K^B₁ and I^B₁ to be perpendicular unity vectors again , note that all our logic is approximated and dependent on dt being small, the larger the dt the larger the error we’ll accumulate.

So if vectors I^B₀, J^B₀, K^B₀ form a valid DCM matrix , in other words they are orthogonal to each other and are unity vectors, then we can’t say the same about I^B₁, J^B₁, K^B₁ , the formulas used for calculating them does not guarantee the orthogonality or length of the vector to be preserved , however we will not get a big error if dt is small, all we need to do is to correct them after each iteration.

First let’s see how we can ensure that two vectors are orthogonal again. Let’s consider two unity vectors a and b that are “almost orthogonal” in other words the angle between these two vectors is close to 90°, but not exactly 90°. We’re looking to find a vector b’ that is orthogonal to a and that is in the same plane formed by the vectors a and b. Such a vector is easy to find as shown in Figure 5. First we find vector c = a x b that by the rules of cross product is orthogonal to both a and b and thus is perpendicular to the plane formed by a and b. Next the vector b’ = c x a is calculated as the cross product of c and a. From the definition of cross product b’ is orthogonal to a and because it is also orthogonal to c – it end up in the plane orthogonal to c , which is the plane formed by a and b. Thus b’ is the corrected vector we’re seeking that is orthogonal to a and belongs to the plane formed by a and b.

Figure 5

We can extend the equation using the triple product rule and the fact that a.a = |a| = 1:

b’ = c x a = (a x b) x a = -a (a.b) + b(a.a) = b – a (a.b) = b + d , where d = – a (a.b) (Scenario 1, a is fixed b is corrected)

You can reflect a little bit on the results … So we obtain corrected vector b’ from vector b by adding a “correction” vector d = – a (a.b). Notice that d is parallel to a. Its direction is dependent upon the angle between a and b, for example in Figure 5 a.b = cos (a,b) > 0 , because angle between a and b is less than 90°thus d has opposite direction from a and a magnitutde of cos(a,b) = sin(b,b’).

In the scenario above we considered that vector a is fixed and we found a corrected vector b’ that is orthogonal to a. We can consider the symmetric problem – we fix b and find the corrected vector a’:

a’ = a – b (b.a) = a – b (a.b) = a + e, where e = - b (a.b) (Scenario 2, b is fixed a is corrected)

Finally in the third scenario we want both vectors to move towards their corrected state, we consider them both “equally wrong”, so intuitively we apply half correction to both vectors from scenario 1 and 2:

a’ = a – b (a.b) / 2 (Scenario 3, both a and b are corrected)
b’ = b – a (a.b) / 2

Figure 6

This is an relatively easy formula to calculate on a microprocessor since we can pre-compute Err = (a.b)/2 and then use it to correct both vectors:

a’ = a – Err * b
b’ = b – Err * a

Please note that we’re not proving that a’ and b’ are orthogonal in Scenario 3, but we presented the intuitive reasoning why the angle between a’ and b’ will get closer to 90°if we apply the above corrective transformations.

Now going back to our updated DCM matrix that consists of three vectors I^B₁, J^B₁, we apply the following corrective actions before reintroducing the DCM matrix into the next loop:

Err = ( I^B₁ . J^B₁ ) / 2

I^B₁^’ = I^B₁ – Err * J^B₁
J^B₁^’ = J^B₁ – Err * I^B₁
I^B₁^’’ = Normalize[I^B₁^’]
J^B₁^’’ = Normalize[J^B₁^’]
K^B₁^’’ = I^B₁^’’ x J^B₁^’’

Where Normalize[a] = a / |a| , is the formula calculating the unit vector co-directional with a.

So finally our corrected DCM₁ matrix can be recomposed from vectors I^B₁^’’, J^B₁^’’, K^B₁^’’ that have been ortho-normalized (each vector constitutes a row of the updated and corrected DCM matrix).

We repeat the loop to find DCM_{2 ,} DCM_{3 ,} or in general DCM _n , at any time interval n.

References

1. Theory of Applied Robotics: Kinematics, Dynamics, and Control (Reza N. Jazar)

2. Linear Algebra and Its Applications (David C. Lay)

3. Fundamentals of Matrix Computations (David S. Watkins)

4. Direction Cosine Matrix IMU: Theory (W Premerlani)

Additional Notes

For the implementation of the algorithm for now see my quadcopter project in particular releases 6/7 have a nice Processing program for visual display of the DCM matrix and a model plane. The entire code is on SVN repository:

http://code.google.com/p/picquadcontroller/source/browse/?r=7#svn%2Ftrunk

The code is in imu.h file:

http://code.google.com/p/picquadcontroller/source/browse/trunk/imu.h?r=7

A PDF Version of this article is available here

DCM Tutorial – An Introduction to Orientation Kinematics by Starlino (PDF, Rev 0.1 Draft)

Please mention and link to the source when using information in this article:

http://www.starlino.com/dcm_tutorial.html

Starlino Electronics  // Spring , 2011

$26.95  STEVAL-MKI093V1 ( breakout for LYPR540AH  – 3 axis analog gyro )

http://www.mouser.com/ProductDetail/STMicroelectronics/STEVAL-MKI093V1/?qs=Mmr5WwCtLzNTGieQUi715w%3d%3d

Looks like , Mouser has all datasheet , pictures and descriptions wrong at the time of this post.

correct datasheet   http://www.st.com/stonline/products/literature/bd/17906/steval-mki093v1.pdf

$26.95 STEVAL-MKI064V1 (breakout for LSM303DLH  3-axis magnetometer + 3-axis accelerometer)

http://www.mouser.com/ProductDetail/STMicroelectronics/STEVAL-MKI064V1/?qs=Mmr5WwCtLzNMjRxMWs1ArA%3d%3d

correct datasheet http://www.st.com/stonline/products/literature/bd/17844/steval-mki064v1.pdf

Order while they are in stock :-)

The source code will be Open-Source and will be hosted on Google Code, you can get code for quad copter here:

http://code.google.com/p/picquadcontroller/source/browse/#svn/trunk

As usual I like to start with a video demo, it's basically me controlling the tilt of the quad using a RC controller:

This is a PID feedback algorithm and a "Simplified Kalman Filter" as  described in my other articles.

First of all there's a a RCGroups thread where I first introduced the project

http://www.rcgroups.com/forums/showthread.php?t=1235360

You'll find some specs there and I will put some material here  as well.

Here is Revision 0.1 of schematic (what I started with):

PicQuadControllerRev01.pdf

The major parts list is as follows:

MCU:  DSPIC33FJ128MC802  (http://www.microchip.com/wwwproducts/Devices.aspx?dDocName=en532302)

Sensors: 5DOF Acc_Gyro ( http://gadgetgangster.com/213 )  + 1Dof Pololu LIST300AL-BREAKOUT (http://www.pololu.com/catalog/product/765).

Motor Drivers:  Power N-Channel Mosfets (Many choices -Low Rds(on) and rated for the current , I used  IRLR8743 ). Schottky diodes to protect against back-EMF ( I used SS2H10-E3/52T )

Frame , motors propellers: I picked a ready platform that was on sale Dragandfly IV Frame these are actually brushed motors same ones as used in Esky helicopters.

I did some tests on the lift force potential of this frame+motors+props, and was surprise to find out it performed better than some entry-level brushless motors. The lift force of this platform is up to 2.25 lbs.  The results of lift-force experiments are compiled in this spreadheet: MotorLiftPower.pdf.

How I measured the lift force ?  I didn't have any fancy equipment so I simply fixed the quad in a vise and weighted  it it at different throttle levels – the difference in weight gave me the lift power.

For testing just one motor, I used a string attached to the weight:

I built a test rig out of wood for testing the tilt balancing algorithm as you see in the video.

Here are some close-ups of the motor drivers. You'll  see the Mosfets and the Schottky diodes, also note the extra solder added to support the high currents that will flow through those traces ( up to 5A per motor !).

Finally here is a view on the top of the board ( notice the PIC , the switching regulator  ,  Acc_Gyro 5DOF IMU and single axes Gyro breakout , from top-left to bottom right).

This is it for now hope to bring you more interesting stuff on this and other projects if time allows  !

//starlino//

The Acc_Gyro is mounted on a regular proto-shield on top of an Arduino Duemilanove board.

Parts needed to complete the project:

- Arduino Duemilanove (or similar Arduino platform)
- Acc_Gyro 5DOF IMU board
- Protoshield (optional)
- Breadboard
- Hook-up wire 22AWG

The hook-up diagram is as follows:

Acc_Gyro <—>  Arduino
5V          <—>  5V 
GND      <—>  GND
AX       <—>  AN0
AY       <—>  AN1
AZ       <—>  AN2
GX4      <—>  AN3 
GY4      <—>  AN4  

Once you have completed the hardware part, load the following sketch to your Arduino.

imu_arduino.zip

Run the project and make sure you are receiving an output on your serial terminal (you can start the terminal from your Arduino IDE).

To analyze the data I have developed a small utility called SerialChart. It is open-source so feel free to customize it for your own needs.

Here is the output from SerialChart software:

The test was performed as follows:

- first I was tilting the board slowly (marked "smooth tilting" on the screenshot)

- next I continued tilting the board, but I also started applying some vibration – by tapping the board quickly with my finger (marked "Titlting with vibration noise")

As you can see from the chart the filtered  signal (red line)  is indeed more immune to noise than the accelerometer readings alone (blue line). The filtered signal was obtained by combining the Accelerometer and Gyroscope data. Gyroscope data is important, because if you would simply average the Accelerometer data you would get a delayed signal. Given the simplicity of the code and of the algorithm I am satisfied with the results. One feature that I would like to add is compensation for the drift effect that you might encounter with some gyroscopes. However the Acc_Gyro board proved to be very stable in this respect, since it has built-in high pass filters.

If you'd like to experiment on your own, I recommended first reproducing this testing setup ,  then shift slowly towards your application needs. For example you may take the C code and port it to PIC's C18/C30 or AVR-GCC (it shouldn't be too dificult).

Below are some useful resources and their descriptions.

SerialChart executables can be downloaded from here:

SerialChart_01.zip

Once you start SerialChart application you will need to load the imu_arduino.scc configuration file for this project(included in the imu_arduino.zip) archive.

In this configuration file make sure to update the 'port' settings to Arduino's COM port. On my computer Arduino was detected on COM3, on yours it might be different.

For more information on configuration file syntax see:

http://code.google.com/p/serialchart/wiki/ConfigurationFileSyntax

You can also download and compile SerialChart from Google Code:

http://code.google.com/p/serialchart/source/browse/#svn/trunk

You will need a SVN client to checkout the code (I use RapidSVN for Windows).

SerialChart was developed using  Qt SDK from Nokia: http://qt.nokia.com/downloads

UPDATE 2010-03-18

Many people ask me what about the other 2 axis, here is the code that outputs 3 axis, including the SerialChart configuration script.

Imu_Arduino_3axis_output_2010-03-18.zip

I also removed some overhead code that Alex pointed out in the comments, this reduced the interval between samples.

In the example below I rotate the board around the X axis(blue) which is parallel to the ground.I do it by hand so X is not exactly 0, but close. The axes that change are Y(red) and Z(green).  Please note the relationship X^2+Y^2+Z^2 = 1. The dashed  cyan, magenta and lime lines are unfiltered signals coming from accelerometer alone (RwAcc).

//starlino//

This guide is intended to everyone interested in inertial MEMS (Micro-Electro-Mechanical Systems) sensors, in particular Accelerometers and Gyroscopes as well as combination IMU devices (Inertial Measurement Unit).

Example IMU unit:  Acc_Gyro_6DOF on top of MCU processing unit UsbThumb providing USB/Serial connectivity

I'll try try to cover few basic but important topics in this article:

- what does an accelerometer measure
- what does a gyroscope (aka gyro) measure
- how to convert analog-to-digital (ADC) readings that you get from these sensor to physical units (those would be g for accelerometer, deg/s for gyroscope)
- how to combine accelerometer and gyroscope readings in order to obtain accurate information about the inclination of your device relative to the ground plane

Throughout the article I will try to keep the math to the minimum. If you know what Sine/Cosine/Tangent are then you should be able to understand and use these ideas in your project no matter what platform you're using Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC, etc. There are people out there who believe that you need complex math in order to make use of an IMU unit (complex FIR or IIR filters such as Kalman filters, Parks-McClellan filters, etc). You can research all those and achieve wonderful but complex results. My way of explaining things require just basic math. I am a great believer in simplicity. I think a system that is simple is easier to control and monitor, besides many embedded devices do not have the power and resources to implement complex algorithms requiring matrix calculations.

I'll use as an example a new IMU unit that I designed – the Acc_Gyro Accelerometer + Gyro IMU. We'll use parameters of this device in our examples below. This unit is a good device to start with because it consists of 3 devices:

- LIS331AL (datasheet) – analog 3-axis 2G accelerometer
- LPR550AL (datasheet) – a dual-axis (Pitch and Roll), 500deg/second gyroscope
- LY550ALH (datasheet) – a single axis (Yaw) gyroscope (this last device is not used in this tutorial but it becomes relevant when you move on to DCM Matrix implementation)

Together they represent a 6-Degrees of Freedom Inertial Measurement Unit. Now that's a fancy name! Nevertheless, behind the fancy name is a very useful combination device that we'll cover and explain in detail below.

Part 1. Accelerometer

To understand this unit we'll start with the accelerometer. When thinking about accelerometers it is often useful to image a box in shape of a cube with a ball inside it. You may imagine something else like a cookie or a donut , but I'll imagine a ball:

If we take this box in a place with no gravitation fields or for that matter with no other fields that might affect the ball's position – the ball will simply float in the middle of the box. You can imagine the box is in outer-space far-far away from any cosmic bodies, or if such a place is hard to find imagine at least a space craft orbiting around the planet where everything is in weightless state . From the picture above you can see that we assign to each axis a pair of walls (we removed the wall Y+ so we can look inside the box). Imagine that each wall is pressure sensitive. If we move suddenly the box to the left (we accelerate it with acceleration 1g = 9.8m/s^2), the ball will hit the wall X-. We then measure the pressure force that the ball applies to the wall and output a value of -1g on the X axis.

Please note that the accelerometer will actually detect a force that is directed in the opposite direction from the acceleration vector. This force is often called Inertial Force or Fictitious Force . One thing you should learn from this is that an accelerometer measures acceleration indirectly through a force that is applied to one of it's walls (according to our model, it might be a spring or something else in real life accelerometers). This force can be caused by the acceleration , but as we'll see in the next example it is not always caused by acceleration.

If we take our model and put it on Earth the ball will fall on the Z- wall and will apply a force of 1g on the bottom wall, as shown in the picture below:

In this case the box isn't moving but we still get a reading of -1g on the Z axis. The pressure that the ball has applied on the wall was caused by a gravitation force. In theory it could be a different type of force – for example, if you imagine that our ball is metallic, placing a magnet next to the box could move the ball so it hits another wall. This was said just to prove that in essence accelerometer measures force not acceleration. It just happens that acceleration causes an inertial force that is captured by the force detection mechanism of the accelerometer.

While this model is not exactly how a MEMS sensor is constructed it is often useful in solving accelerometer related problems. There are actually similar sensors that have metallic balls inside, they are called tilt switches, however they are more primitive and usually they can only tell if the device is inclined within some range or not, not the extent of inclination.

So far we have analyzed the accelerometer output on a single axis and this is all you'll get with a single axis accelerometers. The real value of triaxial accelerometers comes from the fact that they can detect inertial forces on all three axes. Let's go back to our box model, and let's rotate the box 45 degrees to the right. The ball will touch 2 walls now: Z- and X- as shown in the picture below:

The values of 0.71 are not arbitrary, they are actually an approximation for SQRT(1/2). This will become more clear as we introduce our next model for the accelerometer.

In the previous model we have fixed the gravitation force and rotated our imaginary box. In last 2 examples we have analyzed the output in 2 different box positions, while the force vector remained constant. While this was useful in understanding how the accelerometer interacts with outside forces, it is more practical to perform calculations if we fix the coordinate system to the axes of the accelerometer and imagine that the force vector rotates around us.

Please have a look at the model above, I preserved the colors of the axes so you can make a mental transition from the previous model to the new one. Just imagine that each axis in the new model is perpendicular to the respective faces of the box in the previous model. The vector R is the force vector that the accelerometer is measuring (it could be either the gravitation force or the inertial force from the examples above or a combination of both). Rx, Ry, Rz are projection of the R vector on the X,Y,Z axes. Please notice the following relation:

R^2 = Rx^2 + Ry^2 + Rz^2     (Eq. 1)

which is basically the equivalent of the Pythagorean theorem in 3D.

Remember that a little bit earlier I told you that the values of SQRT(1/2) ~ 0.71 are not random. If you plug them in the formula above, after recalling that our gravitation force was 1 g we can verify that:

1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2

simply by substituting R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2) in Eq.1

After a long preamble of theory we're getting closer to real life accelerometers. The values Rx, Ry, Rz are actually linearly related to the values that your real-life accelerometer will output and that you can use for performing various calculations.

Before we get there let's talk a little about the way accelerometers will deliver this information to us. Most accelerometers will fall in two categories: digital and analog. Digital accelerometers will give you information using a serial protocol like I2C , SPI or USART, while analog accelerometers will output a voltage level within a predefined range that you have to convert to a digital value using an ADC (analog to digital converter) module. I will not go into much detail about how ADC works, partly because it is such an extensive topic and partly because it is different from one platform to another. Some microcontroller will have a built-in ADC modules some of them will need external components in order to perform the ADC conversions. No matter what type of ADC module you use you'll end up with a value in a certain range. For example a 10-bit ADC module will output a value in the range of 0..1023, note that 1023 = 2^10 -1. A 12-bit ADC module will output a value in the range of 0..4095, note that 4095 = 2^12-1.

Let's move on by considering a simple example, suppose our 10bit ADC module gave us the following values for the three accelerometer channels (axes):

AdcRx = 586
AdcRy = 630
AdcRz = 561

Each ADC module will have a reference voltage, let's assume in our example it is 3.3V. To convert a 10bit adc value to voltage we use the following formula:

VoltsRx = AdcRx * Vref / 1023

A quick note here: that for 8bit ADC the last divider would be 255 = 2 ^ 8 -1 , and for 12bit ADC last divider would be 4095 = 2^12 -1.

Applying this formula to all 3 channels we get:

VoltsRx = 586 * 3.3V / 1023 =~ 1.89V (we round all results to 2 decimal points)
VoltsRy = 630 * 3.3V / 1023 =~ 2.03V
VoltsRz = 561 * 3.3V / 1023 =~ 1.81V

Each accelerometer has a zero-g voltage level, you can find it in specs, this is the voltage that corresponds to 0g. To get a signed voltage value we need to calculate the shift from this level. Let's say our 0g voltage level is VzeroG = 1.65V. We calculate the voltage shifts from zero-g voltage as follows::

DeltaVoltsRx = 1.89V – 1.65V = 0.24V
DeltaVoltsRy = 2.03V – 1.65V = 0.38V
DeltaVoltsRz = 1.81V – 1.65V = 0.16V

We now have our accelerometer readings in Volts , it's still not in g (9.8 m/s^2), to do the final conversion we apply the accelerometer sensitivity, usually expressed in mV/g. Lets say our Sensitivity = 478.5mV/g = 0.4785V/g. Sensitivity values can be found in accelerometer specifications. To get the final force values expressed in g we use the following formula:

Rx = DeltaVoltsRx / Sensitivity

Rx = 0.24V / 0.4785V/g =~ 0.5g
Ry = 0.38V / 0.4785V/g =~ 0.79g
Rz = 0.16V / 0.4785V/g =~ 0.33g

We could of course combine all steps in one formula, but I went through all the steps to make it clear how you go from ADC readings to a force vector component expressed in g.

Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity (Eq.2)
Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

We now have all 3 components that define our inertial force vector, if the device is not subject to other forces other than gravitation, we can assume this is the direction of our gravitation force vector. If you want to calculate inclination of device relative to the ground you can calculate the angle between this vector and Z axis. If you are also interested in per-axis direction of inclination you can split this result into 2 components: inclination on the X and Y axis that can be calculated as the angle between gravitation vector and X / Y axes. Calculating these angles is more simple than you might think, now that we have calculated the values for Rx,Ry and Rz. Let's go back to our last accelerometer model and do some additional notations:

The angles that we are interested in are the angles between X,Y,Z axes and the force vector R. We'll define these angles as Axr, Ayr, Azr. You can notice from the right-angle triangle formed by R and Rx that:

cos(Axr) = Rx / R , and similarly :
cos(Ayr) = Ry / R
cos(Azr) = Rz / R

We can deduct from Eq.1 that R = SQRT( Rx^2 + Ry^2 + Rz^2).

We can find now our angles by using arccos() function (the inverse cos() function ):

Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)

We've gone a long way to explain the accelerometer model, just to come up to these formulas. Depending on your applications you might want to use any intermediate formulas that we have derived. We'll also introduce the gyroscope model soon, and we'll see how accelerometer and gyroscope data can be combined to provide even more accurate inclination estimations.

But before we do that let's do some more useful notations:

cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R

This triplet is often called Direction Cosine , and it basically represents the unit vector (vector with length 1) that has same direction as our R vector. You can easily verify that:

SQRT(cosX^2 + cosY^2 + cosZ^2) = 1

This is a nice property since it absolve us from monitoring the modulus(length) of R vector. Often times if we're just interested in direction of our inertial vector, it makes sense to normalize it's modulus in order to simplify other calculations.

Part 2. Gyroscope

We're not going to introduce any equivalent box model for the gyroscope like we did for accelerometer, instead we're going to jump straight to the second accelerometer model and we'll show what does the gyroscope measure according to this model.

Each gyroscope channel measures the rotation around one of the axes. For instance a 2-axes gyroscope will measure the rotation around (or some may say "about") the X and Y axes. To express this rotation in numbers let's do some notations. First let's define:

Rxz – is the projection of the inertial force vector R on the XZ plane
Ryz – is the projection of the inertial force vector R on the YZ plane

From the right-angle triangle formed by Rxz and Rz, using Pythagorean theorem we get:

Rxz^2 = Rx^2 + Rz^2 , and similarly:
Ryz^2 = Ry^2 + Rz^2

also note that:

R^2 = Rxz^2 + Ry^2 , this can be derived from Eq.1 and above equations, or it can be derived from right-angle triangle formed by R and Ryz
R^2 = Ryz^2 + Rx^2

We're not going to use these formulas in this article but it is useful to note the relation between all the values in our model.

Instead we're going to define the angle between the Z axis and Rxz, Ryz vectors as follows:

Axz – is the angle between the Rxz (projection of R on XZ plane) and Z axis
Ayz – is the angle between the Ryz (projection of R on YZ plane) and Z axis

Now we're getting closer to what the gyroscope measures. Gyroscope measures the rate of changes of the angles defined above. In other words it will output a value that is linearly related to the rate of change of these angles. To explain this let's assume that we have measured the rotation angle around axis Y (that would be Axz angle) at time t0, and we define it as Axz0, next we measured this angle at a later time t1 and it was Axz1. The rate of change will be calculated as follows:

RateAxz = (Axz1 – Axz0) / (t1 – t0).

If we express Axz in degrees, and time in seconds , then this value will be expressed in deg/s . This is what a gyroscope measures.

In practice a gyroscope(unless it is a special digital gyroscope) will rarely give you a value expressed in deg/s. Same as for accelerometer you'll get an ADC value that you'll need to convert to deg/s using a formula similar to Eq. 2 that we have defined for accelerometer. Let's introduce the ADC to deg/s conversion formula for gyroscope (we assume we're using a 10bit ADC module , for 8bit ADC replace 1023 with 255, for 12bit ADC replace 1023 with 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity Eq.3
RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

AdcGyroXZ, AdcGyroYZ – are obtained from our adc module and they represent the channels that measure the rotation of projection of R vector in XZ respectively in YZ planes, which is the equivalent to saying rotation was done around Y and X axes respectively.

Vref – is the ADC reference voltage we'll use 3.3V in the example below

VzeroRate – is the zero-rate voltage, in other words the voltage that the gyroscope outputs when it is not subject to any rotation, for the Acc_Gyro board it is for example 1.23V (you can find this values in the specs – but don't trust the specs most gyros will suffer slight offset after being soldered so measure VzeroRate for each axis output using a voltmeter, usually this value will not change over time once the gyro was soldered, if it variates – write a calibration routine to measure it before device start-up, user must be instructed to keep device in still position  upon start-up for gyros to calibrate).

Sensitivity – is the sensitivity of your gyroscope it is expressed in mV / (deg / s) often written as mV/deg/s , it basically tells you how many mV will the gyroscope output increase , if you increase the rotation speed by one deg/s. The sensitivity of Acc_Gyro board is for example 2mV/deg/s or 0.002V/deg/s

Let's take an example, suppose our ADC module returned following values:

AdcGyroXZ = 571
AdcGyroXZ = 323

Using the above formula, and using the specs parameters of Acc_Gyro board we'll get:

RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s

In other words the device rotates around the Y axis (or we can say it rotates in XZ plane) with a speed of 306 deg/s and around the X axis (or we can say it rotates in YZ plane) with a speed of -94 deg/s. Please note that the negative sign means that the device rotates in the opposite direction from the conventional positive direction. By convention one direction of rotation is positive. A good gyroscope specification sheet will show you which direction is positive, otherwise you'll have to find it by experimenting with the device and noting which direction of rotation results in increasing voltage on the output pin. This is best done using an oscilloscope since as soon as you stop the rotation the voltage will drop back to the zero-rate level. If you're using a multimeter you'd have to maintain a constant rotation rate for at least few seconds and note the voltage during this rotation, then compare it with the zero-rate voltage. If it is greater than the zero-rate voltage it means that direction of rotation is positive.

Part 3. Putting it all together. Combining accelerometer and gyroscope data.

If you're reading this article you probably acquired or are planning to acquire a IMU device, or probably you're planning to build one from separate accelerometer and gyroscope devices.

NOTE: FOR PRACTICAL IMPLEMENTATION AND TESTING  OF THIS ALGORITHM PLEASE READ THIS ARTICLE:

http://starlino.com/imu_kalman_arduino.html

The first step in using a combination IMU device that combines an accelerometer and a gyroscope is to align their coordinate systems. The easiest way to do it is to choose the coordinate system of accelerometer as your reference coordinate system. Most accelerometer data sheets will display the direction of X,Y,Z axes relative to the image of the physical chip or device. For example here are the directions of X,Y,Z axes as shown in specifications for the Acc_Gyro board:

Next steps are:

- identify the gyroscope outputs that correspond to RateAxz , RateAyz values discussed above.
- determine if these outputs need to be inverted due to physical position of gyroscope relative to the accelerometer

Do not assume that if a gyroscope has an output marked X or Y, it will correspond to any axis in the accelerometer coordinate system, even if this output is part of an IMU unit. The best way is to test it.

Here is a sample sequence to determine which output of gyroscope corresponds to RateAxz value discussed above.

- start from placing the device in horizontal position. Both X and Y outputs of accelerometer would output the zero-g voltage (for example for Acc_Gyro board this is 1.65V)
- next start rotating the device around the Y axis, another way to say it is that you rotate the device in XZ plane, so that X and Z accelerometer outputs change and Y output remains constant.
- while rotating the device at a constant speed note which gyroscope output changes, the other gyroscope outputs should remain constant
- the gyroscope output that changed during the rotation around Y axis (rotation in XZ plane) will provide the input value for AdcGyroXZ, from which we calculate RateAxz
- the final step is to ensure the rotation direction corresponds to our model, in some cases you may have to invert the RateAxz value due to physical position of gyroscope relative to the accelerometer
- perform again the above test, rotating the device around the Y axis, this time monitor the X output of accelerometer (AdcRx in our model). If AdcRx grows (the first 90 degrees of rotation from horizontal position), then AdcGyroXZ should decrease. This is due to the fact that we are monitoring the gravitation vector and when device rotates in one direction the vector will rotate in oposite direction (relative to the device coordonate system, which we are using). So, otherwise you need to invert RateAxz , you can achieve this by introducing a sign factor in Eq.3, as follows:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity , where InvertAxz is 1 or -1

same test can be done for RateAyz , by rotating the device around the X axis, and you can identify which gyroscope output corresponds to RateAyz, and if it needs to be inverted. Once you have the value for InvertAyz, you should use the following formula to calculate RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

If you would do these tests on Acc_Gyro board you would get following results:

- the output pin for RateAxz is GX4 and InvertAxz = 1
- the output pin for RateAyz is GY4 and InvertAyz = 1

From this point on we'll consider that you have setup your IMU in such a way that you can calculate correct values for Axr, Ayr, Azr (as defined Part 1. Accelerometer) and RateAxz, RateAyz (as defined in Part 2. Gyroscope). Next we'll analyze the relations between these values that turn out useful in obtaining more accurate estimation of the inclination of the device relative to the ground plane.

You might be asking yourself by this point, if accelerometer model already gave us inclination angles of Axr,Ayr,Azr why would we want to bother with the gyroscope data ? The answer is simple: accelerometer data can't always be trusted 100%. There are several reason, remember that accelerometer measures inertial force, such a force can be caused by gravitation (and ideally only by gravitation), but it might also be caused by acceleration (movement) of the device. As a result even if accelerometer is in a relatively stable state, it is still very sensitive to vibration and mechanical noise in general. This is the main reason why most IMU systems use a gyroscope to smooth out any accelerometer errors. But how is this done ? And is the gyroscope free from noise ?

The gyroscope is not free from noise however because it measures rotation it is less sensitive to linear mechanical movements, the type of noise that accelerometer suffers from, however gyroscopes have other types of problems like for example drift (not coming back to zero-rate value when rotation stops). Nevertheless by averaging data that comes from accelerometer and gyroscope we can obtain a relatively better estimate of current device inclination than we would obtain by using the accelerometer data alone.

In the next steps I will introduce an algorithm that was inspired by some ideas used in Kalman filter, however it is by far more simple and easier to implement on embedded devices. Before that let's see first what we want our algorithm to calculate. Well , it is the direction of gravitation force vector R = [Rx,Ry,Rz] from which we can derive other values like Axr,Ayr,Azr or cosX,cosY,cosZ that will give us an idea about the inclination of our device relative to the ground plane, we discuss the relation between these values in Part 1. One might say – don't we already have these values Rx, Ry , Rz from Eq.2 in Part 1 ? Well yes, but remember that these values are derived from accelerometer data only, so if you would be to use them directly in your application you might get more noise than your application can tolerate. To avoid further confusion let's re-define the accelerometer measurements as follows:

Racc – is the inertial force vector as measured by accelerometer, that consists of following components (projections on X,Y,Z axes):

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

So far we have a set of measured values that we can obtain purely from accelerometer ADC values. We'll call this set of data a "vector" and we'll use the following notation.

Racc = [RxAcc,RyAcc,RzAcc]

Because these components of Racc can be obtained from accelerometer data , we can consider it an input to our algorithm.

Please note that because Racc measures the gravitation force you'll be correct if you assume that the length of this vector defined as follows is equal or close to 1g.

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),

However to be sure it makes sense to update this vector as follows:

Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].

This will ensure the length of your normalized Racc vector is always 1.

Next we'll introduce a new vector and we'll call it

Rest = [RxEst,RyEst,RzEst]

This will be the output of our algorithm , these are corrected values based on gyroscope data and based on past estimated data.

Here is what our algorithm will do:
- accelerometer tells us: "You are now at position Racc"
- we say "Thank you, but let me check",
- then correct this information with gyroscope data as well as with past Rest data and we output a new estimated vector Rest.
- we consider Rest to be our "best bet" as to the current position of the device.

Let's see how we can make it work.

We'll start our sequence by trusting our accelerometer and assigning:

Rest(0) = Racc(0)

By the way remember Rest and Racc are vectors , so the above equation is just a simple way to write 3 sets of equations, and avoid repetition:

RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)

Next we'll do regular measurements at equal time intervals of T seconds, and we'll obtain new measurements that we'll define as Racc(1), Racc(2) , Racc(3) and so on. We'll also issue new estimates at each time intervals Rest(1), Rest(2), Rest(3) and so on.

Suppose we're at step n. We have two known sets of values that we'd like to use:

Rest(n-1) – our previous estimate, with Rest(0) = Racc(0)
Racc(n) – our current accelerometer measurement

Before we can calculate Rest(n) , let's introduce a new measured value, that we can obtain from our gyroscope and a previous estimate.

We'll call it Rgyro , and it is also a vector consisting of 3 components:

Rgyro = [RxGyro,RyGyro,RzGyro]

We'll calculate this vector one component at a time. We'll start with RxGyro.

Let's start by observing the following relation in our gyroscope model, from the right-angle triangle formed by Rz and Rxz we can derive that:

tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)

Atan2 might be a function you never used before, it is similar to atan, except it returns values in range of (-PI,PI) as opposed to (-PI/2,PI/2) as returned by atan, and it takes 2 arguments instead of one. It allows us to convert the two values of Rx,Rz to angles in the full range of 360 degrees (-PI to PI). You can read more about atan2 here.

So knowing RxEst(n-1) , and RzEst(n-1) we can find:

Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).

Remember that gyroscope measures the rate of change of the Axz angle. So we can estimate the new angle Axz(n) as follows:

Axz(n) = Axz(n-1) + RateAxz(n) * T

Remember that RateAxz can be obtained from our gyroscope ADC readings. A more precise formula can use an average rotation rate calculated as follows:

RateAxzAvg = ( RateAxz(n) + RateAxz(n-1) ) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T

The same way we can find:

Ayz(n) = Ayz(n-1) + RateAyz(n) * T

Ok so now we have Axz(n) and Ayz(n). Where do we go from here to deduct RxGyro/RyGyro ? From Eq. 1 we can write the length of vector Rgyro as follows:

|Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

Also because we normalized our Racc vector, we may assume that it's length is 1 and it hasn't changed after the rotation, so it is relatively safe to write:

|Rgyro| = 1

Let's adopt a temporary shorter notation for the calculations below:

x =RxGyro , y=RyGyro, z=RzGyro

Using the relations above we can write:

x = x / 1 = x / SQRT(x^2+y^2+z^2)

Let's divide numerator and denominator of fraction by SQRT(x^2 + z^2)

x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )

Note that x / SQRT(x^2 + z^2) = sin(Axz), so:

x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )

Now multiply numerator and denominator of fraction inside SQRT by z^2

x = sin(Axz) / SQRT (1 + y^2  * z ^2 / (z^2 * (x^2 + z^2)) )

Note that z / SQRT(x^2 + z^2) = cos(Axz) and y / z = tan(Ayz), so finally:

x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )

Going back to our notation we get:

RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )

same way we find that

RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )

Side Note: it is possible to further simplify this formula. By dividing both parts of the fraction by sin(Axz(n)) you get:
RxGyro =  1  / SQRT (1/ sin(Axz(n))^2  + cos(Axz(n))^2 / sin(Axz(n))^2  * tan(Ayz(n))^2 )
RxGyro =  1  / SQRT (1/ sin(Axz(n))^2  + cot(Axz(n))^2  * sin(Ayz(n))^2  / cos(Ayz(n))^2 ) 
now add and substract     cos(Axz(n))^2/sin(Axz(n))^2   = cot(Axz(n))^2
RxGyro =  1  / SQRT (1/ sin(Axz(n))^2  -  cos(Axz(n))^2/sin(Axz(n))^2   + cot(Axz(n))^2  * sin(Ayz(n))^2  / cos(Ayz(n))^2  + cot(Axz(n))^2 )
and by grouping  terms 1&2 and then 3&4  we get 
RxGyro =  1  / SQRT (1  +   cot(Axz(n))^2 * sec(Ayz(n))^2 ),      where  cot(x) = 1 / tan(x)  and  sec(x) = 1 / cos(x)
This formula uses only 2 trigonometric functions and can be computationally less expensive. If you have Mathematica program you can verify it
by evaluating    FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2  * Tan[B]^2)]

Now, finally we can find:

RzGyro  =  Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).

Where Sign(RzGyro) = 1 when RzGyro>=0 , and Sign(RzGyro) = -1 when RzGyro<0.

One simple way to estimate this is to take:

Sign(RzGyro) = Sign(RzEst(n-1))

In practice be careful when RzEst(n-1) is close to 0. You may skip the gyro phase altogether in this case and assign:  Rgyro = Rest(n-1). Rz is used as a reference for calculating Axz and Ayz angles and when it's close to 0, values may overflow and trigger bad results. You'll be in domain of large floating point numbers where tan() / atan() function implementations may lack precision.

So let's recap what we have so far, we are at step n of our algorithm and we have calculated the following values:

Racc – current readings from our accelerometer
Rgyro – obtained from Rest(n-1) and current gyroscope readings

Which values do we use to calculate the updated estimate Rest(n) ? You probably guessed that we'll use both. We'll use a weighted average, so that:

Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)

We can simplify this formula by dividing both numerator and denominator of the fraction by w1.

Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)

and after substituting w2/w1 = wGyro we get:

Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)

In the above formula wGyro tells us how much we trust our gyro compared to our accelerometer. This value can be chosen experimentally usually values between 5..20 will trigger good results.

The main difference of this algorithm from Kalman filter is that this weight is relatively fixed , whereas in Kalman filter the weights are permanently updated based on the measured noise of the accelerometer readings. Kalman filter is focused at giving you "the best" theoretical results, whereas this algorithm can give you results "good enough" for your practical application. You can implement an algorithm that adjusts wGyro depending on some noise factors that you measure, but fixed values will work well for most applications.

We are one step away from getting our updated estimated values:

RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)

Now let's  normalize this vector again:

R = SQRT(RxEst(n) ^2 + RyEst(n)^2 +  RzEst(n)^2 )

RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R
RzEst(n) = RzEst(n)/R

And we're ready to repeat our loop again.

NOTE: FOR PRACTICAL IMPLEMENTATION AND TESTING  OF THIS ALGORITHM PLEASE READ THIS ARTICLE:

http://starlino.com/imu_kalman_arduino.html

Other Resources on Accelerometer and Gyroscope IMU Fusion:

http://www.mikroquad.com/pub/Research/ComplementaryFilter/filter.pdf

http://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data

http://www.dimensionengineering.com/accelerometers.htm

//starlino//

Release Notes / Change Log:

——————————————
2009-09-25
Config Utility Version 1.0.0
Device Firmware Version 1.5
——————————————

- device supports a wide range of accelerometers and gyroscopes
- implements smoothing filtering algorithm
- implements algorithm for combining gyroscope and accelerometer data
- HID USB interface makes the device compatible virtually with all modern operating systems (Mac,Windows,Linux)
- configuration utility (for Windows only for now)
- adjustable smoothing and scale factor for outputs
- configurable sensibility for accelerometer and gyroscope (can be set per axis)
- configurable midpoint (0g voltage) for accelerometer (can be set per axis)
- auto-find gyroscope midpoint (0 deg/s voltage)
- you can use separate accelerometer/gyroscopes for different axis
- Z axis not implemented in this release
- configurable analog input ports AN0-AN5 for each axis (please note 18F2550 does not have AN5 input , but 18F4550 does)
- ability to invert axis
- capture output and raw adc data into a comma separated values file (.csv)
- charting feature of all inputs/outputs
- graphical display of output and buttons
- supports up to 8 buttons
- configuration is stored in device EEPROM
- it is possible to use accelerometer alone on any axis (1, 2 or 3-axis) in this case set Smoothing to 0-10 value
- it is possible to gyro alone on any axis (1,2 or 3 axis) in this case set Smoothing to 100-255 , please note that the output will snap to middle after a while since gyro only provides rate information
- it is possible to use to use gyro and accelerometer on any axis (RECOMMENDED) in this case set smoothing to 20-100 value
- you can use multiple accelerometers or gyroscopes
- you can orient gyro or accelerometer as you like (but aligned to 90 degrees increments)
- you can connect your gyro and accelerometer outputs to any of the AN0-AN4 ports (for PIC18F2550).

Suggested Circuit Schematic

Below is a new suggested schematic , using a 2-axis Gyroscope and an a 2-axis Accelerometer. As mentioned above the new firmware and configuration utility is very flexible regarding the way the gyro and accelerometer are connected (this is one of the features requested , to be able to use device with various MEMS sensors).

Configuration Utility

Here is a screen shot of the configuration utility, please note that I am using the gyro just for one output axis (X), thus I set smoothing to 50 as recommended in release notes above. The other axis (Y) uses just accelerometer data so Smoothing is set to 5. Device automatically detected the Gyro midpoint to be 1.35V , you must hold device still for at least 1s to let it calibrate when you plug it in. The Gyro axis is also inverted due to the way it is mounted in the case.

Hardware Setup

My old accelerometer-only device , got modified to make space for a gyro. Here is how it all fit inside the case:

Demo

Finally here is a new demo with another game I found to work well with this device PURE. In this game you can actually use the FORWARD/BACK tilt (Y axis of the device), besides the usual LEFT/RIGHT (X axis) I demonstrated in the TMNations demos (see my first usb gamepad article).

Downloads

Configuration Utility Setup (Windows XP/Vista) StarlinoGamepadConfigSetup_100.exe

PIC18F2550 Firmware 18Fx550_USB_GAMEPAD_GYRO_15.hex (right click on the link and choose Save Link/Target As if HEX file opens in browser).

I will continue to edit this article , please let me know what questions/suggestions you have in the comments area below. I will try to update this page and design based on your feedback.

//starlino//