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中存在一个封闭的问题，该问题指向Kalman滤波器的更新的单元测试。使用他们的初始化方式，我设法获得了看起来更稳定的东西。

            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;

不完全相同，但是现在对我来说已经足够稳定和足够了。