modified kalman

Signed-off-by: bubi <m.bubestinger@gmail.com>
small
added linker
test