# Kalman filter code

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 ...
edit retag close merge delete

 //KF.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);