hector_pose_estimation_core: first functional version with multiplicative EKF

I still have to check the PoseUpdate class...