fixed minor bugs in kalman.c and tests