Help! My Cockpit Is Drifting Away

Developers Blog | Posted by Steve LaValle | |

When I was a kid, my dad gave me this nice compass:

compass

I was amazed that the arrow in this magical device always seemed to point North. I carried this belief about compasses into my adult life. They are well known to help people overcome dead reckoning problems, which prevent them from traveling along a straight course. Robots face similar problems: In my previous research, I imagined that we can simply give a mobile robot a sensor called a “compass”, which allows it to know which direction it is facing.

It turns out that the Rift Development Kit similarly suffers from yaw drift error, which is a steadily growing error in the estimated “forward” direction. Fortunately, the sensor board inside the Rift contains a magnetometer. Determining which way your Rift is facing ought to be as simple as reading the magnetometer values to obtain a virtual compass arrow, right? Like most research pursuits, however, the quest to solve a problem leads to several waves of confusion and further enlightenment. In this post, I will explain how we correct yaw drift error in the Rift. The key for me, at least, was to shatter my belief in the perfect compass, and understand what a magnetometer, the real sensor, is actually trying to measure.

The Yaw Drift Problem

In an earlier blog post, I explained the basic head tracking problem and our solution. When we integrate gyroscope readings of angular velocity, the resulting, estimated head orientation gradually drifts away from the true orientation. An accelerometer is used to compensate for tilt drift error, which leaves one important component: yaw drift error. This is incorrect rotation about a vertical axis (aligned with gravity).

In some games, such a first-person shooter, yaw drift error may be inconsequential because you can re-orient your avatar without reference to a fixed direction; however, in others it becomes quite an annoyance. For example, if you are in a cockpit game, you may start off facing forward (Hawken is shown here):

CockpitNormal

but over time, the cockpit will drift the side, even though you are still facing forward in the physical world:

CockpitDrift

You could rotate your chair to compensate, but we should be solving this problem, rather than you!

Magnetic Field Sensing

The interaction of magnetic materials and electric currents is conveniently described by a 3D vector field. At every point in the space around you, a 3D magnetic field vector exists that has both magnitude and direction. You can visualize the vector at each point as an arrow of some length that points in some direction (including upward or downward). In a constant vector field the field vector does not depend on the particular time or position; all arrows have the same length and point in the same direction:

ConstantField

Although not true in practice, it is a convenient approximation within a small area.

Now consider measuring this field vector. We are fortunate that magnetic field sensing has come a long way over the past century. Magnetometers have followed the same rapid trend we have observed across many technologies: They have become smaller and cheaper at an exponential rate. Further helping this trend is the proliferation of MEMS-based magnetometers in recent smartphones to provide help with directions. Even as a hobbyist, you can now buy a tiny, three-axis magnetometer online for around $1 US.

Now imagine placing a tiny sensor into the world to measure the magnetic field vector (m_x, m_y, m_z). The sensor cannot simply output the three components m_x, m_y, and m_z because it measures the field relative to its own orientation. A three-axis magnetometer measures the magnetic field vector projected along each of its three perpendicular axes. Each axis reads a single intensity value that corresponds to the projection along the axis. The figure shows a 2D version of this, in which there are two perpendicular axes:

RotMag

If the sensor is rotated counterclockwise by \theta, then the measured field vector is rotated by -\theta. For the 3D case, this means that if the sensor is rotated, then the sensor reading of the field vector should appear rotated by the inverse rotation. This implies that a magnetometer reading from inside of the Rift allows us infer something about its orientation based on the sensed magnetic field vector.

The Earth’s Strange Magnetic Field

What magnetic field should we use to align the Rift? It is popular fiction that the Earth produces a magnetic field vector that points North, perfectly horizontally, from every point on the Earth. If you believe this, then you are in for three surprises. The first is that the field vector points upward or downward a significant amount, depending on the location. Here is a map of the so-called inclination angles:

EarthMagInclination

