卡尔曼滤波器与加速度的应用

Question

我将尝试使用速度-加速度模型实现基于卡尔曼滤波的鼠标跟踪（首先作为测试）。我想尝试这个简单的模型，我的状态转移方程如下：

X(k) = [x(k), y(k)]'   (Position)
V(k) = [vx(k), vy(k)]' (Velocity)
X(k) = X(k-1) + dt*V(k-1) + 0.5*dt*dt*a(k-1)
V(k) = V(k-1) + t*a(k-1)
a(k) = a(k-1)

使用这个，我基本上写下了以下代码:

#include <iostream>
#include <vector>
#include <cstdio>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace std;


struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

void on_mouse(int event, int x, int y, int flags, void* param)
{
    //if (event == CV_EVENT_LBUTTONUP)
    {
        last_mouse = mouse_info;
        mouse_info.x = x;
        mouse_info.y = y;
    }
}


void printmat(const cv::Mat &__mat, std::string __str)
{
    std::cout << "--------" << __str << "----------\n";
    for (int i=0 ; i<__mat.rows ; ++i)
    {
        for (int j=0 ; j<__mat.cols ; ++j)
            std::cout << __mat.at<double>(i,j) << "  ";
        std::cout << std::endl;
    }
    std::cout << "-------------------------------------\n";
}


int main (int argc, char * const argv[])
{
    int nStates = 5, nMeasurements = 2, nInputs = 1;
    Mat img(500, 900, CV_8UC3);
    KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);
    Mat state(nStates, 1, CV_64F); /* (x, y, Vx, Vy, a) */
    Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0));
    Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0));
    int key = -1, dt=100, T=1000;
    float /*a=100, acclErrMag = 0.05,*/ measurementErrVar = 100, noiseVal=0.001, covNoiseVal=0.9e-4;

    namedWindow("Mouse-Kalman");
    setMouseCallback("Mouse-Kalman", on_mouse, 0);

    //while ( (char)(key=cv::waitKey(100)) != 'q' )
    {
        /// A
        KF.transitionMatrix.at<double>(0,0) = 1;
        KF.transitionMatrix.at<double>(0,1) = 0;
        KF.transitionMatrix.at<double>(0,2) = (dt/T);
        KF.transitionMatrix.at<double>(0,3) = 0;
        KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);

        KF.transitionMatrix.at<double>(1,0) = 0;
        KF.transitionMatrix.at<double>(1,1) = 1;
        KF.transitionMatrix.at<double>(1,2) = 0;
        KF.transitionMatrix.at<double>(1,3) = (dt/T);
        KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);

        KF.transitionMatrix.at<double>(2,0) = 0;
        KF.transitionMatrix.at<double>(2,1) = 0;
        KF.transitionMatrix.at<double>(2,2) = 1;
        KF.transitionMatrix.at<double>(2,3) = 0;
        KF.transitionMatrix.at<double>(2,4) = (dt/T);

        KF.transitionMatrix.at<double>(3,0) = 0;
        KF.transitionMatrix.at<double>(3,1) = 0;
        KF.transitionMatrix.at<double>(3,2) = 0;
        KF.transitionMatrix.at<double>(3,3) = 1;
        KF.transitionMatrix.at<double>(3,4) = (dt/T);

        KF.transitionMatrix.at<double>(4,0) = 0;
        KF.transitionMatrix.at<double>(4,1) = 0;
        KF.transitionMatrix.at<double>(4,2) = 0;
        KF.transitionMatrix.at<double>(4,3) = 0;
        KF.transitionMatrix.at<double>(4,4) = 1;


        /// Initial estimate of state variables
        KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
        KF.statePost.at<double>(0) = mouse_info.x;
        KF.statePost.at<double>(1) = mouse_info.y;
        KF.statePost.at<double>(2) = 0;
        KF.statePost.at<double>(3) = 0;
        KF.statePost.at<double>(4) = 0;

        KF.statePre = KF.statePost;

        /// Ex or Q
        setIdentity(KF.processNoiseCov, Scalar::all(noiseVal));


        /// Initial covariance estimate Sigma_bar(t) or P'(k)
        setIdentity(KF.errorCovPre, Scalar::all(1000));

        /// Sigma(t) or P(k)
        setIdentity(KF.errorCovPost, Scalar::all(1000));

        /// B
        KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
        KF.controlMatrix.at<double>(0,0) = 0;
        KF.controlMatrix.at<double>(1,0) = 0;
        KF.controlMatrix.at<double>(2,0) = 0;
        KF.controlMatrix.at<double>(3,0) = 0;
        KF.controlMatrix.at<double>(4,0) = 1;

        /// H
        KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);

        /// Ez or R
        setIdentity(KF.measurementNoiseCov, Scalar::all(measurementErrVar*measurementErrVar));

        printmat(KF.controlMatrix, "KF.controlMatrix");
        printmat(KF.transitionMatrix, "KF.transitionMatrix");
        printmat(KF.statePre,"KF.statePre");
        printmat(KF.processNoiseCov, "KF.processNoiseCov");
        printmat(KF.measurementMatrix, "KF.measurementMatrix");
        printmat(KF.measurementNoiseCov, "KF.measurementNoiseCov");
        printmat(KF.errorCovPost,"KF.errorCovPost");
        printmat(KF.errorCovPre,"KF.errorCovPre");
        printmat(KF.statePost,"KF.statePost");

        while (mouse_info.x < 0 || mouse_info.y < 0)
        {
            imshow("Mouse-Kalman", img);
            waitKey(30);
            continue;
        }

        while ( (char)key != 's' )
        {
            /// MAKE A MEASUREMENT
            measurement.at<double>(0) = mouse_info.x;
            measurement.at<double>(1) = mouse_info.y;

            /// MEASUREMENT UPDATE
            Mat estimated = KF.correct(measurement);

            /// STATE UPDATE
            Mat prediction = KF.predict();



            cv::Mat u(nInputs,1,CV_64F);
            u.at<double>(0,0) = 0.0 * sqrt(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
                                        + pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));

            /// STORE ALL DATA
            Point predictPt(prediction.at<double>(0),prediction.at<double>(1));
            Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
            Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));

            /// PLOT POINTS
