implemented Kalman filter as state estimator, but position is converging very slowly and causes oscillations in x, y and z direction