Attitude update in Unity using Input.gyro

In my previous blog post, I initialized the tip and tilt of phone in Unity using Input.acceleration.  The next step is to measure the relative attitude change from the initial attitude using Unity's gyro input.  Sure you can use Input.gyro.attitude to use the OS or Unity's estimate of the device attitude, but I want to learn inertial navigation equations, so I am going to trudge along, with only Paul Groves' GNSS 2nd edition to guide me.

The rotation rate in Unity player's body frame

The first order of business is to draw the gyro triad (which is reported in the RIGHT handed IMU frame) in the Unity PlayerController's (LEFT handed) body frame, as I've done in the previous blog entry, whereupon you should notice that the X and Y components should change sign.  Therefore, the angular rate in the player's body frame should be expressed like this:

Vector3 w_inb_lb = //Input.gyro.rotationRate // in mu frame (RH!)
                    Input.gyro.rotationRateUnbiased
                ;
w_inb_lb.Set (-w_inb_lb.x, -w_inb_lb.y, w_inb_lb.z);

First order attitude update equation

The core idea of attitude update is to rotate the current attitude (however it is expressed--quaternion, directional cosine matrix, rotation vector and angle, etc) by the INCREMENTAL rotation from the previous update to the current update.  It is mathematically captured in the attitude differential equation, as in GNSS 2nd edition Appendix E, equation E.33.  A first order approximate discrete time implementation of that differential equation for in Earth centered inertial frame (ECI) is

q^b_i(+) ~ q^b_i(-) × Quaternion(α^b_ib, 1)

where the attitude increment α^b_ib is approximated as a product of the average angular rate sensed in the last interval and the sampling time Ts.

α^b_ib ~ 𝟂^b_ib Ts

But since  the local tangent frame is located at latitude L and yawed at an angle from the longitudinal line (the one that goes from the South Pole to the North Pole) and rotates with Earth, there will be an extra term for the quaternion from the local tangent frame to the body frame:

q^b_l(+) ~ q^b_l(-) × Quaternion(α^b_lb, 1) - Quaternion(𝟂^l_ie Ts/2, 0) × q^b_l(-)

where the Earth's rotation vector is resolved into the local tangent frame by the latitude and angle from the longitude:

𝟂_ie^l_ie = |𝟂_ie| [ cosL cos𝟁_el ; cosL sin𝟁_el ; sinL ]

Figuring out the phone's ROUGH lat/long is not that onerous, but when consider the magnitude of the Earth's rotation |𝟂_ie| (2𝞹 / 24 hr = 72.7 urad/s), I feel safe in ignoring this term for a game that should last on the order of 10 minutes.  So I came up with the following Unity code:

Vector3 alpha = (0.5f * Time.deltaTime) * w_inb_lb; Quaternion q_inc_lb = new Quaternion(alpha.x, alpha.y, alpha.z, 1);

q_lb = q_lb * q_inc_lb; //Unity quaternion multiples LEFT -> RIGHT
q_Ub = Q_Ul * q_lb; //Rotate the q_lb by Q_lU to go to Unity
Normalize(ref q_Ub); //Normalizing quaternion is less onerous than orthogonalizing a DCM
myRigidbody.MoveRotation(q_Ub);//Move the player to the est. attitude

You can find this source code in my github repo: https://github.com/henrychoi/OpenCVUnity

The following video is the result of the above algorithm in the Unity Remote (which relays the phone's Input.gyro updates to the Unity editor--and Unity editor plays the video in the Unity Remote screen).  I start the session by initializing the attitude while the phone is stationary on my desktop vise (yes I have a desktop vise), and then take it out of the vise and rotate it around for like 30 seconds before putting it back in the vise.  To help visualize the current attitude, I show a chase camera view from right behind and slightly above the player (upper left corner), and also a target ("ENEMY") located straight ahead.  When the player's forward vector (0, 0, 1) lands on the target, the target lights up red.

Clearly, the attitude is not coming back to the starting value after this brief excursion, so that I cannot build a playable game with this algorithm.  I suspect that the problem is the high bias and noise of MEMS gyro in my phone, but how much improvement can higher order attitude update equation be?

Higher order attitude increment

According to Groves GNSS 2nd ed. Appendix E, section 6.3, a more precise attitude increment than the Quaternion(α^b_ib, 1) is

q^b+_b- = Quaternion( α^b_ib sin(|α^b_ib| / 2 ) / |α^b_ib|, cos( |α^b_ib| / 2 ) )

q^b_i(+) = q^b_i(-) × q^b+_b-

But due to the division by 0 problem, this precise increment cannot be implemented; instead, 4th order expression for q^b+_b- is suggested

q^b+_b- = Quaternion( As α^b_ib, Ac );

As = 1/2 - (|α^b_ib| / 2)² / 12

Ac = 1 - (|α^b_ib| / 2)² / 2 + (|α^b_ib| / 2)⁴ / 24

Here's the Unity implementation, showing only the parts that changed from the 1st order approximation given earlier.

              Vector3 alpha = // Mathf.Deg2Rad * //attitude increment
                    Time.deltaTime * w_inb_lb;
float alpha_div2 = 0.5f * alpha.magnitude;
float alpha_div2_sq = alpha_div2 * alpha_div2;
float alpha_div2_qd = alpha_div2_sq * alpha_div2_sq;
float As = 0.5f - 0.083333333333333f * alpha_div2_sq;
float Ac = 1.0f - 0.5f * alpha_div2_sq + 0.041666666666667f * alpha_div2_qd;
alpha *= As;
q_inc_lb.Set (alpha.x, alpha.y, alpha.z, Ac); // 4st order approximation

When I tried this solution, the drift in non-stationary case was still there, so it looks like my intuition was correct after all.