on computing act_vel and cmd_wheel_speed
build J_2 using diag mat type
odom nav in sim
Merge pull request #29 from tttor/nav-5

Nav 5: odom nav in sim.