added multiplication with transposed B
implemented Kalman filter prediction
implemented Kalman filter prediction
implemented Kalman filter prediction
added kalman update