Update algorithm_kalman.c