Added code to quicky compute control law

100x speedup