Removed multivariate Guassian and incorportated it into the main system
Improvements to the kalman filter code
More improvements, now compiles and runs however results are incorrect