Compact portable navigation system for GPS-challenged regions

A method of determining a location and orientation within an environment of an apparatus that comprises a controller, a camera, and an associated range sensor, referred to herein as the “position” of the apparatus, the method comprising:

• A) determining an initial position estimate of the apparatus;
• B) determining by the controller of a 3D geo-located frustum representing an anticipated field of view of the camera, wherein the 3D geo-located frustum is based on the initial position and a three-dimensional geo-located model (3D model) of the environment of the camera;
• C) causing, by the controller, the camera to obtain a camera image of the camera's field of view;
• D) generating, by the controller, a synthetic image of the anticipated field of view of the camera, wherein the synthetic image is a prediction of the camera image according to the initial position estimate of the apparatus and the frustum;
• E) registering, by the controller, the synthetic image of the anticipated field of view with the camera image, thereby determining an estimate of a “cross-plane” position of the apparatus in a plane perpendicular to a pointing direction of the camera, and a first estimate of a “downrange” position of the apparatus in the pointing direction of the camera, wherein said registering comprises:
• a) estimating a geometric transformation between the synthetic image of the anticipated field of view and the camera image of the camera's field of view that optimizes their mutual alignment;
• b) revising, by the controller, the position estimate of the apparatus according to the estimated geometric transformation;
• c) adjusting, by the controller, the synthetic image according to the revised position estimate;
• and
• d) repeating steps a) through c) until a correspondence between the camera image and the synthetic image is optimized;
• F) causing, by the controller, the range sensor to obtain at least one downrange measurement in the pointing direction of the camera;
• G) determining, by the controller, a second downrange estimate using the first downrange estimate and the at least one downrange measurement by minimizing a cost function defined on range error values, the second downrange estimate being more accurate than the first downrange estimate;
• and
• H) causing by the controller, according to the cross-plane and second downrange estimates, at least one of:
• navigating to a new position;
• adjusting a location and orientation of a device;
• and
• presenting the cross-plane and refined downrange estimates to an operator of the apparatus.