fixed backward kalman update issue