Starlino_DCM_Tutorial_01

DCM TUTORIAL

–

AN INTRODUCTION TO

ORIENTATION KINEMATICS (REV 0.1)

Introduction

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, tha

t’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 - /wiki/Cartesian_coordinate_system

Rotation - /wiki/Rotation_%28mathematics%29

Vector scalar product - /wiki/Dot_product

Vector cross product - /wiki/Cross_product

Matrix Multiplication - /wiki/Matrix_multiplication

Block Matrix - /wiki/Block_matrix

Transpose Matrix - /wiki/Transpose

Triple Product - /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 o

f 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 3 , where I

3 is the 3x3

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 transf

ormation 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 3 , see (Eq. 1.3):

DCM B r G = DCM B DCM G r B = DCM GT DCM G r B = I 3 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 chara

cteristic 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| 2 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| 2 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| 2 d

t) (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| 2 dt) = (r x r + r x dr)) / (|r| 2 dt) = r x (dr/dt) / |r| 2

And finally:

w = r x v / |r| 2 (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| 2 ) x r = (r x v) x r / |r| 2 = ((r.r) v + (v.r)r) / |r| 2 = ( |r| 2 v + 0) |r| 2

= 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: /imu_ . 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 x dt, about y axis by

an angle of dθ y = w y dt and about z axis by an angle of dθ z = w z dt. 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 1 = dt v 1 = dt (w x x r) dr 2 = dt v 2 = dt (w y x r) dr 3 = dt v 3 = dt (w z x r) .

The combined effect of these three displacements will be:

dr = dr 1

+ dr 2

+ dr 3 = 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 rotation

s 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 th

e