卡尔曼预测和校正与起始值相同

Posted 2023-02-21

【中文标题】卡尔曼预测和校正与起始值相同

【英文标题】：Kalman prediction & correction same as start values

【发布时间】：2014-02-26 15:42:00

【问题描述】：

我正在实现一个卡尔曼滤波器，该滤波器在先前使用 Haar Cascade 检测到面部之后，从 camshift 头部跟踪接收实际测量值。我使用来自 Haar Cascade 的头部位置初始化来自卡尔曼滤波器的状态前和状态后变量，并在进行 camshift 时调用卡尔曼预测和校正以获得一些平滑。问题是预测值和校正值始终是 haar 级联的起始值。在进行 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;

在做haar级联时调用了一次initkalman，跟踪窗口就是初始头部位置。

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);

我在每次 camshift 迭代中调用 processKalman 函数，现在正在跟踪窗口的实际头部位置

    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;

这不起作用，总是只返回初始头部位置。来自this优秀博客的家伙在调用预测函数之前用实际测量值更改状态帖子，但这样做对我来说唯一改变的是是预测值和校正值现在与每帧的 camshift 头位置相同。

【问题讨论】：

向我们展示您的卡尔曼预测和校正代码。很可能在这里你出错了......

我使用来自 opencvsharp 库的标准方法。但就像我说的，我认为状态前后变量及其初始化有些不对劲。也可能是标准方法的内部问题，因为除了我提到的博客中使用 opencvsharp 包装器的那个人之外，我没有见过任何人。如果是这样，我想我需要按照您的建议制定自己的方法。

是的，也许，但在上面我没有看到数据数组，只有一个点。如果您运行过滤器一点，您当然会得到起始状态向量。卡尔曼滤波器的整个想法是它是递归的，并且以某种方式“训练”自己以在离散的时间内找到与观察向量相关的状态向量。如果这是来自一个众所周知的库，它可能正在做正确的事情，您需要有一个时间序列的观察向量（可能是 1D）y_n[nPoints] 并在其上运行过滤器。我会回去阅读有关实施的更多信息...

好吧，我错了！！我缺少存储预测和校正的校正和估计数组！非常感谢你。我要疯了！！！

【参考方案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]);

我知道这没有使用您的代码，但我希望它对您有所帮助。如果你决定要走这条路，我会帮你用实际的卡尔曼滤波器代码......