Comment get_val method since it's weird
Cleanup
Use min function for d0 update
Adjust formatting
Add newlines to long lines
Convert qb0w to Quaternion
Convert xb0wHat
Modify pibr
Move tempMat for better scoping
Conver xiwhat to vec3d
Remove block of commented code
Simplify Vec3d assignment
Calculate f with dynamics() method
Remove unused euler2Quaternion function
Remove unneeded initializer
Simplify Fi initialization
replace pow(,2) with x*x
Move feature dynamics into dynamics()
Remove k from jacobianH