more small changes and added matlab script for some data analysis
optimization for rigid body w computation