My humble dream
I am self-learning Unity3D game programming these days, to write an augmented reality peer-to-peer shooter game on a smartphone--just to play a game with my friend's 5-year old son in the living room.The problem at hand
For a seamless integration of game objects on the screen, I need to track the phone's position and orientation within the room. Since I don't know much about image processing beyond basic filtering and how to use OpenCV, I am looking into using relative 6 DOF tracking using the accelerometer and gyro available on every modern smartphones--despite the expected problems due to the high bias and noise of the MEMS sensors in a smartphone. A solution discussed in many textbooks (I like Paul Groves the best so far) is to initialize the 6 DOF with some constraint (like when the IMU is stationary), and then integrate the angular rate and the acceleration sensed in the body frame of the phone.All of this is elementary stuff for mechanical engineers. But when I tried to apply this simple idea in Unity, I ran into complications: firstly Unity3D's left-handed coordinate frame. At first, I tried to keep the navigation equations in RH system (as described in textbooks) and just convert to LH right before rendering, but the confusion did not go away. So here's an attempt to re-derive the coarse attitude (but without the heading) initialization in Unity's LH system. To motivate you to persevere through the trigonometry below, here is a screenshot illustrates the idea of putting the target plane (the ENEMY text) overlaid on top of the rear camera view. As I tip and tilt my phone, my view of the target plane--which does NOT move or rotate in this demo--changes in the opposite angle of my current attitude estimate, which is displayed in the upper left corner of the video.
1st attempt: IMU frame as documented for the phone; wrong
When the device is (mostly) stationary, the accelerometer is just sensing gravity (reaction to gravity actually, but I'll explore that idea in a different blog entry). In the diagram below, I've drawn an IMU (accel and gyro package) placed arbitrarily on a phone--in the RIGHT HANDED frame labeled 𝞵 (in navigation equations, the frame appears in superscripts and subscripts, so "IMU" is unwieldy), which is oriented also arbitrarily relative to the LEFT HANDED local tangent frame l (letter "ell"). Since I will play the game while holding my phone like I am taking a picture, I've drawn the LEFT HANDED body frame to be oriented the same way as the Unity's player frame when the phone is held that way: +Z away from the phone's rear facing camera, and +Y toward the top of the screen in a landscape mode, with the "Home" button on the right of the screen.
The gravity sensed by the IMU is g𝞵l𝞵: I've drawn the gravity in the -Z direction of the local tangent frame, and the gravity is resolved in 𝞵--specifically the x, y, z triad of the accelerometer. As far as I know, the IMU is placed like this on all smartphones. What are the x/y/z triad values as functions of the phone's roll and pitch? If I define the roll and pitch to be zero when the body frame's Z axis is parallel to the l frame's -Z axis, and the body frame's X axis is parallel to the l frame's -Y axis, we can follow the roll and pitch as shown below.
Navigation equations can be confusing to anyone without spatial IQ of 200, so let's make a note that the blue line above is the physical components of the gravity, which will be SENSED by the accelerometer in 𝞵 frame (the blown) as the a vector. After the roll and pitch, we can read off the components of gravity in each of the orthogonal axes of the IMU frame (brown above)--which may NOT be co-located with the body frame or even oriented exactly as drawn above (there are elaborate engineering requirement specs for the PCB placement of the IMU). BUT if it were, the values would be as follows:
ax = -g cos𝟇 cos𝞱
ay = g sin𝟇
az = -g cos𝟇 sin𝞱
Let's not worry about the accel bias or the IMU mounting error relative to the body frame for now, and just solve the above over-constrained equations for the roll and pitch angles.
𝟇 = asin(ay/g)
𝞱 = atan2(-az, -ax)
Note the negative sign into the arc tangent, necessary to land on the correct quadrant.But when I tried initializing with these equations, the answer was clearly wrong: my spaceship jumped to a completely wrong orientation!
2nd attempt: Empirically determined IMU frame
I don't know if Unity will rotate this frame for a portrait mode, but it looks like this frame is just the same as the player frame, except for being RIGHT handed.
The accel triad in the IMU frame (length of the blue arrows on the brown axes) then change to
ax = -g sin𝟇
ay = -g cos𝟇 cos𝞱
az = -g cos𝟇 sin𝞱
In which case the angles are:
𝟇 = asin(-ax/g)
𝞱 = atan2(-az, -ay)
The sequence of rotation from the local tangent frame l to the current body frame attitude, expressed as LEFT to right (because that's how Unity applies the rotation--the opposite of almost all quaternion books I've read) quaternion multiplication is then
q_lb = Quaternion(0, 0, sin(𝞹/4), cos(𝞹/4)) // quarter turn about Z
* Quaternion(sin(𝞹/4), 0, 0, cos(𝞹/4)) // quarter turn about X
* Quaternion(0, 0, sin(𝟇/2), cos(𝟇/2))
* Quaternion(sin(𝞱/2), 0, 0, cos(𝞱/2))
where I use the quaternion notation of the form [x, y, z, w] (rather than [w, x, y, z]). Above algorithm has a singularity at roll angle = +/- 90 deg, because the y and z components of the acceleration are small (and ax is almost 1).
3rd attempt: yaw to avoid singularity at roll +/- 90 deg
To avoid singularity when roll is near +/- 90, I need to apply a yaw around Y (instead of the pitch around X as above). I drew 2 pictures to convince myself that the same equation can handle both the +90 and the -90 case, as you can see below:
Note that the acceleration triad expressions are the same around positive and negative roll, so I don't have to come up with 2 distinct cases. So how shall we solve the following accelerometer triad?
ax = -g sin𝟇 cos𝟁
ay = -g cos𝟇
az = g sin𝟇 sin𝟁
The angles are:
𝟇 = -sign(ax) acos(-ay/g)
𝟁 = asin2(az/(g sin𝟇))
The switch between the 1st and this alternative rotation scheme is whether the normalized |ax| is larger than some value, say 0.8. The quaternion from the local frame to the body frame is then𝟁 = asin2(az/(g sin𝟇))
q_lb = Quaternion(0, 0, sin(𝞹/4), cos(𝞹/4))
* Quaternion(sin(𝞹/4), 0, 0, cos(𝞹/4))
* Quaternion(0, 0, sin(𝟇/2), cos(𝟇/2))
* Quaternion(0, sin(𝟁/2), 0, cos(𝟁/2))
Unity implementation
I tried to initialize my current attitude with Quaternion.Euler(x, y, z), whose definition of the rotation order miraculously matches the rotation order presented above. But for some reason, it did not work (I am on Unity version 5.2.4f2), so I had to just brute-force multiply the quaternions in my PlayerController.FixedUpdate() method, as shown below:Vector3 a_b = a_inb_lb_ens.Average.normalized;
float roll, pitch;
roll = Mathf.Asin (-a_b.x);
pitch = Mathf.Atan2 (-a_b.z, -a_b.y);
roll *= 0.5f; // Halve the Euler angles feed to quaternion construction
pitch *= 0.5f;
Quaternion q = // Apply the roll first, then pitch
new Quaternion (0, 0, Mathf.Sin (roll), Mathf.Cos (roll))
* new Quaternion (Mathf.Sin (pitch), 0, 0, Mathf.Cos (pitch));
const float THRESHOLD = 0.8f;
if (Mathf.Abs (a_b.x) < THRESHOLD) { // 1st case: cos(roll) reasonable value
q_lb = Q_lb * q;
} else { // 2nd case, roll near +/- 90; ay << ax
roll = -Mathf.Sign (a_b.x) * Mathf.Acos (-a_b.y);
float yaw = Mathf.Asin (a_b.z / Mathf.Sin (roll));
roll *= 0.5f;
yaw *= 0.5f;
Quaternion q2 =// Apply the roll first, then yaw about Y
new Quaternion (0, 0, Mathf.Sin (roll), Mathf.Cos (roll))
* new Quaternion (0, Mathf.Sin (yaw), 0, Mathf.Cos (yaw));
q_lb = Q_lb * Quaternion.Slerp (q, q2, Mathf.Abs (a_b.x) - THRESHOLD);
}
q_Ub = Q_Ul * q_lb; //Rotate the q_lb by Q_lU to go to Unity
myRigidbody.MoveRotation(q_Ub);
Q_lb and Q_Ub are constant quaternions relating respectively the attitude of the player WRT the local tangent frame, and the local tangent frame to Unity's inertial frame.
static Quaternion Q_lb = new Quaternion (0, 0, Mathf.Sin (Mathf.PI / 4), Mathf.Cos (Mathf.PI / 4))
* new Quaternion (Mathf.Sin (Mathf.PI / 4), 0, 0, Mathf.Cos (Mathf.PI / 4))
;
static Quaternion Q_Ul = //Will be used often, so calculate once
new Quaternion(0, Mathf.Sin(-Mathf.PI/4), 0, Mathf.Cos(-Mathf.PI/4))
* new Quaternion(Mathf.Sin(-Mathf.PI/4), 0, 0, Mathf.Cos(-Mathf.PI/4))
;
And here is the video of a short session, where I use the attitude initialization at 50 Hz (i.e. I am NOT yet integrating the INS input).
Conclusion: use the (left) hand, Luke--and sweat it out
Attitude initialization is the 1st step in an INS (inertial navigation system). While the idea of using the side components of accelerometer triad to estimate the current tip and tilt is simple to imagine, actually pulling off a Unity game demo was far more difficult than I imagined at first, partly because Unity's left handed frame forced me to re-derive the gravity triad expressions. In Groves' GNSS 2nd edition, this subject takes up just a single page (and the book and the appendices all combine to just shy of 1000 pages)! Drawing clear pictures, and applying successive LEFT handed rotation step by step was the only way I could to come through the whole mess in one piece. Although it was grueling, I am glad that I persevered through it; it was a character-building experience, and now I am on my way to integrating the gyro and accel into a 6 DOF solution.
