Add python function for testing estimator on the robot