Improved edge detection. Fixed some bugs inc. IMU speed.