Here at Oculus Headquarters in Irvine, the field vector points 60 degrees into the Earth! Off the coast of Antarctica, the vector may point straight up. If the vector is close to parallel with gravity, then the magnetic field becomes useless as an additional source of information because the accelerometer already estimates alignment with respect to this direction (up or down).

The second surprise is that the vectors do not actually point North, resulting in a declination angle:

EarthMagDeclination

It is rumored that downtown Portland, Oregon has misaligned streets due to 19th century debates on magnetic versus true North. However, most people live in a place where the declination angle is within 10 degrees, which can go unnoticed. If you need accuracy to within a degree, for example to point a satellite dish using a compass, then it becomes important.

The final surprise is that the intensity varies substantially around the world:

EarthMagIntensity

In Paraguay, the field intensity is only around 0.22 Gauss, whereas in Central Asia it is over 0.6 Gauss— expect strong readings in Siberia! If we want to use a magnetometer directly as a high-precision compass, which calculates the exact direction of North, then we need to take these field variations into account.

This presents no trouble for us, however. We can consistently align the Rift without knowing which way is North. All we need is a vector field that is constant in our small work area while also having a substantial horizontal component (at least 0.1 Gauss, for example, when projected into the horizontal plane).

All the Other Magnetic Fields

If you can place your magnetometer in an isolated spot on the Earth, then the Earth’s magnetic field will dominate the sensor reading. If you are using it in the vicinity of other magnetic materials or electric circuits, then it will happily observe magnetic fields caused by these as well. Unfortunately, they are superimposed onto the Earth’s field, resulting in a jumble that is difficult to disentangle. This situation occurs for almost everyone using the Rift.

First consider materials inside the Rift, including the screen and other components on the sensor circuit board. In any coordinate frame attached to the Rift, the materials move together with the sensor. Although numerous materials may generate or interfere with magnetic fields, they are commonly grouped into two classes based on their magnetic properties: hard iron and soft iron [5]. These need not contain iron; for example, nickel and cobalt are ferromagnetic materials. Hard iron simply generates a magnetic field around it. This will combine additively with the Earth’s field, but note that it is vector addition. If placed correctly, it is even possible to completely cancel the Earth’s field and obtain zero. Soft iron cannot generate its own field, but it distorts any field around it in an orientation-dependent way.

The net effect of all hard iron in the Rift is to generate a constant magnetic field vector at the sensor. This means that a fixed offset is produced for each of the three magnetometer sensor axes. If you hold the magnetometer fixed in space and bring it through all possible orientations, we expect the observed values to lie on a sphere (ignoring noise). The sensor observes the same field vector, but from different relative directions. The distortion caused by soft iron generally warps the sphere into an ellipsoid [1, 4, 10].

Calibration: One Step Closer to a Compass

The first step toward making the magnetometer useful for yaw drift correction is to transform its output to compensate for the hard and soft-iron biases. We consider two cases: A crude method and a careful method. For the crude method, ignore soft-iron bias and focus only on determining the offset due to hard-iron bias. In this case, the magnetometer values should lie on a sphere. How many sensor readings are needed to find the sphere? Dropping down to 2D helps. A basic fact from planar geometry is that any three non-collinear points determines a unique circle [3]. This result extends to 3D (and higher!) to obtain that any four linearly independent points uniquely determines a sphere. Through several 4×4 matrix determinant calculations, the sphere center can be quickly found [8]. Once the center is found, its three coordinates are subtracted from the magnetometer output values.

Because only four magnetometer readings are needed, the center can be estimated automatically, without the user even being aware. Due to noise, the points need to be well-separated: Far from each other and far from being linearly dependent.

The crude method has the advantage of requiring minimal effort and disruption to the user; however, after much experimentation, it seems preferable to request the user to perform a more intensive, once-in-a-while task that gathers hundreds of magnetometer readings. In this case, we request the user to spin the Rift around through as many orientations as possible. An ellipsoid is fit to the data using least-squares minimization of the algebraic distance. (Some technical issues arise due to implicit surface fitting; these are covered in [7,9].)

