Hi everyone, this is my first time on a forum so if I do something wrong please forgive me. I'm trying to build my own Kalman Filter, not because I don't like OpenCV's but because I think that when you want to really understand how something works you should get your hands dirty on it. So I made my researches and then modified a Kalman filter tutorial I found online, in order to see the different behaviour of my KF and OpenCV's one. The results were not really optimal: my KF seems to have a perfect and fast tracking but with way too much noise...OpenCV's one is a bit slower but much more stable...and for what I'm going to do with KF I prefer stability to velocity. So the question is: what am I doing wrong? Any help will be really appreciated. I don't think there's something wrong in the algorithm itself, I found plenty of papers online dealing with the algorithm...so I think the problem it somwhere in the coding part, but I really can't figure out where. Main things I added to the original: the GBKalman structure, the functions GBKalman_init, GBKalman_predict, GBKalman_correct, GBKalman_update. Here's the code:
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include <Windows.h>
#include <iostream>
#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 )
using namespace cv;
using namespace std;
typedef struct GBKalman
{
int n;
int m;
Mat A;
Mat H;
Mat Q;
Mat R;
Mat x;
Mat x_priori;
Mat x_previous;
Mat P;
Mat P_priori;
Mat P_previous;
Mat Kk;
Mat Htransp;
Mat Atransp;
Mat I;
Mat temp;
};
void GBKalman_init(GBKalman* G, int a, int c, Mat D, Mat F, Mat I, Mat J, Mat X, Mat Y)
{
G->n = a;
G->m = c;
G->A = D;
G->H = F;
G->Q = I;
G->R = J;
G->x_previous = X;
G->x_priori = Mat_<float>(a, 1);
G->x = Mat_<float>(a, 1);
G->P_previous = Y;
G->P_priori = Mat_<float>(a, a);
G->P = Mat_<float>(a, a);
G->Kk = Mat_<float>(a, c);
G->Htransp = Mat_<float>(a, c); G->Htransp = G->H.t();
G->Atransp = Mat_<float>(a, a); G->Atransp = G->A.t();
G->I.eye(a, a, CV_32F);
G->temp = Mat(c, c, CV_32F);
}
void GBKalman_predict(GBKalman* G)
{
G->x_priori = G->A * G->x_previous;
G->P_priori = G->A * G->P_previous * G->Atransp + G->Q;
}
void GBKalman_correct(GBKalman* G, Mat zk)
{
G->temp = G->R + G->H * G->P_priori * G->Htransp;
G->Kk = G->P_priori * G->Htransp * G->temp.inv();
G->x = G->x_priori + G->Kk * (zk - G->H * G->x_priori);
G->P = (G->I - G->Kk * G->H) * G->P_priori;
}
void GBKalman_update(GBKalman *G)
{
G->x_previous = G->x;
G->P_previous = G->P;
}
int main()
{
KalmanFilter KF(4, 2, 0);
POINT mousePos;
GetCursorPos(&mousePos);
// intialization of KF...
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
Mat_<float> measurement(2, 1); measurement.setTo(Scalar(0));
KF.statePre.at<float>(0) = mousePos.x;
KF.statePre.at<float>(1) = mousePos.y;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(1));
setIdentity(KF.errorCovPost, Scalar::all(.1));
// initialization of my Kalman filter
GBKalman GBK;
Mat A = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
Mat H = (Mat_<float>(2, 4) << 1, 0, 0, 0, 0, 1, 0, 0);
Mat Q = (Mat_<float>(4, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
Mat R = (Mat_<float>(2, 2) << 3, 0, 0, 3);
Mat x0 = (Mat_<float>(4, 1) << mousePos.x, mousePos.y, 0, 0);
Mat P0 = (Mat_<float>(4, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
GBKalman_init(&GBK, 4, 2, A, H, Q, R, x0, P0);
// Image to show mouse tracking
Mat img(600, 800, CV_8UC3);
vector<Point> mousev, kalmanv;
mousev.clear();
kalmanv.clear();
Mat_<float> v(2, 1); setIdentity(v, Scalar::all(1e-1));
while (1)
{
// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
//gaussian measurement noise
randn(v, 0, 5);
// Get mouse point without noise. Noise will be added when calling the update functions
GetCursorPos(&mousePos);
measurement(0) = mousePos.x;
measurement(1) = mousePos.y;
// my kalman routine
GBKalman_predict(&GBK);
GBKalman_correct(&GBK, measurement+v);
GBKalman_update(&GBK);
// The update phase
Mat estimated = KF.correct(measurement+v);
Point statePt(estimated.at<float>(0), estimated.at<float>(1)); // opencv estimated point
Point measPt(measurement(0), measurement(1)); // true point without noise
Point truepoint(measurement(0)+v(0), measurement(1)+v(1)); // true mesured point (with noise)
Point myPt(GBK.x.at<float>(0),GBK.x.at<float>(1)); // my KF estimated point
// plot points
imshow("mouse kalman", img);
img = Scalar::all(0);
drawCross(myPt, Scalar(255, 0, 0), 5); // blue : estimated position by my KF
drawCross(statePt, Scalar(255, 255, 255), 5); // white : estimated by OpenCV's KF
//drawCross(measPt, Scalar(0, 0, 255), 5); // red: real position without noise
//drawCross(truepoint, Scalar(0, 255, 0), 5); // green: real position + noise
waitKey(10);
}
return 0;
}
thank everybody in advance