#define drawCross( center, color, d )                                 \
            line( img, Point( center.x - d, center.y - d ),                \
            Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
            line( img, Point( center.x + d, center.y - d ),                \
            Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

            /// DRAW ALL ON IMAGE
            img = Scalar::all(0);
            drawCross( predictPt, Scalar(255,255,255), 9 );     //WHITE
            drawCross( estimatedPt, Scalar(0,0,255), 6 );       //RED
            drawCross( measuredPt, Scalar(0,255,0), 3 );        //GREEN


            line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
            line( img, estimatedPt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );

            prevMeasurement = measurement;

            imshow( "Mouse-Kalman", img );
            key=cv::waitKey(10);
        }
    }
    return 0;
}

这是代码的输出结果：http://www.youtube.com/watch?v=9_xd4HSz8_g 如你所见，跟踪非常缓慢。我不明白模型出了什么问题，为什么估计如此缓慢。我不认为应该有任何控制输入。

有人能解释一下吗？

- ekmungi

---

我觉得，你的sigma(t)值相当高（1000）。它与“收敛速度”成反比。试试一个更小的值。 - berak

@berak，那并没有帮助。然而，当我将我的dt/T更改为1时，似乎收敛得更快，但是在跟踪过程中突然间就会出现问题，跳来跳去的。这里有一个结果视频。我选择sigma为1，dt/T = 1。 - ekmungi

http://www.youtube.com/watch?v=4XjBQrQ2Dqs - ekmungi

@berak，我想我找到了问题所在。我已经更新了代码以反映这一点。你对协方差矩阵的值选择是正确的。它们是控制滤波器性能的方式。为了让其他人受益，我将留下我的代码的更新版本，其中我添加了测量噪声，并展示了如何通过改变不同的协方差矩阵来过滤噪声。 - ekmungi

嘿，回想起来很抱歉我有点懒。我有很多类似的代码可以使用trackbars玩弄变量，早些时候应该告诉你的。不管怎样，你找到了也很好;) - berak

Answer 1

我修改了代码，并将其发布给那些想要调整它以进行更多操作的人。主要问题在于协方差的选择。

#include <iostream>
#include <vector>
#include <cstdio>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace std;


struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

