Adding integral gain for distance