Add Kalman filter functions for use.

Signed-off-by: cctsao1008 <chiacheng.tsao@gmail.com>