Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system

A method for operating a real-time vision-aided inertial navigation system (VINS), comprising:

• receiving image data for a plurality of poses of a frame of reference along a trajectory in an environment over a period of time;
• receiving IMU measurements for the frame of reference over the period of time;
• and
• computing a state vector corresponding to a plurality of poses of the frame of reference along the trajectory, wherein, computing the state vector comprises:
• for features observed at one or more poses, of the plurality of poses along the trajectory, computing one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed;
• and
• updating the state vector, wherein:
• the state vector comprises:
• a sliding window of predicted state estimates for the VINS at each of the plurality of poses along the trajectory, and
• uncertainty data corresponding to the predicted state estimates, wherein the uncertainty data comprises uncertainty estimates, corresponding to each of the predicted state estimates, and maintained as a Cholesky factor of a covariance matrix;
• and
• updating the state vector comprises updating:
• the predicted state estimates according to the IMU measurements, the image data, and the one or more constraints;
• the Cholesky factor of the covariance matrix according to the updated predicted state estimates;
• and
• the state vector based, at least in part, on the updated Cholesky factor of the covariance matrix.

A non-transitory, computer-readable medium comprising instructions that, when executed, are configured to cause a processor of a vision-aided inertial navigation system (VINS) to:

• receive image data for a plurality of poses of a frame of reference along a trajectory in an environment over a period of time;
• receive IMU measurements for the frame of reference over the period of time;
• and
• compute a state vector corresponding to a plurality of poses of the frame of reference along the trajectory, wherein, computing the state vector comprises:
• for features observed at one or more poses, of the plurality of poses along the trajectory, compute one or more constraints that geometrically relate to the one or more poses from which the corresponding features were observed;
• and
• update the state vector, wherein:
• the state vector comprises:
• a sliding window of predicted state estimates for the VINS at each of the plurality of poses along the trajectory, and
• uncertainty data corresponding to the predicted state estimates, wherein the uncertainty data comprises uncertainty estimates, corresponding to each of the predicted state estimates, and maintained as a Cholesky factor of a covariance matrix;
• and
• updating the state vector comprises updating:
• the predicted state estimates according to the IMU measurements, the image data, and the one or more constraints;
• the Cholesky factor of the covariance matrix according to the updated predicted state estimates;
• and
• the state vector based, at least in part, on the updated Cholesky factor of the covariance matrix.