In Orientation Representations Part 1 and Part 2, we explore some of the mathematical ways to represent the
orientation of an object. Now we’re going to apply that knowledge to build a virtual gyroscope using data
from a 3-axis accelerometer and 3-axis magnetometer. Reasons you might want to do this include “cost”
and “cost”. Cost #1 is financial. Gyros tend to be more expensive than the other two sensors.
Eliminating them from the BOM is attractive for that reason. Cost #2 is power. The power consumed by a typical
accel/mag pair is significantly less than that consumed by a MEMS gyro. The downside of a virtual gyro is that it is
sensitive to linear acceleration and uncorrected magnetic interference. If either of those is present, you probably
still want a physical gyro.
So how do we go from orientation to angular rates? It’s conceptually easy if you step back and consider the
problem from a high level. Angular rate can be defined as change in orientation per unit time. We already know lots
of ways to model orientation. Figure out how to take the derivative of the orientation and we’re there!
In our prior postings, we’ve discussed a number of ways to represent orientation. For this discussion, we will
use the basic rotation matrix. Jack B. Kuipers has a nice derivation of the derivative of direction cosine matrices
in his “Quaternions and Rotation Sequences” text – one of my most used textbooks. It makes a
good starting point. Paraphrasing his math:
- vf = some vector v measured in a fixed reference frame
- vb = same vector measured in a moving body frame
- RMt = rotation matrix which takes vf into vb
- ω = angular rate through the rotation
Then at any time t:
- vb= RMt vf
Differentiate both sides (use the chain rule on the RHS):
- dvb/dt = (dRMt/dt) vf + RMt(dvf /dt)
Our restrictions on no linear acceleration or magnetic interference imply that:
- dvf/dt = 0
- dvb/dt = (dRMt/dt) vf
We know that:
- vf = RMt-1 vb
Plugging this into (8) yields
- dvb/dt = (dRMt/dt) RMt-1 vb
In a previous posting (Accelerometer placement — where and
why) , we learned about the transport theorem, which describes the rate of change of a vector in a
- dvf/dt = dvb/dt – ω X vb
Those who take the time to check will note that we have inverted the polarity of the ω in Equation 11 from that
shown in the prior posting. In that case ω was the angular velocity of the body frame in the fixed reference
frame. Here we want it from the opposite perspective (which would match gyro outputs).
- dvf/dt = 0
- dvb/dt = ω X vb
Equating equations 10 and 13:
- ω X vb = (dRMt/dt) RMt-1vb
- ω X = (dRMt/dt) RMt-1
|ω X =
Going back to the fundamentals in our first calculus course and using a one-sided approximation to the derivative:
- dRMt/dt = (1/Δt)(RMt+1 — RMt)
where Δt = the time between orientation samples
- ωb X = (1/Δt)(RMt+1 — RMt) RMt-1
Recall that for rotation matrices, the transpose is the same as the inverse:
- RMtT = RMt-1
- ωb X = (1/Δt)(RMt+1 – RMt)
Equation 15 is a truly elegant equation. It shows that you can calculate angular rates based upon knowledge of only
the last two orientations. That makes perfect intuitive sense, and I’m ashamed when I think how long it took
me to arrive at it the first time.
An alternate form that is even more attractive can be had by carrying out the multiplications on the RHS:
- ωb X = (1/Δt)(RMt+1 RMtT – RMt
- ωb X = (1/Δt)(RMt+1 RMtT –
For the sake of being explicit, let’s expand the terms. A rotation matrix has dimensions 3×3. So both
left and right hand sides of Eqn. 22 have dimensions 3×3.
- (1/Δt)(RMt+1 RMtT — I3×3) = (1/Δt) W
|W = RMt+1 RMtT – I3X3 =
The zero value diagonal elements in W result from small angle approximations since the diagonal terms on
RMt+1 RMtT will be close to one, which will be canceled by the subtraction of the
identity matrix. Then:
|ω X =
and we have:
- ωx= (1/2Δt) (W3,2 – W2,3)
- ωy= (1/2Δt) (W1,3 – W3,1)
- ωz= (1/2Δt) (W2,1 – W1,2)
Once we have orientations, we’re in a position to compute corresponding angular rates with
- One 3×3 matrix multiply operation
- 3 scalar subtractions
- 3 scalar multiplications
at time each point. Sweet!
You will notice that we started with an assumption that we already know how to calculate orientation given
accelerometer/magnetometer readings. There are many ways to do this. I can think of three off the top of my head:
- Compute roll, pitch and yaw as described in AN4248. Use those values to compute rotation matrices as described in
Orientation Representations: Part 1. This
approach uses Euler angles, which I like to stay away from, but you could give it a go.
- Use the Android getRotationMatrix  to compute rotation matrices directly. This method uses a sequence of
cross-products to arrive at the current orientation.
- Use a solution to compute the optimal rotation
for each time point. This is my personal favorite, but I think I’ll save further explanation for a future
Whichever technique you use to compute orientations, you need to pay attention to a few details:
- Remember that non-zero linear acceleration and/or uncorrected magnetic interference violate the physical
assumptions behind the theory.
- The expressions shown generally rely on a small angle assumption. That is, the change in orientation from one
time step to the next is relatively small. You can encourage this by using a short sampling interval. You should
soon see an app note that my colleague Mark Pedley is working on that discards that assumption and deals with
large angles directly. I like the form I’ve shown here because it is more intuitive.
- Noise in the accelerometer and magnetometer outputs will result in very visible noise in the virtual gyro
output. You will want to low pass filter your outputs prior to using them. Mark will be providing an example
implementation in his app note.
This is one of my favorite fusion problems. There’s a certain beauty in the way that nature provides different
perspectives of angular motion. I hope you enjoy it also.
Note Number AN4248: Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer
Orientation Representations: Part 1
Orientation Representations: Part 2
- getRotationMatrix() function defined at
- U.S. Patent Application 13/748381, SYSTEMS AND METHOD FOR GYROSCOPE CALIBRATION, Michael Stanley