[Sensors] Implement calibration algorithms

Implement calibration algorithms for accelerometers, gyroscopes and magnetometers. Also implement "filters" that can be used during flight to correct sampled data from IMUs.