modified kalman

Signed-off-by: bubi <m.bubestinger@gmail.com>