Здравствуйте. Может кто подскажет в чем тут ошибка?
Хочу прикрутить фильтр кальмана для 2д пространства.
Взял пример из оригинальной OCV
Написал свой класс:
public KlmaFilter()
{
int stateSize = 2;
int measSize = 1;
int contrSize = 0;
KF = new KalmanFilter(stateSize, measSize, contrSize);
state = new Mat(stateSize, 1, MatType.CV_32FC1); // [x,y]
meas = new Mat(measSize, 1, MatType.CV_32FC1); // [x,y]
noise = new Mat(measSize, 1, MatType.CV_32FC1); // [x,y]
OpenCvSharp.Cv2.Randn(state, Scalar.All(0), Scalar.All(0.1));
float[] A = new float[] { 1, 1, 0, 1 };
Marshal.Copy(A, 0, KF.TransitionMatrix.Data, A.Length);
KF.MeasurementMatrix.SetIdentity(1);
KF.ProcessNoiseCov.SetIdentity(1e-5);
KF.MeasurementNoiseCov.SetIdentity(1e-1);
KF.ErrorCovPost.SetIdentity(1);
OpenCvSharp.Cv2.Randn(KF.StatePost, Scalar.All(0), Scalar.All(0.1));
}
public Point2f Process(double dT, Point2f value)
{
var prd = KF.Predict();
var x = prd.At<float>(0);
var y = prd.At<float>(1);
Marshal.Copy(new float[] { (float)value.X, (float)value.Y }, 0, meas.Data, 2);
KF.Correct(meas);
state = KF.TransitionMatrix * state;
Point2f result = new Point2f(x,y);
return result;
}
Вроде бы все также как и в примере за исключением того что я подаю реальные точки.
Но на выходе почему то точки всегда в одной плоскости крутятся.
Хотя сами точки должны образовывать крест.
Что я не так сделал то?