kalman update and graph, broken