Implemented the full probabilistic filtration method for calculating posterior movement estimates. To be improved, 2D gaussians, adaptive variance, cleanup of code