Matrix math, Kalman filter, drawing changes etc.

-Added matrix math functions. Works really fast without even using SIMD
(though I should investigate that as well).
-Added Kalman filter based on these matrix functions.
-Added 3D Kalman filtering to the tracking point in tracking module.
Filtering is done in all 3 dimensions, so to get segmentation seed point
I have to project the point back into plane.
-Changed all setters and getters to be more inline with C++. So both set
and get is done by the same named member.
-Changed draw_plan_view to also draw the tracking origin point and also
draw a rotated rectangle around the selected points.