Sensor calibration starting to work (not totally finished)