Added some stuff for the kalman thingy