Silvère Bonnabel, Philippe Martin, Erwan Salaün: Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem. CDC 2009: 1297-1304