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.
COMP: move member variables that are really local temps out of class & into method
COMP: cleanup
ENH: Remove outdated documentation.
STYLE: Code consolidation to prepare for performance improvements.
COMP: merge conflicts resolved