Patent ID: 11874116
Assignee: TRIMBLE INC.
Field: Measurement (Instruments)
Classification: CPC G | IPC G

Claim 12:
13. A method of navigation comprising:
measuring, using an inertial measurement unit (IMU) attached to a dynamic platform, a specific force vector and an angular rate vector of the dynamic platform;
determining a current inertial navigation system (INS) solution of a position and an orientation of the dynamic platform based on at least: (i) a previous INS solution, and (ii) the specific force vector and the angular rate vector measured by the IMU;
acquiring, using a range image sensor (RI sensor) attached to the dynamic platform, range image data (RI data);
performing an a priori transformation of the RI data using the current INS solution to obtain transformed RI data;
retrieving, from an RI map database, a valid keyframe map among a plurality of keyframe maps based on the transformed RI data, the valid keyframe map having a known position and a known orientation in a navigation coordinate frame;
constructing a map registration cost gradient (MRCG) measurement based on at least (i) the transformed RI data, and (ii) the known position and the known orientation of the valid keyframe map;
determining INS error corrections and INS solution statistics based on the MRCG measurement;
updating the current INS solution based on the INS error corrections; and
determining an absolute INS solution of a current position and a current orientation of the dynamic platform based on the current INS solution and the INS solution statistics.