Kalman filtering code without arma