System and method for hybrid optical/inertial headtracking via numerically stable Kalman filter

• US 10,216,265 B1
• Filed: 08/07/2017
• Issued: 02/26/2019
• Est. Priority Date: 08/07/2017
• Status: Active Grant

• Current Assignee: Rockwell Collins, Inc. (Raytheon Technologies Corporation d/b/a RTX)
• Original Assignee: Rockwell Collins, Inc. (Raytheon Technologies Corporation d/b/a RTX)
• Inventors: Kirchner, William T., Foote, Bobby D., George, Christopher L., Sattigeri, Ramachandra J.
• Primary Examiner(s): Cunningham, Gregory F
• Application Number:
  US15/670,934
• Publication Number:
  US 20190041979A1
• Time in Patent Office: 568 Days
• Field of Search: 382103
• CPC Class Codes: B64D 43/00 Arrangements or adaptations of instruments arrangements of cameras B64D47/08; aeronautical measuring instruments per se G01C
• CPC Class Codes: G01C 21/1654 with electromagnetic compass
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G01P 15/08 with conversion into electric or magnetic values
• CPC Class Codes: G01P 15/105 by magnetically sensitive devices
• CPC Class Codes: G02B 2027/0187 slaved to motion of at least a part of the body of the user, e.g. head, eye
• CPC Class Codes: G02B 27/0093 with means for monitoring data relating to the user, e.g. head-tracking, eye-tracking
• CPC Class Codes: G06F 3/012 Head tracking input arrangements
• CPC Class Codes: G06F 3/0325 using a plurality of light emitters or reflectors or a plurality of detectors forming a reference frame from which to derive the orientation of the object, e.g. by triangulation or on the basis of reference deformation in the picked up image
• CPC Class Codes: G06T 2207/10016 Video; Image sequence
• CPC Class Codes: G06T 2207/30204 Marker
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80

Abstract

A system and related method for hybrid headtracking receives head-referenced pose data from a head-mounted IMU and platform-referenced or georeferenced position and orientation (pose) data from a platform-mounted IMU, determines error models corresponding to uncertainties associated with both IMUs, and performs an initial estimate of head pose relative to the platform reference frame based on the head-referenced and platform-referenced pose data. A numerically stable UD factorization of the Kalman filter propagates the estimated head pose data forward in time and corrects the initial head pose estimate (and may correct the error models associated with the head-mounted and platform-mounted IMUs) based on secondary head pose estimates, and corresponding error models, received from an optical or magnetic aiding device. The corrected head pose data is forward to a head-worn display to ensure high accuracy of displayed imagery and symbology.

Claims

1. A hybrid headtracking system incorporating a numerically stable Kalman fitter, comprising:

at least one platform-referenced inertial measurement unit (IMU) configured to be rigidly mounted to a mobile platform associated with a platform reference frame, the at least one platform-referenced IMU configured to provide platform-referenced position and orientation (pose) data;

at least one head-referenced IMU configured to be rigidly mounted to a head of a user, the head associated with a head reference frame, the at least one head-referenced IMU configured to provide head-referenced pose data;

at least one aiding device configured to be rigidly mounted to at least one of the mobile platform and the head and to provide first estimated pose data, the at least one aiding device including at least one aiding sensor configured to generate the first estimated pose data based on one or more fiducial markers;

and

a controller configured to be coupled to the platform-referenced IMU, the head-referenced IMU, and the at least one aiding device, the controller including at least one processor configured to;

receive the head-referenced pose data, the platform-referenced pose data, and the first estimated pose data;

determine at least one of a first error model associated with the head-referenced pose data, a second error model associated with the platform-referenced pose data, and a third error model associated with the first estimated pose data;

generate second estimated pose data based on the head-referenced pose data and the platform-referenced pose data;

generate, via the numerically stable Kalman filter, time-propagated pose data based on the second estimated pose data and one or more of the first error model and the second error model;

and

generate, via the numerically stable Kalman fitter, at least one of corrected pose data and a corrected error model based on the time-propagated pose data, the first estimated pose data, and the at least one third error model.

2. The hybrid headtracking system of claim 1, wherein:

the at least one head-referenced IMU includes at least one microelectromechanical (MEMS) IMU.

3. The hybrid headtracking system of claim 1, wherein:

the at least one platform-referenced IMU is coupled to a georeferenced position sensor configured to generate georeferenced pose data corresponding to the mobile platform;

and

the platform-referenced pose data includes the georeferenced pose data.

4. The hybrid headtracking system of claim 1, wherein the at least one aiding device is an optical tracking system and:

the at least one aiding sensor includes a camera;

and

the first estimated pose data includes camera calibration data associated with the camera.

5. The hybrid headtracking system of claim 4, wherein the first estimated pose data is associated with at least one of:

a location of the one or more fiducial markers relative to an image generated by the camera;

and

one or more coordinates corresponding to the one or more fiducial markers.

6. The hybrid headtracking system of claim 1, wherein the at least one aiding device is a magnetic tracking system and:

the at least one aiding sensor includes a magnetometer;

and

the one or more fiducial markers include at least one magnetic source.

7. The hybrid headtracking system of claim 1, wherein the controller is configured to correct, via the numerically stable Kalman filter, at least one of the first error model and the second error model based on at least the first estimated pose data.

8. A head-worn display (HWD) incorporating a hybrid headtracking system, comprising:

a controller including at least one control processor configured to;

receive platform-referenced position and orientation (pose) data from at least one platform-referenced IMU rigidly mounted to a mobile platform and associated with a platform reference frame;

