Update algorithm_kalman.c
Merge pull request #3 from Wayne-Weng/patch-2

Update algorithm_kalman.c