PERF: Reorganizing code to minimize computational burden

The QuadProg and unscented_kalman filters have many performance
bottle necks due to unnesary computations.

This patch reorganizes the code to make it a little more readalbe
and to add const correctness along with referenced variables wherever
possible.