Finding the cardinal direction accelerated with an Algorithm for using the magnetometer, accelerometer, and gyro readings

Question

I want to find the cardinal direction accelerated by an iphone. I thought I could just use the accelerometer to do this, however, as you can see from the picture below the accelerometers axes are defined by the device orientation.

I figured that if i used the gyroscope to correct for yaw, spin, rotation then I could get a more accurate reading and not have to hold the phone in the same orientation during movement.

But this still does not tell me what cardinal direction the iphone is moving in. For that I would also have to use the the magnetometer.

Can anybody tell me how to use a three sensor readings to find the cardinal direction accelerated in? I dont even know where to start. I dont even know if the phone takes these measurements at the same rates of time either.

Answer 1

Taking the cross product of the magnetometer vector with the "down" vector will give you a horizontal magnetic east/west vector; from that, a second cross product gets the magnetic north/south vector. That's the easy part.

---

The harder problem is tracking the "down" vector effectively. If you integrate the accelerometers over time, you can filter out the motion of a hand-held mobile device, to get the persistent direction of gravity. Or, you could, if your device weren't rotating at the same time...

That's where the rate gyros come in: the gyros can let you compensate for the dynamic rotation of the hand-held device, so you can track your gravity in real-time. The classic way to do this is called a Kalman filter, which can integrate (both literally and figuratively) multiple data sources in order to evaluate the most likely state of your system.

---

A Kalman filter requires a mathematical model both of your physical system, and of the sensors that observe it; each of these models must be both accurate and "sufficiently linear" for the Kalman filter to work properly. As it happens, the iphone/accelerometer/gyro system is in fact sufficiently linear.

The Kalman filter uses both calculus and linear algebra, so if you're rolling your own, you will need a certain amount of math.

Also, as a practical matter, you should understand that physical sensors typically have offsets that need to be compensated for -- in particular, you need to pay attention to the rate gyro offsets in this kind of inertial navigation system, or your tracker will never stabilize. This means you will need to add your rate gyro offsets to your Kalman state vector and system model.