replace the F estimate func
model 1 method 2 but not pass test
change some coeficient in robot. rocord some bugs in TODO.org