fixed indexing problem when making the kalman filter