jacobian functions fixed
fixed jacobian bucs
pose fixes and auxiliary functions
added some auxiliary functions
added analytical jacobians