Finished trust calculation and distance in time scale function!