EmguCV Kalman filter giving unstable results

Question

I'm new to Kalman filtering and trying to put together a bunch of tutorials to get EMGU.CV's Kalman filter to work.

I've found a functional kalman filter at https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/13-Smoothing.ipynb that I can compare my results to.

I set up the EMGU Kalman filter with the same values, and get mostly the same results. However, sometimes it will go wrong very suddenly. (Measurement noise = 10, Q = 0.001)

Further, small changes to the measurement noise variable can suddenly make it correct (Measurement noise = 9.999, Q = 0.001)

Am I doing something wrong in the code or is it something to do with a bug or instability in the implementation?

            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

EDIT: Turns out that the stability was just that I'd found a set of values that didn't show spikes in that dataset. The instability is still there. Using this initialisation pattern does match the expected pattern until instability sets in though. So it looks like a bug in the underlying Kalman filter.

---

So after searching more, I found a closed issue in EMGU that pointed to the updated unit tests for the Kalman filter. Using their way of initialising, I've managed to get something that seems a lot more stable.

            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;

It's not exactly the same, but it's stable and good enough for me right now.