我将尝试使用速度-加速度模型实现基于卡尔曼滤波的鼠标跟踪(首先作为测试)。我想尝试这个简单的模型,我的状态转移方程如下:
使用这个,我基本上写下了以下代码:
这是代码的输出结果:http://www.youtube.com/watch?v=9_xd4HSz8_g 如你所见,跟踪非常缓慢。我不明白模型出了什么问题,为什么估计如此缓慢。我不认为应该有任何控制输入。
有人能解释一下吗?
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 如你所见,跟踪非常缓慢。我不明白模型出了什么问题,为什么估计如此缓慢。我不认为应该有任何控制输入。
有人能解释一下吗?