fix one bug in the model calculation
Merge branch 'master' of https://github.com/kaihumuc/nao_dhri_tum