receive head-referenced pose data from at least one head-referenced IMU rigidly mounted to a head of a user, the head associated with a head reference frame;

determine at least one first error model associated with the platform-referenced pose data;

determine at least one second error model associated with the head-referenced pose data;

generate first estimated pose data based on the head-referenced pose data and the platform-referenced pose data;

receive second estimated pose data from at least one aiding device rigidly mounted to one or more of the head and the mobile platform, the aiding device including at least one aiding sensor configured to generate the second estimated pose data based on one or more fiducial markers;

implement at least one numerically stable Kalman fitter configured to;

generate time-propagated pose data based on the first estimated pose data and one or more of the first error model and the second error model;

and

generate at least one of corrected pose data and a corrected error model based on the time-propagated pose data, the second estimated pose data, and the at least one third error model;

and

at least one display unit configured to be mounted to the head of the user, the at least one display unit comprising;

at least one display processor configured to receive the corrected pose data from the control processor;

and

at least one display surface configured to display one or more images to the user based on the received corrected pose data.

9. The HWD of claim 8, wherein one or more of the controller, the head-referenced IMU, the at least one aiding device, and the at least one display unit is rigidly mounted to a helmet wearable by the user.

10. The HWD of claim 8, wherein:

the at least one platform-referenced IMU is coupled to at least one georeferenced position sensor configured to generate georeferenced pose data corresponding to the mobile platform;

and

the platform-referenced pose data includes the georeferenced pose data.

11. The HWD of claim 8, wherein the at least one aiding device is an optical tracking system and:

the at least one aiding sensor includes a camera;

and

the second estimated pose data includes camera calibration data associated with the camera.

12. The HWD of claim 11, wherein the second estimated pose data is associated with at least one of:

a location of the one or more fiducial markers relative to an image generated by the camera;

and

one or more coordinates corresponding to the one or more fiducial markers.

13. The HWD of claim 8, wherein the at Least one aiding device is a magnetic tracking system and:

the at least one aiding sensor includes a magnetometer;

and

the one or more fiducial markers include at least one magnetic source.

14. A method for hybrid headtracking via a numerically stable Kalman filter, the method comprising:

receiving, via a controller coupled to a head worn display (HWD), platform-referenced position and orientation (pose) data from at least one platform-referenced inertial measurement unit (IMU) rigidly mounted to a mobile platform associated with a platform reference frame;

receiving, via the controller, head-referenced pose data from at least one head-referenced IMU rigidly mounted to a head of a user;

determining, via the controller, at least one first error model associated with the platform-referenced pose data;

determining, via the controller, at least one second error model associated with the head-referenced pose data;

generating, via the controller, first estimated pose data based on the platform-referenced pose data and the head-referenced pose data, the first estimated pose data corresponding to the head and relative to the platform reference frame;

generating, via a Kalman filter associated with the controller, time-propagated pose data based on the first estimated pose data, the first error model, and the second error model;

receiving, via the controller, second estimated pose data from at least one aiding device coupled to the controller, the aiding device rigidly mounted to at least one of the head and the mobile platform;

determining, via the controller, at least one third error model associated with the second estimated pose data;

and

generating, via the Kalman filter, at least one of corrected pose data and a corrected error model based on the time-propagated pose data, the second estimated pose data, and the third error model.

15. The method of claim 14, wherein receiving, via a controller coupled to a head worn display (HWD), platform-referenced position and orientation (pose) data from at least one platform-referenced inertial measurement unit (IMU) rigidly mounted to a mobile platform associated with a platform reference frame includes:

receiving, via a controller coupled to a HWD, platform-referenced pose data from at least one GNSS-aided platform-referenced IMU rigidly mounted to a mobile platform associated with a platform reference frame, the platform-referenced pose data including georeferenced pose data corresponding to the mobile platform.

16. The method of claim 14, wherein receiving, via the controller, second estimated pose data from at least one aiding device coupled to the controller, the aiding device rigidly mounted to at least one of the head and the mobile platform includes:

receiving, via the controller, second estimated pose data from at least one optical tracker coupled to the controller, the optical tracker comprising a camera and one or more fiducial markers, the second estimated pose data including calibration data associated with the camera.

17. The method of claim 16, wherein receiving, via the controller, second estimated pose data from at least one optical tracker coupled to the controller, the optical tracker comprising a camera and one or more fiducial markers, the second estimated pose data including calibration data associated with the camera includes:

receiving, via the controller, second estimated pose data from at least one optical tracker coupled to the controller, the optical tracker comprising a camera and one or more fiducial markers, the second estimated pose data including at least one of a) a location of the one or more fiducial markers relative to an image captured by the camera and b) a coordinate corresponding to the one or more fiducial markers.

18. The method of claim 14, wherein receiving, via the controller, second estimated pose data from at Least one aiding device coupled to the controller, the aiding device rigidly mounted to at Least one of the head and the mobile platform includes:

receiving, via the controller, second estimated pose data from at least one magnetic tracker coupled to the controller, the magnetic tracker comprising a magnetometer and one or more magnetic sources.

19. The method of claim 14, wherein generating, via the Kalman filter, at least one of corrected pose data and a corrected error model based on the time-propagated pose data, the second estimated pose data, and the third error model includes:

correcting, via the Kalman filter, at least one of the first error model and the second error model based on at least the second estimated pose data.

20. The method of claim 14, further comprising:

forwarding the corrected pose data to the HWD;

and

displaying to the user, via the HWD, one or more images based on the corrected pose data.