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

Question

我是卡尔曼滤波的新手，我正在尝试整理一堆教程来让 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}");
            }

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;

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