Настройка фильтра Кальмана

Программирование
#1

Здравствуйте. Может кто подскажет в чем тут ошибка?

Хочу прикрутить фильтр кальмана для 2д пространства.
Взял пример из оригинальной OCV

Написал свой класс:

        public KlmaFilter()
        {
            int stateSize = 2;
            int measSize = 1;
            int contrSize = 0;

            KF = new KalmanFilter(stateSize, measSize, contrSize);

            state = new Mat(stateSize, 1, MatType.CV_32FC1);  // [x,y]
            meas = new Mat(measSize, 1, MatType.CV_32FC1);    // [x,y]
            noise = new Mat(measSize, 1, MatType.CV_32FC1);    // [x,y]                                  

            OpenCvSharp.Cv2.Randn(state, Scalar.All(0), Scalar.All(0.1));

            float[] A = new float[] { 1, 1, 0, 1 };
            Marshal.Copy(A, 0, KF.TransitionMatrix.Data, A.Length);

            KF.MeasurementMatrix.SetIdentity(1);
            KF.ProcessNoiseCov.SetIdentity(1e-5);
            KF.MeasurementNoiseCov.SetIdentity(1e-1);
            KF.ErrorCovPost.SetIdentity(1);

            OpenCvSharp.Cv2.Randn(KF.StatePost, Scalar.All(0), Scalar.All(0.1));
            
        }

        public Point2f Process(double dT, Point2f value)
        {
            var prd = KF.Predict();
            var x = prd.At<float>(0);
            var y = prd.At<float>(1);

            Marshal.Copy(new float[] { (float)value.X, (float)value.Y }, 0, meas.Data, 2);

            KF.Correct(meas);

            state = KF.TransitionMatrix * state;

            Point2f result = new Point2f(x,y);
            return result;
        }

Вроде бы все также как и в примере за исключением того что я подаю реальные точки.

Но на выходе почему то точки всегда в одной плоскости крутятся.
image

Хотя сами точки должны образовывать крест.
Что я не так сделал то?