Add and improve math classes

VectorFloat: add += operator
Implement Kalman filter (early state, not tested)
Implement Matrix template