[KALMAN] Started implementation inside software