PyS60: more fun with N85 accelerometer and rotation sensors

I am no longer surprised about how many accelerometer-related iPhone applications have been created. This thing is really fun and addictive to play with. It has been very pleasant to exercise the brain's L2 cache and remember high-school Physics classes.

Unfortunately, there will be no time to deep discussion of this topic in upcoming Python for Series 60 course, in PythonBrasil event. So FWIW I will post some extra information here, where any interested party can find it.

In that video I made some days ago, I stated that accelerometer applet was lacking gravity compensation. First thing I had to discover, is why gravity induces a opposite-sign reading on accelerometers. That's why:



When the phone is accelerated e.g. by your hand, the seismic weight keeps "behind" the frame. The circuit is wired to measure this kind of acceleration. But, when gravity acceleration is being measured, the opposite happens:


Since the phone is not moving, the gravity pulls the weight to the same direction as the acceleration vector, giving a inverted-sign reading.

Having understood this "feature" of accelerometers, I could work on gravity compensation, so my graphical acceleration applet could discount gravity from readings. First I need to take a "sample" of gravity force, since N85 sensors return accelerometer values in an arbitrary unit. I did it the laziest way possible:

    # detect gravity value
    # for this, cell phone is expected to over a table,
    # perfectly horizontal, and steady
    e32.ao_sleep(.5)
    g = accel.z
    print "gravity = %d" % g

From now on, every Z axis accelerometer can be subtracted by "g", in order to get a gravity-free acceleration.

Of course, this is valid only while the cell phone is kept horizontally to the ground. If you tilt it in a different way, the gravity effect will spread through all axis. And, to detect tilt angles, I need to read the rotation sensor too.

After some trail-and-error based on common sense, I found the following formulas to calculate the gravity force for each axis:

# Get raw accelerometer measures
ax, ay, az = accel.x, accel.y, accel.z
# Calculate gravity force based on tilt angles
gz = g * math.sin(rx) * abs((-math.cos(ry)))
gx = g * math.sin(ry) * abs(math.sin(rz))
gy = g * math.cos(rx) * abs(math.cos(rz))
# Discount gravity from original measures
ax = ax - gx
ay = ay - gy
az = az - gz
# Now we have gravity-free accelerometer measures in ax, ay, az

I am not 100% sure those formulas are right; I can just say that they worked, that is, they allowed me to show only "true" acceleration forces, regardless of tilt. Also, I know that a matrix multiplication would be opportune, but honestly I never learnt matrixes very well :)

Sometimes the accelerometer applet gave spurious readings, flashing big acceleration values briefly. While debugging this, I saw that rotation sensors returned -1 at some readings, breaking down that gravity discount calculations. I chose to ignore all sensor data on -1 angles, and the spurious flashes were gone.

I have made a series of pictures to show how the accelerometer and rotation axis "feel" on the phone. Let's begin with accelerometers, which are simpler to understand:



The axis are relative to the cell phone; it is obvious, but very easy to forget -- after playing too much time with the phone over a table, numbers feel funny for a while when you put it straight up.

When acceleration happens in the direction of the arrow, the measure will be positive. Z axis is positive when phone is accelerated towards front. As we saw, gravity will be perceived as opposite-sign measures.

At least for me, the rotation axis are more difficult to "get", so I made several pictures in order to understand it better:



The axis are the same as the acceleration axis. The circular arrows try to show the tilt direction that increases the angle (by the way, they are clockwise for all three axis). The measured angles for this position are: X=90º, Y=180º and Z=undefined (may return any angle).

The "North" (0º) position for the X axis is when cell phone is upright, like in the following picture:



Note that, in this case, the Y axis becomes undefined. Here we have a limitation of these rotation sensors: they are not gyroscopes, so they depend on gravity to read the angle. When two axis get parallel to the ground, the third one becomes undefined.

The origin (0º) for the Y axis is when the cell phone is e.g. on a flat table, face down:



In the following example, cell phone rests on its right side, and so the Y axis reads 270º:




The origin for Z axis is, like for the X axis, the phone in upright position. Z axis detects "yaw" rotation, like the following picture shows:



The "undefined axis" problem disappears when phone is tilted (neither perpendicular nor parallel to the ground). In the example above, as we add yaw to the phone, Y axis tends to resolve to 270º (assuming that phone is perfectly vertical in X and Y axis).

When the phone has left yaw, Y resolves to 90º:



And finallly, an example of X axis rotation:



DYI INS

Unfortunately, relative rotation plus acceleration is not enough input for a inertial navigation system (INS). That's too bad, I was willing to write something like this, despite the low precision of rotation sensors (15º). A true, three-dimentional INS system (which can estimate position and altitude) needs 3 accelerometers and 3 gyroscopes (or one gyroscope with 3 gimbals).

Just to illustrate the impossibility: if the cell phone undergoes a circular trajectory parallel to the ground, the perceived centrifugal acceleration can't be distinguished from a linear acceleration, because the change in heading angle is not known. The INS wouldn't know if it is going somewhere or if it is walking in circles!

We can only integrate the acceleration if we guarantee an one-dimensional trajectory. Like a straight line, or a train that runs on a predefined track. And we must strap down the phone to the vehicle in a predefined position and angle.

One possibility would be to integrate GPS data with sensors, but the very objective of an INS is to navigate without the need of a GPS... fail!

Some cell phones (including Nokia ones) have a magnetometer sensor (compass). This allows for a two-dimensional INS. If we add some simplifying assumptions e.g. that vehiche is a car and will never have a "roll" angle (except in case of accident :) I think it is even possible to have a 3D INS, without gyros.