kalman_demo working well with sensor hil.