Kalman filter finalized woot!! filled with errors though..