Kalman prediction and correction are the same as the initial values

Question

Im implements a Kalman filter, which receives real measurements from camshift head tracking after it has previously detected a face with a Haar cascade. I initialize the preliminary state variables and column states from the Kalman filter using the head position from the Haar cascade, and call kalman to predict and correct when you make the cam to get some smoothing. The problem is that the predicted and corrected values ​​are always the initial values ​​from the haras cascade. Do I have to update pre-state or state variables when doing camshift?

private CvKalman Kf ;
public CvMat measurement = new CvMat(2,1, MatrixType.F32C1);
public int frameCounter = 0;
public float[] A = {1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1};
public float[] H = {1,0,0,0, 0,1,0,0};
public float[] Q = {0.0001f,0,0,0, 0,0.0001f,0,0, 0,0,0.0001f,0, 0,0,0,0.0001f};
public float[] R = {0.2845f,0.0045f,0.0045f,0.0455f};
public float[] P = {100,0,0,0, 0,100,0,0, 0,0,100,0, 0,0,0,100};

initkalman is called once during the execution of the cascade of hara, and the tracking window is the initial position of the head.

void initKalman(CvRect trackinWindow){
    Kf = new CvKalman (4, 2, 0);
    Marshal.Copy (A, 0, Kf.TransitionMatrix.Data, A.Length);
    Marshal.Copy (H, 0, Kf.MeasurementMatrix.Data, H.Length);
    Marshal.Copy (Q, 0, Kf.ProcessNoiseCov.Data, Q.Length);
    Marshal.Copy (R, 0, Kf.MeasurementNoiseCov.Data, R.Length);
    Marshal.Copy (P, 0, Kf.ErrorCovPost.Data, P.Length);
    measurement.mSet (0, 0, trackingWindow.X);
    measurement.mSet (1, 0, trackingWindow.Y);

    Kf.StatePost.mSet(0,0,trackingWindow.X);
    Kf.StatePost.mSet(1,0,trackingWindow.Y);
    Kf.StatePost.mSet(2, 0, 0);
    Kf.StatePost.mSet(3, 0, 0);
}

processKalman camshift,

    CvPoint processKalman(CvRect trackingwindow)
{

    CvMat prediction = Cv.KalmanPredict(Kf);

    CvPoint predictionPoint;
    predictionPoint.X = (int)prediction.DataArraySingle [0];
    predictionPoint.Y = (int)prediction.DataArraySingle [1];

    Debug.Log (predictionPoint.X);

    measurement.mSet (0, 0, trackingWindow.X);
    measurement.mSet (1, 0, trackingWindow.Y);

    CvMat estimated = Cv.KalmanCorrect(Kf,measurement);

    CvPoint auxCP;

    auxCP.X = (int)estimated.DataArraySingle [0];
    auxCP.Y = (int)estimated.DataArraySingle [1];
    return auxCP;

}

. , , , , .

Answer 1

. .

public interface IKalmanFilter
{
    /// <summary>
    /// The current observation vector being used.
    /// </summary>
    Vector<double> Observation { get; }

    /// <summary>
    /// The best estimate of the current state of the system.
    /// </summary>
    Vector<double> State { get; }

    /// <summary>
    /// The covariance of the current state of the filter. Higher covariances
    /// indicate a lower confidence in the state estimate.
    /// </summary>
    Matrix<double> StateVariance { get; }

    /// <summary>
    /// The one-step-ahead forecast error of y given the previous measurement.
    /// </summary>
    Vector<double> ForecastError { get; }

    /// <summary>
    /// The one-step ahead forecast error variance.
    /// </summary>
    Matrix<double> ForecastErrorVariance { get; }

    /// <summary>
    /// The Kalman Gain matrix.
    /// </summary>
    Matrix<double> KalmanGain { get; }

    /// <summary>
    /// Performs a prediction of the next state of the system.
    /// </summary>
    /// <param name="T">The state transition matrix.</param>
    void Predict(Matrix<double> T);

    /// <summary>
    /// Perform a prediction of the next state of the system.
    /// </summary>
    /// <param name="T">The state transition matrix.</param>
    /// <param name="R">The linear equations to describe the effect of the noise
    /// on the system.</param>
    /// <param name="Q">The covariance of the noise acting on the system.</param>
    void Predict(Matrix<double> T, Matrix<double> R, Matrix<double> Q);

    /// <summary>
    /// Updates the state estimate and covariance of the system based on the
    /// given measurement.
    /// </summary>
    /// <param name="y">The measurements of the system.</param>
    /// <param name="T">The state transition matrix.</param>
    /// <param name="Z">Linear equations to describe relationship between
    /// measurements and state variables.</param>
    /// <param name="H">The covariance matrix of the measurements.</param>
    void Update(Vector<double> y, Matrix<double> T, 
        Matrix<double> Z, Matrix<double> H, Matrix<double> Q);
}

Vector<T> Matrix<T> - MathNet.Numerics. , , , ( ).

, ( , ). / :

// Set default initial state and variance.
a = Vector<double>.Build.Dense(1, 0.0d);
P = Matrix<double>.Build.Dense(1, 1, Math.Pow(10, 7));

// Run the filter.
List<double> filteredData = new List<double>();
IKalmanFilter filter = new KalmanFilter(a, P);
for (int i = 0; i < Data.Length; i++)
{
    filter.Predict(T, R, Q);
    Vector<double> v = DenseVector.Create(1, k => Convert.ToDouble(Data[i]));
    filter.Update(v, T, Z, H, Q);

    // Now to get the filtered state values, you use. (0 as it is one-dimensional data)
    filteredData.Add(filter.State[0]);
}

, , , . , , ...