Simultaneous Localization and mapping in sensor networks: A GES sensor-based filter with moving object tracking

This paper presents the design, analysis, and validation of a globally exponentially stable (GES) filter for tridimensional (3-D) range-only simultaneous localization and mapping in sensor networks with moving object tracking. For observability analysis purposes, two nonlinear sensor-based dynamic systems are formulated resorting only to exact linear and angular kinematics and a motion model for the target. A state augmentation is exploited that allows the proposed formulation to be considered as linear time-varying without linearizing the original nonlinear systems. Constructive observability results can then be established, leading naturally to the design of a Kalman Filter with GES error dynamics. These results also provide valuable insight on the motion planning of the vehicle. Simulation results demonstrate the good performance of the algorithm and help validate the theoretical results, as well as illustrate the necessity of a proper trajectory.