Fixed A matrix dimensions and bug fixing in updateStateHistory