Efficient orientation estimation system using magnetic, angular rate, and gravity sensors
A system that efficiently estimates an object's orientation using magnetic, angular rate, and gravity sensors. The object may be for example a virtual reality headset or a user in a virtual reality environment. Magnetic and gravity data are used to correct errors that accumulate from integrating angular velocity. Unlike systems that use Kalman filter approaches, embodiments of the system apply a simple, highly efficient technique to generate magnetic and gravity error vectors; these error vectors are added directly to the angular velocity prior to integration. Error calculations are performed in the sensor reference frame rather than in the Earth reference frame. Magnetic error correction uses only the horizontal component of the magnetic field, which is efficiently calculated by subtracting off the projection of the magnetic field onto the measured gravity vector. Sensors and processors for calculating orientation may be integrated into a low-latency virtual reality display system.
1. An efficient orientation estimation system using magnetic, angular rate, and gravity sensors, comprising:
a magnetometer configured to be attached to an object, wherein said magnetometer generates magnetic field data;
a rate gyroscope configured to be attached to said object, wherein said rate gyroscope generates angular velocity data;
an accelerometer configured to be attached to said object, wherein said accelerometer generates acceleration data; and,
at least one processor comprising a tangible memory medium, coupled to said accelerometer, to said rate gyroscope, and to said magnetometer, and configured to
define a sensor orientation as a rotation that rotates a vector in a sensor reference frame into a corresponding vector in an Earth reference frame; and,
calculate an updated value of said sensor orientation as
obtain said angular velocity data from said rate gyroscope;
obtain said acceleration data from said accelerometer;
obtain said magnetic field data from said magnetometer;
calculate a gravity prediction in said sensor reference frame;
calculate a horizontal magnetic field prediction in said sensor reference frame;
decompose said magnetic field data into a vertical magnetic component and a horizontal magnetic component;
calculate a gravity error based on said gravity prediction and said acceleration data;
calculate a magnetic field error based on said horizontal magnetic field prediction and said horizontal magnetic component;
calculate an orientation derivative based on
said angular velocity,
said gravity error, and
said magnetic field error; and,
integrate said orientation derivative to form said updated value of said sensor orientation.
2. The system of claim 1 , wherein
said sensor orientation is a sensor orientation quaternion having unit length.
3. The system of claim 2 , wherein
said calculate an orientation derivative comprises
calculate said orientation derivative as one half of said sensor orientation quaternion multiplied by a sum of
said angular velocity,
said gravity error multiplied by a gravity error proportional gain, and,
said magnetic field error multiplied by a magnetic field proportional gain.
4. The system of claim 2 , wherein said calculate an orientation derivative comprises
calculate said orientation derivative as one half of said sensor orientation quaternion multiplied by a sum of
said angular velocity,
said gravity error multiplied by a gravity error proportional gain,
said magnetic field error multiplied by a magnetic field proportional gain,
a cumulative sum of said gravity error multiplied by a gravity error integral gain, and,
a cumulative sum of said magnetic field error multiplied by a magnetic field error integral gain.
5. The system of claim 2 , wherein said calculate an orientation derivative comprises
calculate said orientation derivative as one half of said sensor orientation quaternion multiplied by a sum of
said angular velocity,
said gravity error multiplied by a gravity error proportional gain,
said magnetic field error multiplied by a magnetic field proportional gain,
a cumulative sum of said gravity error multiplied by a gravity error integral gain,
a cumulative sum of said magnetic field error multiplied by a magnetic field error integral gain,
a derivative of said gravity error multiplied by a gravity error derivative gain, and,
a derivative of said magnetic field error multiplied by a magnetic field error derivative gain.
6. The system of claim 1 , further comprising
at least one display viewable by a user, wherein said object is one or more body parts of said user;
a 3D model of a scene; and,
a scene renderer coupled to said at least one display, said at least one processor, and said 3D model, wherein said scene renderer
receives said sensor orientation from said at least one processor;
calculates one or more rendering virtual camera poses, based on said sensor orientation;
calculates one or more 2D projections of said 3D model, based on said one or more rendering virtual camera poses; and,
transmits said one or more 2D projections to said at least one display.
7. The system of claim 6 , further comprising
an image warper coupled to said at least one display, said scene renderer, and said at least one processor, wherein said image warper
receives said one or more rendering virtual camera poses from said scene renderer;
receives said sensor orientation from said at least one processor;
calculates a change in pose between said one or more rendering virtual camera poses and said sensor orientation;
generates a rerendering approximation of said one or more 2D projections on said at least one display based on said change in pose; and,
modifies one or more pixels of said at least one display based on said rerendering approximation.
8. The system of claim 7 , wherein said rerendering approximation comprises calculating a pixel translation vector; and,
translating one or more pixels of said one or more 2D projections by said pixel translation vector.
9. The system of claim 8 , wherein calculating said pixel translation vector comprises
approximating said change in pose as a rotation around a unit vector {circumflex over (ω)} comprising {circumflex over (ω)} y and {circumflex over (ω)} x by an angle Δθ;
calculating a spatial translation vector ({circumflex over (ω)} y Δθ, −{circumflex over (ω)} x Δθ);
calculating a scaling factor to convert spatial distances to pixels based on pixel dimensions and fields of view of said one or more 2D projections; and,
calculating said pixel translation vector by scaling said spatial translation vector by said scaling factor.
10. The system of claim 7 , wherein said image warper modifies one or more pixels of said at least one display using hardware acceleration.
11. The system of claim 7 , wherein said image warper
compares said change in pose to a threshold value; and,
generates said rerendering approximation if said change in pose is below said threshold value.
12. An efficient orientation estimation system using magnetic, angular rate, and gravity sensors, comprising:
a magnetometer configured to be attached to one or more body parts of a user, wherein said magnetometer generates magnetic field data;
a rate gyroscope configured to be attached to said one or more body parts of said user, wherein said rate gyroscope generates angular velocity data;
an accelerometer configured to be attached to said one or more body parts of said user, wherein said accelerometer generates acceleration data;
at least one processor comprising a tangible memory medium, coupled to said accelerometer, to said rate gyroscope, and to said magnetometer, and configured to
define a sensor orientation quaternion, which rotates a vector in a sensor reference frame into a corresponding vector in an Earth reference frame;
calculate an updated value of said sensor orientation quaternion as
obtain said angular velocity data from said rate gyroscope;
obtain said acceleration data from said accelerometer;
obtain said magnetic field data from said magnetometer;
calculate a gravity prediction in said sensor reference frame;
calculate a horizontal magnetic field prediction in said sensor reference frame;
decompose said magnetic field data into a vertical magnetic component and a horizontal magnetic component;
calculate a gravity error based on said gravity prediction and said acceleration data;
calculate a magnetic field error based on said horizontal magnetic field prediction and said horizontal magnetic component;
calculate an orientation derivative as one half of said sensor orientation quaternion multiplied by a sum of
said angular velocity,
and one or more of
said gravity error multiplied by a gravity error proportional gain,
said magnetic field error multiplied by a magnetic field proportional gain,
a cumulative sum of said gravity error multiplied by a gravity error integral gain,
a cumulative sum of said magnetic field error multiplied by a magnetic field error integral gain,
a derivative of said gravity error multiplied by a gravity error derivative gain, and,
a derivative of said magnetic field error multiplied by a magnetic field error derivative gain; and,
integrate said orientation derivative to form said updated value of said sensor orientation quaternion;
at least one display viewable by said user;
a 3D model of a scene;
a scene renderer coupled to said at least one display, said at least one processor, and said 3D model, wherein said scene renderer
receives said sensor orientation quaternion from said at least one processor;
calculates one or more rendering virtual camera poses, based on said sensor orientation quaternion;
calculates one or more 2D projections of said 3D model, based on said one or more rendering virtual camera poses; and,
transmits said one or more 2D projections to said at least one display; and,
an image warper coupled to said at least one display, said scene renderer, and said at least one processor, wherein said image warper
receives said one or more rendering virtual camera poses from said scene renderer;
receives said sensor orientation quaternion from said at least one processor;
calculates a change in pose between said one or more rendering virtual camera poses and said sensor orientation quaternion;
generates a rerendering approximation of said one or more 2D projections on said at least one display based on said change in pose; and,
modifies one or more pixels of said at least one display based on said rerendering approximation;
wherein said rerendering approximation comprises
approximating said change in pose as a rotation around a unit vector {circumflex over (ω)} comprising {circumflex over (ω)} y and {circumflex over (ω)} x by an angle Δθ;
calculating a spatial translation vector ({circumflex over (ω)} y Δθ, −{circumflex over (ω)} x Δθ);
calculating a scaling factor to convert spatial distances to pixels based on pixel dimensions and fields of view of said one or more 2D projections;
calculating a pixel translation vector by scaling said spatial translation vector by said scaling factor; and,
translating one or more pixels of said one or more 2D projections by said pixel translation vector.