Фильтр EmguCV Kalman дает нестабильные результаты

Question

Я новичок в области фильтрации Калмана и пытаюсь собрать кучу учебников, чтобы заставить работать фильтр Калмана EMGU.CV.

Я нашел функциональный фильтр Калмана в https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/13-Smoothing.ipynb , с которым я могу сравнить свои результаты.

Я настроил фильтр Калмана EMGU с теми же значениями и получаю в основном те же результаты. Тем не менее, иногда это будет go неправильно очень внезапно. (Измеренный шум = 10, Q = 0,001)

Кроме того, небольшие изменения в измеряемой переменной шума могут внезапно сделать ее правильной (Измеренный шум = 9,999, Q = 0,001)

Я что-то не так делаю в коде или это связано с ошибкой или нестабильностью в реализации?

            measurementNoise = 9.999f;
            processNoise = 0.001f;
            List<float> measuredResult = new List<float>();
            List<float> smoothedResult = new List<float>();

            var depthType = DepthType.Cv32F;
            var kal = new KalmanFilter(4, 1, 0, depthType);

            kal.StatePost.SetTo(new float[] { 0, 1, 1, 1 }); //[x, v_x, a_x, da_dx]
            var meas = new Mat(1, 1, depthType, 1); //[x]

            //Transition State Matrix A
            //Note: Set dT at each processing step
            //[1 1 0 0]
            //[0 1 1 0]
            //[0 0 1 1]
            //[0 0 0 1]
            CvInvoke.SetIdentity(kal.TransitionMatrix, new MCvScalar(1));
            kal.TransitionMatrix.SetValue(0, 1, 1.0f);
            kal.TransitionMatrix.SetValue(1, 2, 1.0f);
            kal.TransitionMatrix.SetValue(2, 3, 1.0f);

            //Measure Matrix H
            //[1 0 0 0]
            kal.MeasurementMatrix.SetTo(new float[] { 1, 0, 0, 0 });

            //Process Noise Covariance Matrix Q
            CvInvoke.SetIdentity(kal.ProcessNoiseCov, new MCvScalar(processNoise));

            //Measurement Noise Covariance Matrix R
            CvInvoke.SetIdentity(kal.MeasurementNoiseCov, new MCvScalar(measurementNoise));

            //Error Covariance Matrix
            CvInvoke.SetIdentity(kal.ErrorCovPost, new MCvScalar(10));

            for (int count = 0; count < times.Length; count++)
            {
                measuredResult.Add(values[count]);

                meas.SetValue(0, 0, values[count]);
                kal.Predict();

                var mat = kal.Correct(meas);

                smoothedResult.Add(((float[,])mat.GetData())[0, 0]);
            }

            foreach (var f in smoothedResult)
            {
                Console.Out.WriteLine($"{f}");
            }

Answer 1

РЕДАКТИРОВАТЬ : Оказалось, что стабильность была просто в том, что я нашел набор значений, которые не показывали всплески в этом наборе данных. Нестабильность все еще там. Использование этого шаблона инициализации соответствует ожидаемому шаблону, пока не наступит нестабильность. Так что это похоже на ошибку в базовом фильтре Калмана.

---

Так что после поиска больше я нашел закрытую проблему в EMGU, которая указывала на обновленные модульные тесты для фильтра Калмана. Используя их способ инициализации, мне удалось получить нечто более стабильное.

            KalmanFilter tracker = new KalmanFilter(4, 1, 0);
            var transitionMatrix = new Matrix<float>(new[,]
            {
                {1f, 1f, 0f, 0f},
                {0, 1, 1, 0},
                {0, 0, 1, 1},
                {0, 0, 0, 1}
            });
            var measurementMatrix = new Matrix<float>(new[,] { { 1f, 0, 0, 0 } });

            var procNoiseCov = new Matrix<float>(4, 4);
            procNoiseCov.SetIdentity(new MCvScalar(processNoise));

            var measurementNoise = new Matrix<float>(1, 1);
            measurementNoise.SetIdentity(new MCvScalar(measurementNoiseValue));

            var errorCovPost = new Matrix<float>(4, 4);
            errorCovPost.SetIdentity(new MCvScalar(10));

            transitionMatrix.Mat.CopyTo(tracker.TransitionMatrix);

            measurementMatrix.Mat.CopyTo(tracker.MeasurementMatrix);
            procNoiseCov.Mat.CopyTo(tracker.ProcessNoiseCov);
            measurementNoise.Mat.CopyTo(tracker.MeasurementNoiseCov);
            errorCovPost.Mat.CopyTo(tracker.ErrorCovPost);
            tracker.StatePost.SetTo(new float[] { 0, 1, 1, 1 });

            List<float> result = new List<float>();
            Matrix<float> corrected = new Matrix<float>(4, 1);

            foreach (var z in values)
            { 
                tracker.Correct(measurement.Mat).CopyTo(corrected);
                tracker.Predict();
                states.Add(corrected[0,0]);
            }

            return states;

Это не точно то же самое, но оно стабильно и достаточно хорошо для меня, верно Теперь.