Step 3: Combining the Accelerometer and Gyro
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.
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. Assuming you have fixed the position of gyroscope relative to the accelerometer. It is assumed that the gyro and accelerometer borders are parallel to each other, i.e. you're placing the gyro at an angle multiple of 90deg relative to the accelerometer chip. If you acquired an IMU board chances are that they are already aligned this way. We're not going to discuss in this article models where gyroscope is placed at an irregular angle relative to accelerometer (let's say 45 or 30 degrees), although this might be useful in some applications.
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 also grow. 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 cane 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 )
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 oveflow 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 forumula 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.
This guide originally appeared on starlino.com, I've made a few light edits and re-posted it with permission. Thanks Starlino!