vector<Point> mousev,kalmanv;
int trackbarProcessNoiseCov = 10, trackbarMeasurementNoiseCov = 10, trackbarStateEstimationErrorCov = 10;

float processNoiseCov=10, measurementNoiseCov = 1000, stateEstimationErrorCov = 50;
int trackbarProcessNoiseCovMax=10000, trackbarMeasurementNoiseCovMax = 10000,
      trackbarStateEstimationErrorCovMax = 5000;

float processNoiseCovMin=0, measurementNoiseCovMin = 0,
      stateEstimationErrorCovMin = 0;
float processNoiseCovMax=100, measurementNoiseCovMax = 5000,
      stateEstimationErrorCovMax = 5000;

int nStates = 5, nMeasurements = 2, nInputs = 1;
KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);

void on_mouse(int event, int x, int y, int flags, void* param)
{
    last_mouse = mouse_info;
    mouse_info.x = x;
    mouse_info.y = y;
}

void on_trackbarProcessNoiseCov( int, void* )
{
    processNoiseCov = processNoiseCovMin +
        (trackbarProcessNoiseCov * (processNoiseCovMax-processNoiseCovMin)/trackbarProcessNoiseCovMax);
    setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));
    std::cout << "\nProcess Noise Cov:     " << processNoiseCov;
    std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}

void on_trackbarMeasurementNoiseCov( int, void* )
{
    measurementNoiseCov = measurementNoiseCovMin +
(trackbarMeasurementNoiseCov * (measurementNoiseCovMax-measurementNoiseCovMin)/trackbarMeasurementNoiseCovMax);
    setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));
    std::cout << "\nProcess Noise Cov:     " << processNoiseCov;
    std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}

