Plug-and-play Measurement Fusion Method for Integrated Navigation System using Low-cost Nonlinear Optimization

This paper addresses the problem of plug and play measurement fusion for integrated navigation system using nonlinear optimization method. We refer to multi-sensor fusion estimation as a sequential, weighted least squares problem. Using fluid re-linearization and partial state updates strategies to reduce computational burden, we propose a low cost nonlinear optimization algorithm. This algorithm not only processes multi rate and asynchronous sensor data, but also provides a natural way to incorporate a new sensor or an ad hoc signal into the system. The proposed multi-sensor fusion estimation algorithm has been successfully implemented in integrated navigation with inertial measurement unit, global position system and star sensor.