Fixed gyro rate integration bug

Simple fix... maybe implement timestamps in the ROS messages? (more accurate/reliable/kosher than current_milli_time)