Fix a major bug in the covariance matrix update

Whoaaa, working so much better now!