Yaw drift correction

For the determination of internal angle at which the robot is standing, it uses the gyroscope in the IMU. Our current IMU, the XSens MTi-3 (see datasheet), is able to determine its orientation in quite a few ways (see this document). Unfortunately most of these features cannot be used within our robots, since they depend on the use of the magnetic field of the earth. The IMU however is located near five motors and two solenoids, which cause disturbances in this field. Hence, our magnetometer is turned off.

As a result from this the reported yaw from the gyroscope tends to drift away quite a bit. In order to resolve this problem, our robots re-calibrate the angle every time when they stand still on the field. If a robot stands still for a longer period of time, it will align its measured angle with the angle it receives from the vision data that is reported to it.

Currently the error between the vision angle (ground-truth) and the measured angle is determined and added to the measurement (see here and here).