Camera and inertial sensor fusion for the PnP problem: algorithms and experimental results