Posts Tagged ‘accelerometer’

My plateless Anti-vibration mounts

image

image

image

image

These 3d printed ball adapters work great! I thought that the adapter plates that come with these are too heavy so I designed and printed these white adapters, now I use them on all my models – much better than sticky foam plus they are removable!

Finally here is the test flight, note how relatively stable the tricopter is. This is without any prop or motor balancing and with stock PID setting, absolutely no tuning.

Cheers,

//starlino//

Inside Coach – Smart Ball IMU Based Technology

 

image

To all my fans, readers and supporters … I just wanted to share a new exciting project to which I dedicated my last few months of work. This project summarizes and is in a way the culmination of all my IMU research that I’ve done in the past few years. The idea of the project is simple, yet no one produced to date a comparable product that would be flexible and open enough to allow developers to build apps on top of it. Inside Coach is a smart ball technology that incorporates a IMU unit and processor as well as a wireless transceiver. The ball connects to your smart phone or device or computer. On the app side we’re developing a way for people to track their progress and share it with friends. In the first phase Inside Coach will be used by pro players and we’ll record data of certain type of ball kicks. In next stage this data will be fed into an expert system which will be able to analyze it and helped by a machine learning system become your personal coach inside your pocket. We don’t claim this will replace your real coach, don’t fire him yet ! However it will help you or even your real coach to understand better the physics behind a good kick and a bad kick and ultimately achieve repeatability while learning a sport. The philosophy behind this is that  anyone  can score a goal by chance, to become a pro player however you need to understand the factors that lead to it or the factors that make you fail. Yes, some people just have it in their blood, but even those people train and perfect their moves. As any pro player will tell you – talent alone is not enough!  Sometimes you’re so close but still  you’re missing something … but what ? Inside Coach will be able to answer those questions and help you succeed by providing you real time advices from our expert system  via a Bluetooth headset – like “Try more spin” , “Try less speed”, “Mind the 5mph sideways wind”. To some people sports is an art, to some it is magic, and don’t get me wrong – we want to keep all that! We hope that Inside Coach will bring the ball based sports (football, baseball, soccer , volleyball , hand ball , tennis , etc) to the next level. Inside Coach  is where technology meets sports and makes learning and perfecting a sport fun once again !  

Current prototype. All of our pieces put together.

At the time of this post we developed a workable prototype and a basic app used to stream data that is transferred wirelessly from the sensor to your device. However to turn this into  a manufacturable product and do more work on the app we need your support! Rather than seek private capital we decided to go the way of KickStarter and connect directly to people that will be using the product and are as much as we are excited by the technology that this product will bring along its path. This is because we want the development of this product to be dictated by YOU an not by some random banker that has no other interest than gaining money.

If you would like to find more about our journey and support us I invite you to visit the Inside Coach KickStarter page.

Thank you !

//starlino//

Shpero – a robotic project success story

http://orbotix.wpengine.netdna-cdn.com/wp-content/uploads/2013/05/Lifestyle2013-4535.jpg

I recently got my first Sphero and I have to admit as with any new product I was skeptical in the beginning. However just after few minutes of playing with it I had to admit – boy, this stuff really works and is  lots of fun !  Sphero is a simple ball (well, not so simple on the inside) robot that you can control via your tablet or phone via bluetooth (major devices supported). There are many available apps on the market besides the stock control app. It also has a nice SDK that allows to use Sphero for your own projects. Because it contains an IMU sensor inside, Sphero can be used as controller for a game or application.

Only few years ago Shpero was just a crazy idea. Now Ortobotix is a company with dozens of employees. As it happens with many new projects Sphero was not perfect from the start. At their lowest point the founders (Ian Bernstein and Adam Wilson) admit they were so depressed that they were contemplating to pull out of CES 2011 at the last moment. Then something magical happened and the team was able to sort out all problems making it a hit at 2011th CES ! Morale of the story – never give up!

To find out more about the Sphero’s success story read up here:  http://www.gosphero.com/tag/startup/ .  Sphero is available on Amazon and other retailers. Get yourself one, I guarantee it – it’s not just a toy but also a great research tool if you’re into electronics or robotics !

//s//

DCM Tutorial – An Introduction to Orientation Kinematics

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

clip_image002[4]

Figure 1

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

