add feedback gain related to distant from previous, need ot test on real robot