estimator: Fixed body / world matrix initialization and reset
Merge branch 'est_reinit_fix'