int main (int argc, char * const argv[])
{
    Mat img(500, 1000, CV_8UC3);
    Mat state(nStates, 1, CV_64F);/* (x, y, Vx, Vy, a) */
    Mat measurementNoise(nMeasurements, 1, CV_64F), processNoise(nStates, 1, CV_64F);
    Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0.0));
    Mat noisyMeasurement(nMeasurements,1,CV_64F); noisyMeasurement.setTo(Scalar(0.0));
    Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0.0));
    Mat prevMeasurement2(nMeasurements,1,CV_64F); prevMeasurement2.setTo(Scalar(0.0));
    int key = -1, dt=50, T=1000;


    namedWindow("Mouse-Kalman");
    setMouseCallback("Mouse-Kalman", on_mouse, 0);
    createTrackbar( "Process Noise Cov", "Mouse-Kalman", &trackbarProcessNoiseCov,
            trackbarProcessNoiseCovMax, on_trackbarProcessNoiseCov );
    createTrackbar( "Measurement Noise Cov", "Mouse-Kalman", &trackbarMeasurementNoiseCov,
            trackbarMeasurementNoiseCovMax, on_trackbarMeasurementNoiseCov );

    on_trackbarProcessNoiseCov( trackbarProcessNoiseCov, 0 );
    on_trackbarMeasurementNoiseCov( trackbarMeasurementNoiseCov, 0 );

    //while ( (char)(key=cv::waitKey(100)) != 'q' )
    {
    /// A (TRANSITION MATRIX INCLUDING VELOCITY AND ACCELERATION MODEL)
    KF.transitionMatrix.at<double>(0,0) = 1;
    KF.transitionMatrix.at<double>(0,1) = 0;
    KF.transitionMatrix.at<double>(0,2) = (dt/T);
    KF.transitionMatrix.at<double>(0,3) = 0;
    KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);

    KF.transitionMatrix.at<double>(1,0) = 0;
    KF.transitionMatrix.at<double>(1,1) = 1;
    KF.transitionMatrix.at<double>(1,2) = 0;
    KF.transitionMatrix.at<double>(1,3) = (dt/T);
    KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);

    KF.transitionMatrix.at<double>(2,0) = 0;
    KF.transitionMatrix.at<double>(2,1) = 0;
    KF.transitionMatrix.at<double>(2,2) = 1;
    KF.transitionMatrix.at<double>(2,3) = 0;
    KF.transitionMatrix.at<double>(2,4) = (dt/T);

    KF.transitionMatrix.at<double>(3,0) = 0;
    KF.transitionMatrix.at<double>(3,1) = 0;
    KF.transitionMatrix.at<double>(3,2) = 0;
    KF.transitionMatrix.at<double>(3,3) = 1;
    KF.transitionMatrix.at<double>(3,4) = (dt/T);

    KF.transitionMatrix.at<double>(4,0) = 0;
    KF.transitionMatrix.at<double>(4,1) = 0;
    KF.transitionMatrix.at<double>(4,2) = 0;
    KF.transitionMatrix.at<double>(4,3) = 0;
    KF.transitionMatrix.at<double>(4,4) = 1;


    /// Initial estimate of state variables
    KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
    KF.statePost.at<double>(0) = mouse_info.x;
    KF.statePost.at<double>(1) = mouse_info.y;
    KF.statePost.at<double>(2) = 0.1;
    KF.statePost.at<double>(3) = 0.1;
    KF.statePost.at<double>(4) = 0.1;

    KF.statePre = KF.statePost;
    state = KF.statePost;

    /// Ex or Q (PROCESS NOISE COVARIANCE)
    setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));


    /// Initial covariance estimate Sigma_bar(t) or P'(k)
    setIdentity(KF.errorCovPre, Scalar::all(stateEstimationErrorCov));

    /// Sigma(t) or P(k) (STATE ESTIMATION ERROR COVARIANCE)
    setIdentity(KF.errorCovPost, Scalar::all(stateEstimationErrorCov));

    /// B (CONTROL MATRIX)
    KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
    KF.controlMatrix.at<double>(0,0) = /*0.5*(dt/T)*(dt/T);//*/0;
    KF.controlMatrix.at<double>(1,0) = /*0.5*(dt/T)*(dt/T);//*/0;
    KF.controlMatrix.at<double>(2,0) = 0;
    KF.controlMatrix.at<double>(3,0) = 0;
    KF.controlMatrix.at<double>(4,0) = 1;

    /// H (MEASUREMENT MATRIX)
    KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);

    /// Ez or R (MEASUREMENT NOISE COVARIANCE)
    setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));


    while (mouse_info.x < 0 || mouse_info.y < 0)
    {
        imshow("Mouse-Kalman", img);
        waitKey(30);
        continue;
    }

    prevMeasurement.at<double>(0,0) = 0;
    prevMeasurement.at<double>(1,0) = 0;
    prevMeasurement2 = prevMeasurement;

    while ( (char)key != 's' )
    {
        /// STATE UPDATE
        Mat prediction = KF.predict();

        /// MAKE A MEASUREMENT
        measurement.at<double>(0) = mouse_info.x;
        measurement.at<double>(1) = mouse_info.y;

        /// MEASUREMENT NOISE SIMULATION
        randn( measurementNoise, Scalar(0),
          Scalar::all(sqrtf(measurementNoiseCov)));
        noisyMeasurement = measurement + measurementNoise;

        /// MEASUREMENT UPDATE
        Mat estimated = KF.correct(noisyMeasurement);

        cv::Mat u(nInputs,1,CV_64F);
        u.at<double>(0,0) = 0.0 * sqrtf(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
                    + pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));

        /// STORE ALL DATA
        Point noisyPt(noisyMeasurement.at<double>(0),noisyMeasurement.at<double>(1));
        Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
        Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));


        /// PLOT POINTS
#define drawCross( center, color, d )                                 \
        line( img, Point( center.x - d, center.y - d ),                \
        Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
        line( img, Point( center.x + d, center.y - d ),                \
        Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

        /// DRAW ALL ON IMAGE
        img = Scalar::all(0);
        drawCross( noisyPt, Scalar(255,255,255), 9 );     //WHITE
        drawCross( estimatedPt, Scalar(0,0,255), 6 );       //RED
        drawCross( measuredPt, Scalar(0,255,0), 3 );        //GREEN


        line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
        line( img, estimatedPt, noisyPt, Scalar(0,255,255), 3, CV_AA, 0 );

        imshow( "Mouse-Kalman", img );
        key=cv::waitKey(dt);
        prevMeasurement = measurement;
    }
    }

    return 0;
}

- ekmungi

---

很高兴听到你找到了问题的原因。如果您能解释一下如何诊断它，特别是哪些证据将在其他地方看到此问题时识别它，那么您的答案可能会更有帮助性，供未来读者参考。 - Toby Speight