Add const and refs to params in main. fix quaternion
Remove unneeded fx in main, update feature constr
Remove redundant Feature::get_body()
Improve feature methods
Make ID and position in feature class private
Pass vector to meas model by reference