Testing ekf_motion3D -> nonlinearity kills -> try IEKF soon