时间戳

首页 > 解决方案 > EmguCV 卡尔曼滤波器给出不稳定的结果

c# - EmguCV 卡尔曼滤波器给出不稳定的结果

问题描述

我是卡尔曼滤波的新手，我试图整理一堆教程来让 EMGU.CV 的卡尔曼滤波器工作。

我在https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/13-Smoothing.ipynb找到了一个功能性卡尔曼滤波器，我可以将结果与之进行比较。

我设置了具有相同值的 EMGU 卡尔曼滤波器，并获得了几乎相同的结果。但是，有时它会非常突然地出错。（测量噪声 = 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}");
            }

标签： c#opencvemgucvkalman-filter

解决方案

编辑：结果证明稳定性只是我发现了一组在该数据集中没有显示峰值的值。不稳定性仍然存在。使用此初始化模式确实与预期模式匹配，直到出现不稳定。所以它看起来像是底层卡尔曼滤波器中的一个错误。

所以在搜索了更多之后，我在 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;

它不完全一样，但它现在对我来说稳定且足够好。

推荐阅读