The figure shows the effect of careful calibration:

magellipse

This plot is a 2D slice of the 3D data. Once the ellipsoid parameters are obtained, they are applied to offset, scale, and rotate the raw magnetometer readings. This causes all transformed readings to lie roughly on a sphere centered at the origin. The process helps to fix the direction of the sensed magnetic field vector; however, it does not recover the true intensity (length of the vector). This is not important for yaw drift correct and we transform the readings so that they approximately have unit intensity. Only the direction is critical.

Detecting Drift with Reference Points

BoatStar

Once we have a calibrated magnetometer, it is ready for use by our drift correction method. The next step is to save a reference point. For intuition, imagine you are sailing the seas on a ship and decide to measure the yaw (azimuth) angle between the direction that the ship is facing and your favorite star. If at any time you want to make sure that the ship is pointing in the same direction, then you can check the angle again. If the angle is wrong, you should turn the ship to correct it. I think of a reference point as a “virtual star”.

To store a reference point, we simply record the magnetometer reading together with the estimate of the Rift orientation at which it was taken.

Assume a right-handed coordinate system with “up” being the y axis. The following steps are used to detect yaw drift error:

  1. Take the reference point and transform the magnetometer into the fixed, world frame using the quaternion (Rift orientation) at which it was stored. This yields m_w = (m_x,m_y,m_z).
  2. Take the current magnetometer reading transform it into the world frame using the current Rift orientation. Let the result be denoted by n_w = (n_x,n_y,n_z).
  3. The two readings m_w and n_w are projected into the horizontal plane by setting their y components to 0.
  4. Convert the projected readings into angles as \theta = {\rm atan2}(m_z,m_x) and \theta' = {\rm atan2}(n_z,n_x). Here, {\rm atan2} denotes the 360-degree arctangent function, a standard C++ library function.

    MagAtan

  5. The yaw drift error is e = \theta - \theta'. Be careful to correctly handle the 2 \pi angle wraparound, to ensure that e always lies between -\pi and \pi.

What happens when a larger error is detected? There are many possibilities; however, one of the worst would be to immediately yaw the camera in the virtual world by -e because the user would notice and perhaps be unhappy. A better idea is to gradually apply the corrective rotations. We would like the corrections to be so slow that the user does not notice, while being fast enough so that they are sufficient to overcome the maximum drift rate during ordinary use. We determined the appropriate rate after careful analysis of experiments involving both humans using the Rift and a robot arm that systematically moves the sensor around. We apply the corrections using a complementary filter. In the simplest case, the filter would apply a tiny correction that is proportional to e in each iteration of our sensor fusion code, which operates at 1000Hz. In the current software, we apply a second-order complementary filter, which combines tilt correction, yaw correction, and gyro bias error estimation into a single, simple framework. We chose complementary filters for their simplicity. One could alternatively use Kalman filters, but they are equivalent at a basic level [2], and the Kalman filter model of a known system with Gaussian noise does not closely match the behavior of humans moving their heads around. The complementary filter provides a small number of simple constants that can be tuned based on perceptual experiments (can the human notice it being applied?) and mathematical proofs of convergence [6].

An important limitation remains. If the magnetometer calibration is not perfect, then yaw error could be incorrectly measured. The ellipsoid center or curvature (eccentricity) are subject to error. This problem becomes worse as the distance between the current orientation and the reference point orientation increases. To handle this problem, we impose a maximum angular limit on the angle between the orientation at which the reference point was saved and the orientation at which it is used. A typical value is 10 degrees. To enable drift correction when facing other directions, we simply generate more reference points. Think star charts! A table of reference points is constructed by adding a new one every time the orientation is far from all of the previous ones in the table. Only the nearest reference point is used for correction at a particular iteration. For improved robustness, we sometimes detect and remove bad reference points. This could happen, for example, if you set the Rift down on a desk (especially one containing hard iron!) for an hour and put it back on again without stopping your program. Our current SDK allows up to 1000 reference points, but the typical amount used in practice is 200 because it is difficult to cover all 3D rotations by turning your head. By using multiple reference points, we can even get reasonably good performance by using the crude four-point method to estimate a sphere center, instead of performing a careful ellipsoidal fit. This way, the magnetometer could be calibrated and used without the user knowing. In our latest software, we require that the magnetometer is carefully calibrated in advance using the profile tool.