IG = {1,0,0} T, JG={0,1,0} T , KG = {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:

iB = {1,0,0} T, jB={0,1,0} T , kB = {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:

iG = {ixG , iyG , izG} T

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

ixG = |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:

ixG = 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 = IB.iB = IG.iG = cos(IB.iB) = cos(IG.iG) , 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:

iyG = J.i , izG=K.i , so now we can write vector i in terms of global coordinate system as:

iG= { I.i, J.i, K.i}T

Furthermore, similarly it can be shown that jG= { I.j, J.j, K.j} T , kG= { 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:

clip_image004[4] (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 IG, JG, KG 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:

IB= { I.i, I.j, I.k}T , JB= { J.i, J.j, J.k}T , KB= { K.i, K.j, K.k}T

and organized in a matrix form:

clip_image006[4] (Eq. 1.2)

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

Also notice that DCMB. DCMG = (DCMG)T .DCMG = DCMB. (DCMB)T = I3 , where I3 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:

clip_image008[4] (Eq. 1.3)

To prove this we use such properties as for example: iGT. iG = | iG|| iG|cos(0) = 1 and iGT. jG = 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:

rB= { rxB, ryB, rzB} T and let’s try to determine its coordinates in the global frame, by using a known rotation matrix DCMG.

We start by doing following notation:

rG = { rxG , ryG , rzG } T.

Now let’s tackle the first coordinate rxG:

rxG = | rG| cos(IG,rG) , because rxG is the projection of rG onto X axis that is co-directional with IG.

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:

| rG| = | rB| , | IG| = | IB| = 1 and cos(IG,rG) = cos(IB,rB), so we can use this property to write

rxG = | rG| cos(IG,rG) = | IB || rB| cos(IB,rB) = IB. rB = IB. { rxB, ryB, rzB} T , by using one the two definition of the scalar product.

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

rxG = IB. rB = { I.i, I.j, I.k}T . { rxB, ryB, rzB} T = rxB I.i + ryB I.j + rzB I.k

In same fashion it can be shown that:

ryG = rxB J.i + ryB J.j + rzB J.k
rzG = rxB K.i + ryB K.j + rzB K.k

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

clip_image010[4] (Eq. 1.4)

 

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

We can use similar logic to prove the reverse process:

clip_image012[4] (Eq. 1.5)

Or we can arrive at the same conclusion by multiplying both parts in (Eq. 1.4) by DCMB which equals to DCMGT, and using the property that DCMGT.DCMG = I3 , see (Eq. 1.3):

DCMB rG = DCMB DCMG rB = DCMGT DCMG rB = I3 rB = rB

 

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:

clip_image014[4]

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 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|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 vr and vw 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 wx , wy , wz 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 = wxdt, about y axis by an angle of dθy = wydt and about z axis by an angle of dθz = wzdt. These rotations can be characterized by the angular velocity vectors: wx = wx i = {wx , 0 , 0 }T , wy = wy j = { 0 , wy , 0 }T , wz = wz k = { 0 , 0, wz }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):

dr1 = dt v1 = dt (wx x r) ; dr2 = dt v2 = dt (wy x r) ; dr3 = dt v3 = dt (wz x r) .

The combined effect of these three displacements will be:

dr = dr1 + dr2 + dr3 = dt (wx x r + wy x r + wz x r) = dt (wx + wy + wz) 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 = (wx + wy + wz) x r = w x r , where we introduce w = wx + wy + wz = {wx , wy , wz }

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 wx , wy , wz is equivalent to one small rotation characterized by angular rotation vector w = wx + wy + wz = {wx , wy , wz }. 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 wx , wy , wz 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.

clip_image016[4]

Figure 3

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

clip_image018[4]

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 KB. If the 3 axis accelerometer output is A = {Ax , Ay , Az } and we assume that there are no external accelerations or we have corrected them then we can estimate that KB = –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 = {Mx , My , Mz }, then according to our model IB is pointing North , thus IB = M.

Knowing IB and KB allows us calculate JB = KB x IB.

Thus an accelerometer and a magnetometer alone can give us the DCM matrix , expressed either as DCMB or DCMG

DCMG = DCMBT = [IB, JB, KB]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 rB = {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):

rG = DCMG rB

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 (DCMG = DCMBT ).

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:

clip_image020[4]

If we read the rows of DCMG we get the vectors IB, JB, KB. We’ll work mostly with vectors KB (that can be directly estimated by accelerometer) and vector IB (that can be directly estimated by the magnetometer). The vector JB is simply calculated as JB = KB x IB , 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 t0 and we note it as KB0. Also let’s say we measured our gyro output and we have determined that our angular velocity is w = {wx , wy , wz }. 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 KB1G . And we find it using (Eq. 2.6):

KB1GKB0 + dt v = KB0 + dt (wg x KB0) = KB0 + ( dθg x KB0)

Where we noted dθg = dt wg. Because wg 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 KB changed during this small period of time dt.

Obviously, another way to estimate KB is by making another reading from accelerometer so we can get a reading that we note as KB1A .

In practice the values KB1G will be different from from KB1A. 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 wa or angular displacement dθa ­= dt wa , from the new accelerometer reading KB1A­ , we’ll use (Eq. 2.5):

w­a = KB0 x va / | KB0|2­

Now va = (KB1A­ KB0) / dt , and is basically the linear velocity of the vector KB0. And | KB0|2­­ = 1 , since KB0 is a unity vector. So we can calculate:

dθa ­= dt wa = KB0 x (KB1A­ KB0)

The idea of calculating a new estimate KB1 ­ that combines both KB1A and KB1G is to first estimate dθ as a weighted average of dθa and dθg :

dθ = (sa dθa + sg dθg) / (sa + s), 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 KB1 ­ is calculated similar to how we calculated KB1G:

KB1KB0 + ( dθ x KB0)

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

IB1IB0 + ( dθ x IB0)

JB1JB0 + ( dθ x JB0)

The idea is that all three versors IB, JB, KB 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 DCM1 matrix at time t1 from our previous estimated DCM0 matrix at time t­­0. 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 KB vector it estimates the vector pointing North IB. 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 wm = IB0 x (IB1M­ IB0)

Now let’s incorporate it into our weighted average:

dθ = (sa dθa + sg dθg + sm dθm) / (sa + sg +­ sm)

From here we go the same path to calculate the updated DCM

IB1IB0 + ( dθ x IB0) , KB1KB0 + ( dθ x KB0) and JB1 JB0 + ( dθ x JB0),

In practice we’ll calculate JB1 = KB1 x IB1, after correcting KB1 and IB1 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 IB0, JB0, KB0 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 IB1, JB1, KB1 , 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.
clip_image022[4]

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) = ba (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’ = ab (b.a) = ab (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’ = ab (a.b) / 2 (Scenario 3, both a and b are corrected)
b’ = ba (a.b) / 2

clip_image024[4]

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 IB1, JB1, we apply the following corrective actions before reintroducing the DCM matrix into the next loop:

Err = ( IB1 . JB1 ) / 2

IB1= IB1 – Err * JB1
JB1= JB1 – Err * IB1
IB1’’ = Normalize[IB1]
JB1’’ = Normalize[JB1]
KB1’’ = IB1’’ x JB1’’

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

So finally our corrected DCM1 matrix can be recomposed from vectors IB1’’, JB1’’, KB1’’ that have been ortho-normalized (each vector constitutes a row of the updated and corrected DCM matrix).

We repeat the loop to find DCM2 , DCM3 , 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:

https://code.google.com/p/picquadcontroller/source/browse/trunk/

The code is in imu.h file:

https://code.google.com/p/picquadcontroller/source/browse/trunk/imu.h

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

IMU breakout boards from ST

Sign that someone from ST is reading the blogs – a series of breakout boards directly from ST , just couple of good ones:

$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 :-)

ST launches Multi sensor application (MuSA) board platform

For $99  MuSA  could be a nice hackable platform for all sorts of motion-enabled applications.  It has a  built-in accelerometer , LCD  and a simple casing. Here is the description from ST:

"The platform comes with the LIS331DLH preinstalled on the board, but can support any digitaloutput accelerometer from ST in 3×3 or 3×5 mm packages connected to the microcontroller through an SPI interface, as well as the LIS344ALH analog-output accelerometer.

The MuSA platform features four buttons for navigating the menus. A mini-USB connector is available to exchange data with a PC, depending on the specific application, and also recharges the internal Li-ion battery. The battery is capable of providing power for approximately 7 hours of operation."

For device demonstration see video here:

http://www.st.com/stonline/domains/support/edemoroom/index.htm?id=14

Upgrade your RC Transmitter with a DIY Tilt Motion Control Module

If you are into Radio Control Models or robotics chances are that you have an old RC transmitter laying around. This article describes how to create a motion control module for your RC transmitter, that will allow you to control your model or robot by simply tilting the transmitter case. That's right not sticks!

Demo

Read More »

Quadcopter Prototype using Acc_Gyro and a PIC

For anyone following this site, here is what I've been up to lately – building a quadcopter based on the Acc_Gyro 5DOF IMU sensor and a 16bit PIC. Although it's still a work in progress I decided to start putting together an article placeholder and build it up as project evolves. It's going to be a long one !

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:

Read More »

Play PC games the iPad style – Using a PIC with USB, accelerometer and optional Gyroscope

The iPad is finally out – one feature that might caught your attention is the built-in accelerometer and the ability to control a game by tilting the device. For more than a year I was working on a similar idea for the PC Notebook market based on my original  motion gamepad project that would  allow playing a game by tilting the laptop/netbook. Now that iPad is out I hope that the notebook/netbook manufactures will catch up by incorporating MEMS sensor into their devices. Here is the result of my prototype , it is a USB attached device, but ideally I think this should be embedded into the laptop.

Demo:

Read More »

Exploring a digital I2C/SPI accelerometer (MMA7456L) with Bus Pirate

Bus Pirate is a great tool for exploring new chips using your PC , without the need to integrate the chip into a MCU project. Once I received my unit, i decided to put it to the test by exploring an accelerometer with I2C/SPI interface – the MMA7456L from Freescale. I am writing this in hope that it will help other people get started with BusPirate and I2C protocol in particular. I will only describe the I2C interface in this article but BusPirate is capable of so much more !

The setup of BusPirate was simple. Simply plug it to your USB port, install the FDTI driver (if necessary). After that it will appear as a virtual COM port on you computer. You then can use your terminal of choice to interact with it. Here is sample session in Termite (my choice of terminal software):

bus pirate terminal termite

Read More »

Arduino code for IMU Guide algorithm. Using a 5DOF IMU (accelerometer and gyroscope combo)

This article introduces an  implementation of a simplified filtering algorithm that was inspired by Kalman filter. The Arduino code is tested using a 5DOF IMU unit from GadgetGangster – Acc_Gyro . The theory behind this algorithm was first introduced in my Imu Guide article.

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

IMG_1359.JPG

Read More »

A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications.

Introduction

There’s now a FRENCH translation of this article in PDF. Thanks to Daniel Le Guern!

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).

Acc_Gyro_6DOF  and UsbThumb MCU 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

Read More »

Accelerometer Experiments – Part 2: LIS331AL and DE-ACCM2G (ADXL322) . Filters, amplifiers and vibration response.

In a previous article I have analized 3 accelerometer side by side comparing their noise level. Today I am going to test a 3 axis accelerometer from ST  LIS331AL. I am going to compare it with the DE-ACCM2G accelerometer from Dimension Engeneering.

For testing purposes I have mounted the LIS331AL on a break-out board. If you're interested how this board can be done and device mounted make sure you read my article "DIY Surface Mount on a Budget".

I have connected both accelerometers to my USB Gamepad device  and will be using the Gamepad Configuration Software to trace the signals (although an osciloscope could be used instead).

IMG_1135.JPG

Read More »

Accelerometer Experiments – Part 1: DE-ACCM2G (ADXL322), LIS244AL, Pololu MMA7260QT

In the process of developing my usb motion gamepad I got a chance to work with different accelerometers. In search of the perfect device I wish there was a place where I can go and compare them side by side.  The problem is that different manufactures have different methods of testing the noise parameters so the only way to get it right is to have them tested by a third party. I will start by analyzing 3 accelerometers I have in my possession, and hope to review more as I get my hands on them.

The accelerometers I will test are:

1) Dimension Engineering DE-ACCM2G (this is the older model based on Analog Devices ADXL322 chip datasheet ).  It is now being replaced by a different model DE-ACCM2G2 based on LIS244ALH chip from ST). This is a quality product that unlike other break-out boards has a built-in amplifier.

2) Second device is a bare-bone LIS244AL ,  a self-mounted using reverse mounting method (yes it still works ! :) ).

3) Finally is one of the cheapest accelerometer break-out boards out there the Pololu MMA7260QT.

Recently I completed my Gamepad Configuration Utility and decided to put  to use for something it was not necessarily built for. I connected 3 accelerometers (to precize their X axis output) to the analog ports of PIC18F4550 (one of my gamepad prototypes "The Brick").

IMG_1118.JPG

Read More »

USB Motion Gamepad Update: wide accelerometer and gyroscope support, configuration utility software

I have received some feedback from my readers regarding my first usb gamepad project , so for the past few weeks I was working on a new imrpoved design. There are plenty of new improvements that I hope will address many of your requests.

Read More »

Accelerometer Controlled Usb Gamepad and Mouse using PIC18F2550 / PIC18F4550

Demo

Read More »