IMU based 6DOF estimation in Unity

In my previous blog posts I initialized tip and tilt attitude estimate with only the Input.acceleration Unity offers, and then integrated Input.gyro.rotationRateUnbiased to get the current attitude.  The 2nd post showed the attitude bias observable after only a few seconds of game play.  In this post, I show that position update using just the MEMS accel and gyro is an even bigger problem.

Specific force, gravity, and acceleration

Core idea of strap-down IMU (where the IMU sensor moves and rotates with the body for which the estimate is sought) based position update is to double-integrate the acceleration sensed.  Paul Groves explains the necessary equations in his GNSS 2nd ed. Chapter 5, but I had a tough time understanding how to take out the gravity sensed by the accelerometer.  The technical term he uses for what the accelerometer is sensing is specific force, first explained in Section 2.4.7, as the difference between the true acceleration (what is sought) and the gravitational force the body is subject (which CANNOT be sensed by IMU):

f = a - 𝞬

where f is the specific acceleration vector, a is the acceleration vector, and 𝞬 (gamma) is the gravitational acceleration vector.

I could not understand this explanation because of few observations I made with Unity's Input.acceleration while waving my phone about:

1. When stationary, the sensed acceleration is downward.  For example, [0; 0; -1] when the phone is lying screen-up on my desk. Does it mean that the 𝞬 is UP ([0; 0; 1])?
2. He gave an example of the specific force felt by a passenger in an elevator accelerating up is greater than 1.  But since > 0 in this example, f should be smaller than the stationary case!

The crystallization moment came while reading a book on the construction of MEMS accelerometer, which can be modeled as a proof mass constrained by a pair of springs, like this:

Whether open-loop or more likely closed-loop (with force compensation electronics) design, MEMS accelerometers are inertial devices (hence the name IMU): the proof mass doesn't (at first) move while the case moves; hence the accelerometer senses the displacement (or force--in the case of the closed loop design) in the opposite direction of the case's movement.  The stationary case can be understood by imagining the bottom spring being compressed due to the gravity pulling down the proof mass: 1 unit of the bottom spring being compressed is what is being reported as the "-1" acceleration by Input.acceleration!  If I put the above case in an upward accelerating accelerometer, the bottom spring will compress even more, and I will get an even more negative value from the accelerometer.

Based on this observation, I concluded that the accelerometer is sensing reaction to the force on the body, rather than the force directly.  I confirmed my hypothesis by printing the Input.acceleration values while accelerating my phone in the x/y/z directions, and finally the fundamental force equation made sense:

a^l_lb = G [ q^l_b × −accel^b_l𝞵 + 𝞬^l_lb ]

where q^l_b is the average attitude of the local frame relative to the body rather than the other around and accel^b_l𝞵 is the average inertial acceleration (which is equal and opposite of the specific force the body is subjected to) during the most recent sample period: Once I rotate the specific force from the body frame to the local frame, I can then add the gravitational acceleration estimate to it, to get the estimated acceleration of the body in the local frame--scaled by the Earth's gravity constant.

Above equation for the body's acceleration is incomplete: even though the local tangent frame is stationary relative to the ECEF (Earth centered Earth fixed) frame, since ECEF rotates relative to the ECI (Earth centered inertial) frame, the body experiences:

• Centrifugal acceleration (greatest at the Equator, in the +Z direction, but zero at the Poles).  This is about 0.3% of the gravitational acceleration, of 3 mg.  Anywhere else except at the Equator and the Poles, the centrifugal force has horizontal components in the local frame.  But breaking down that horizontal vector into x and y requires an estimate of the local frame's True North heading as well as the current latitude.
• Coriolis acceleration proportional to the velocity of the body in the direction toward Earth's axis of rotation.  This also requires the heading and latitude.

Since I do NOT want to care about the player's location, I decided to ignore the above 2 pseudo forces, feeling justified because:

• 3 mg is on the order of accelerometer bias confidence interval of off-the-shelf MEMS accelerometers--which means that when you reboot your phone, the bias might change that much anyway, so that the factory calibration can leave this much residual.
• I expect the player to move at a speed in the order of 1 m/s.  When I multiply this by the Earth's rotation speed 72.7 urad/s, the maximum Coriolis acceleration is on the order of 0.1E-3 m/s^2, or roughly 0.01 mg--which is BURIED under the noise floor.

A word of caution on converting the Input.acceleration into specific force: Input.acceleartion is in the RIGHT handed IMU frame while the Unity body frame is in LEFT handed system, as shown in my previous blog post, so only the x and y axes components should flip sign.  As for the gravitational acceleration estimate, precision navigation systems go through a great deal of trouble to get an accurate gravity estimate, using the world gravity model almanac and some even using expensive gravitometers.  I decided to just use the magnitude of the average acceleration measured during the "hold still" period as the magnitude of the gravitational acceleration.

Navigation update equations

Average attitude

In my last blog post, I showed discrete attitude update in Unity using the Input.gyro sensor input--using the quantity attitude increment α^b_ib. If I assume that the angular rate is relatively constant during the sample period, I can do exactly the same calculation for the average attitude during the last sample period by forming a relative rotation quaternion using HALF of the attitude increment.  I considered briefly whether to just use Unity's Quaternion.Slerp() function to spherically interpolate between q^b_l(-) and q^b_l(+), but decided I could better take advantage of the fact that for splitting the attitude increment into half, rather than iterating (as SLERP seemed to).

Velocity update

Once a^l_lb, the average acceleration during the last sample period is obtained, the velocity estimate is just a rectangular integral of a^l_lb with the sample period Ts: v^b_l(+) ~ v^b_l(-) + a^l_lb Ts

Average velocity

To integrate the velocity by sample period for position change, the average velocity is estimated as a simple average of the previous and the updated values: v^b_l(avg) = ( v^b_l(+) + v^b_l(-) ) / 2

Position update

Finally, the position is integrated from the initial position (I assumed 0 for now): r^b_l(+) ~ r^b_l(-) + v^b_l(avg) Ts

Unity implementation

My Unity implementation (my github repo) is mostly straight-forward--except for the fact I do NOT invert q^b_l (which is the current estimate of the body relative to the local frame) to obtain q^l_b.  I did at first, and found that rotating the specific acceleration by q^l_b using Unity's Quaternion.operator*(Quaternion, Vector3) method rotated the vector to the opposite direction of rotation.  In other words, Quaternion.operator*(q^b_l , -accel^b_l𝞵) yielded correct result!

Even before I tried this implementation, I knew that double-integrating the cheap MEMS accelerometer output will end up with my player "going to the moon"--as I've heard the experts describe the problem--pretty quickly.  But knowing it in your head and actually seeing it is quite another.  See for yourself how bad it is.

Now what?

Clearly, this solution is unfit for the intended purpose: to play a game lasting up to 10 minutes.  I have an idea to improve the navigation solution error by sensor fusing the sound echo based pseudo-range estimate with the INS solution just given.  But the equations I've derived so far are too complicated to present in this post, so let me see if I can pull it off first.