Performance

We performed several experiments in which a user sits and plays a VR game for over twenty minutes. To provide “ground truth” against which to compare our yaw correction methods, we measured their head orientations using an OptiTrack motion capture system. The average amount of yaw error was around 3.7 degrees, while the worst-case performance was within 10 degrees. Several performance plots are shown below.

yaw1

yaw3

yaw2

Although we are satisfied with performance, a reasonable future goal is to keep the error to within one degree. Some unavoidable problems also remain. If the magnetic field, when measured indoors, is nearly aligned with gravity or has nearly zero magnitude, then the magnetometer cannot provide useful readings for yaw correction. (Although this happens outdoors only in exotic locations on Earth, it could occur indoors due to iron reinforcements.) Trouble is also caused by fields that vary highly by position or over time.

Conclusions

Through calibration and the use of reference points, we were able to get a magnetometer to perform almost as well as using a compass in the woods. When performing yaw drift correction, the particular magnetic field intensity and the direction of absolute North are not important. What remains important is to provide a reference direction that is fixed in the physical world. Our methods use the direction of a nearby reference point to gradually correct the yaw drift error. The result is consistent maintenance of “forward”, rather than having a cockpit drift to the side. We have obtained an effective solution by carefully sidestepping the fundamental limitations of passive magnetic field sensing.

Acknowledgments

Thanks to Lee Cooper, Max Katsev, Brant Lewis, and Anna Yershova for their contributions to the ideas, implementations, and experiments described in this post.

References

[1] Gebre-Egziabher, D., Elkaim, G., Powell, J., and Parkinson, B., Calibration of Strapdown Magnetometers in Magnetic Field Domain. ASCE Journal of Aerospace Engineering, Vol. 19, No. 2, Pages 1–16, April 2006.

[2] Higgins, W. T., A Comparison of Complementary and Kalman Filtering. IEEE Transactions on Aerospace and Electronic Systems, Vol. 11, No. 3, Pages 321-325, May 1975.

[3] Khan, Sal, Three Points Defining a Circle. Khan Academy online video. Oct 13, 2011.

[4] Konvalin, C., Compensating for Tilt, Hard-Iron and Soft-Iron Effects. Sensors. December 1, 2009.

[5] Macintyre, S. A., Magnetic Field Measurement, Chapter 48, The Measurement, Instrumentation, and Sensors Handbook, J. G. Webster, Editor, CRC Press, 1999.

[6] Mahoney, R., Hamel, T., and Pflimlin, J.-L., Nonlinear Complementary Filters on the Special Orthogonal Group. In Proceedings IEEE Conference on Decision and Control, 2008.

[7] Markovsky, I., Kukush, A., and Van Huffel, S., Consistent Least Squares Fitting of Ellipsoids. Numerische Mathematik, Vol. 98, No. 1, Pages 177-195, 2004.

[8] Schmitt, S. R., Center and Radius of a Sphere from Four Points. Chegg-Study, Free Textbook Solutions, 2013.

[9] Taubin, G., Cukierman, F., Sullivan, S., Ponce, J., and Kriegman, D. J., Parameterized Families of Polynomials for Bounded Algebraic Curve and Surface Fitting. IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 16, No. 3, March 1994.

[10] Vasconcelos, J. F., Elkaim, G., Silvestre, C., Oliveira, P., Cardeira, B., A Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame. IEEE Transactions on Aerospace and Electronic Systems, Vol. 47, No. 2, Pages 1293-1306, April 2011.


Like This Article?

Loading…