Fusion of sensor and map data using constraint based optimization

A computer readable storage medium comprising instructions that, when executed on a computing system to simultaneously track a tracked device and determine a location of features, cause the computing system to at least:

• obtain sensor data from the tracked device, the sensor data comprising location data and dead reckoning sensor data;
• determine dead reckoning path data based on the sensor data;
• detect one or more features based on the sensor data
• detect a location for each feature based on the dead reckoning path data;
• determine at least one constraint based on the sensor data;
• and
• optimize the dead reckoning path data and update the location of each feature using a convex simultaneous location and mapping algorithm comprising instructions that further cause the computing system to at least:
• define a convex objective function for the dead reckoning path data;
• minimize the convex objective function based on the at least one constraint;
• apply the convex objective function to update the dead reckoning path data;
• and
• update the location of each feature based on the updated dead reckoning path data.

A computing system for simultaneously tracking a tracked device and determining a location of features comprising:

• a processor;
• a memory communicatively coupled to the processor, the memory bearing instructions that, when executed on the processor, cause the computing system to at least:
• obtain sensor data from the tracked device, the sensor data comprising location data and dead reckoning sensor data;
• determine dead reckoning path data based on the sensor data;
• detect one or more features based on the sensor data;
• detect a location for each feature based on the dead reckoning path data;
• determine at least one constraint based on the sensor data;
• and
• optimize the dead reckoning path data and update the location of each feature using a convex simultaneous location and mapping algorithm comprising instructions that further cause the computing system to:
• define a convex objective function for the dead reckoning path data;
• minimize the convex objective function based on the at least one constraint;
• apply the convex objective function to update the dead reckoning path data;
• and
• update the location of each feature based on the updated dead reckoning path data.