Extended kalman filter for 3D localization and vision-aided inertial navigation

Patent metadata

Field	Value
Publication number	US-10670404-B2
Application number	US-201715706149-A
Country	US
Kind code	B2
Filing date	Sep 15, 2017
Priority date	Mar 28, 2008
Publication date	Jun 2, 2020
Grant date	Jun 2, 2020

Abstract

Localization and navigation systems and techniques are described. An electronic device comprises a processor configured to apply an extended Kalman filter (EKF) as the electronic device traverses the trajectory. The extended Kalman filter is configured to maintain a state vector storing estimates for a position of the electronic device at poses along a trajectory within an environment along with estimates for positions for one or more features within the environment. The EKF computes constraints based on features observed from multiple poses along the trajectory, and updates, in accordance with motion data and the one or more computed constraints, the estimates within the state vector of the extended Kalman filter while excluding, from the state vector, state estimates for positions within the environment for the features that were observed from the multiple poses and for which the constraints were computed.

First claim

What is claimed is:

1. An electronic device comprising:
at least one image source to produce image data along a trajectory of the electronic device within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the electronic device along the trajectory;
an inertial measurement unit to produce motion data indicative of motion of the electronic device; and
a processor configured to apply an extended Kalman filter (EKF) as the electronic device traverses the trajectory,
wherein the processor is configured to:
maintain a filter state vector storing estimates for a position of the electronic device at each of the poses along the trajectory and estimates for positions for one or more features within the environment,
compute, from the image data, one or more constraints based on features observed from multiple poses along the trajectory, and
update, in accordance with the motion data and the one or more computed constraints, the estimates within the filter state vector of the extended Kalman filter while excluding, from the filter state vector, state estimates for positions within the environment for the features that were observed from the multiple poses and for which the one or more constraints were computed.

2. The electronic device of claim 1 , wherein the processor is configured to store, in the filter state vector, estimates for an orientation of the electronic device at each of the poses along the trajectory and to update the estimates in accordance with the motion data and the one or more computed constraints.

3. The electronic device of claim 1 , wherein the processor is configured to generate each of the one or more constraints by computing a residual of a measurement for the respective feature.

4. The electronic device of claim 1 , wherein the image source includes a camera.

5. The electronic device of claim 1 further including an output device coupled to the processor, the output device including at least one of a memory, a transmitter, a display, a printer, an actuator, and a controller.

6. The electronic device of claim 1 wherein the processor computes the one or more of the constraints for features observed at three or more of the poses along the trajectory.

7. The electronic device of claim 1 further comprising:
a memory coupled to the processor; and
a display.

8. The electronic device of claim 7 , wherein the processor is configured to provide output to the display based on the estimates for the position of the electronic device.

9. A method comprising:
receiving image data containing a plurality of features observed within an environment at a plurality of poses of an electronic device;
receiving motion data indicative of motion of the electronic device;
computing, with a processor, state estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory;
generating, with the processor, a filter state vector for an extended Kalman filter to include an estimate for a position of the electronic device at one or more of the poses along the trajectory and an estimate for a position for one or more features within the environment,
computing, with the processor and according to the image data, one or more constraints based on at least one feature observed from multiple poses along the trajectory, and
updating, in accordance with the motion data and the one or more computed constraints, the estimates within the filter state vector while excluding, from the filter state vector, an estimate for a position within the environment for the at least one feature that was observed from the multiple poses and for which the one or more constraints were computed.

10. The method of claim 9 , wherein computing one or more constraints comprises manipulating a residual of a feature measurement determined for the at least one feature to reduce feature estimate error.

11. The method of claim 9 further comprising:
generating the filter state vector to include an estimate for an orientation of the electronic device at the one or more poses along the trajectory; and
updating the estimate for the orientation in accordance with the motion data and the one or more computed constraints.

12. The method of claim 9 , wherein receiving image data includes receiving data captured by at least one of a camera-based sensor, a laser-based sensor, a sonar-based sensor, and a radar-based sensor.

13. The method of claim 9 , wherein receiving motion data comprises receiving data generated from at least one of a wheel encoder, a velocity sensor, a Doppler radar based sensor, a gyroscope, an accelerometer, an airspeed sensor, and a global positioning system (GPS) sensor.

14. The method of claim 9 , further comprising generating output for a display based on the estimates for the position of the electronic device.

15. A non-transitory computer-readable storage medium comprising instructions that configure a processor to:
receive image data containing a plurality of features observed within an environment at a plurality of poses of an electronic device;
receive motion data indicative of motion of the electronic device;
compute, with a processor, state estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory;
generate, with the processor, a filter state vector for an extended Kalman filter to include an estimate for a position of the electronic device at one or more of the poses along the trajectory and an estimate for a position for one or more features within the environment,
compute, with the processor and according to the image data, one or more constraints based on at least one feature observed from multiple poses along the trajectory, and
update, in accordance with the motion data and the one or more computed constraints, the estimates within the filter state vector while excluding, from the filter state vector, an estimate for a position within the environment for the at least one feature that was observed from the multiple poses and for which the one or more constraints were computed.

16. The computer-readable storage medium of claim 15 , wherein the instructions configure the processor to compute the one or more constraints by manipulating a residual of a measurement for the at least one feature to reduce an effect of a feature estimate error.

17. The computer-readable storage medium of claim 15 , wherein the instructions configure the processor to:
generate the filter state vector to include an estimate for an orientation of the electronic device at the one or more poses along the trajectory; and
update the estimate for the orientation in accordance with the motion data and the one or more computed constraints.

18. The computer-readable storage medium of claim 15 , wherein the instructions configure the processor to generate, based on the estimates for the position of the electronic device, output for display.

Assignees

• Univ Minnesota

Inventors

• Roumeliotis Stergios I
• Mourikis Anastasios I

Classifications

• G01C21/16 Primary

  by integrating acceleration or speed, i.e. inertial navigation · CPC title

• G01C21/1656 Primary

  with passive imaging devices, e.g. cameras · CPC title

• G01C21/165

  combined with non-inertial navigation instruments · CPC title

• H04W4/027

  using movement velocity, acceleration information · CPC title

• G05D1/00

  Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots (drive control systems specially adapted for autonomous road vehicles B60W60/00) · CPC title