fixed minor bugs in kalman.c and tests
Merge pull request #1 from froj/fix-kalman

fixed minor bugs in kalman.c and tests