move center of mass init to separate function
merge
kalmann filter still needs some tuning, may depend on the data used