Improve the optimization speed

[padhupradheep]

Right now the updated controller is executing at a speed of 30 Hz. Since we haven't yet found an alternate to scipy.optimize in cpp, we will explore the possibility of using the following:

• Numba
• JAX

Possibly use AutoDiff to remove the numerical gradient.