Sensor fusion for planar pose estimation using Immersion and Invariance

We propose a novel approach to sensor fusion problems based on the Immersion and Invariance (I&I) methodology. The problem of fusing different information sources is considered as estimating the states of a dynamic system (equivalent to the observation problem) which is solved following the I&I technique. As an example, we provide simulation results where we derive the position and orientation estimation of a rigid body in two-dimensional space measuring the positions of some transponders on the rigid body as well as acceleration data from an inertial navigation unit. Following the I&I observer design methodology, the pose is estimated as the outcome of the sensor fusion. The results obtained through the truely nonlinear I&I technique are compared to those with an Extended Kalman Filter showing similar performance with the I&I sensor fusion procedure being globally stable.