Kalman filter code

edit

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 ...