More careful setting
1. Simulatneous optimization
2. Scaling factors of 10 for position/velocity