Started on kalman filter. has bugs