Filtre de Kalman (exemple qui ne marche pas)
Bonjour,
alors je suis entrain d'implémenter le filtre de Kalman mais visiblement ça ne marche pas bien.
Après avoir fait un background substraction assez simple j'obtiens la position des connected components (blob) qui bouge sur mon image. Le problème c'est que cette position est parfois très bruitée ou parfois il y a des occlusions. J'aimerais donc faire en sorte que Kalman puissent m'estimer une trajectoire plus stable (plus smooth).
Voici donc la trajectoire d'un de mes connected component mesurée au cours du temps.
http://elraton.free.fr/work/s400/kal...y_measured.jpg
Et ici le fichier contenant ces valeurs: measured_position.txt
Ces données proviennent du dataset de PETS2001 (frames 93 à 673), vous pouvez les télécharger via FTP.
(Utilisez fireFTP avec firefox pour faciliter le download).
Donc afin d'obtenir une trajectoire plus smooth et de suivre mon object meme si j'ai des donnés absentes, j'ai fait un petit code pour estimer la prochaine position.
Le filtre de Kalman est configuré avec les paramètres suivants: Kalman.xml
Et voici le code principale pour effectuer mon estimation :
kalman.c et kalman.h
Code:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
|
const CvMat* updateKalman(CvKalman *kalman, int x, int y) {
CvMat *measure = cvCreateMat(kparam_g->MP,1, CV_32FC1 );
//Set the new measurement
CV_MAT_ELEM(*(measure),float,0,0) = (float)x;
CV_MAT_ELEM(*(measure),float,1,0) = (float)y;
//Kalman Predict (Uk=NULL because we do not use the control)
CvMat* u = cvCreateMat(1,1, CV_32FC1 );
u->data.fl[0]=1;
const CvMat* prediction = cvKalmanPredict(kalman,u);
//Kalman Correct (Correct the prediction with the new measure)
const CvMat* next_state = cvKalmanCorrect(kalman, measure);
cvReleaseMat(&u);
cvReleaseMat(&measure);
return next_state;
} |
main.c
Code:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
|
CvPoint pos_meas;
CvSize size_meas;
CvPoint pos_esti;
CvSize size_esti;
const CvMat *estMx;
while(frame) {
//Update Kalman filter if we have a new measure
if(pos_meas.x >= 0 && pos_meas.y >= 0)
estMx = (CvMat*)updateKalman(fkalman,pos_meas.x,pos_meas.y);
//Otherwise Continue trajectory with estimated position
else
estMx = (CvMat*)updateKalman(fkalman,pos_esti.x,pos_esti.y);
//! @attention: This is a bad thing to update the position but it doesn't work otherwise
pos_esti.x= (int)round(estMx->data.fl[0] + fkalman->state_post->data.fl[2]);
pos_esti.y= (int)round(estMx->data.fl[1] + fkalman->state_post->data.fl[3]);
//The size is for the moment not taken in account (Measure <=> Estimation)
size_esti.width=size_meas.width;
size_esti.height=size_meas.height;
} |
Au final j'obtient le fichier : estimated_position.txt
Si je plot les valeurs obtenues j'obtiens une trajectoire pas du tout smooth avec une grosse erreur quand je ne lui donne pas de measure. En résumé le resultat de ce code n'est pas terrible :
http://elraton.free.fr/work/s400/kal..._estimated.jpg
Donc voila ci qqun pouvais m'aider et me dire si je fait qqc je de mal, ou si mes paramètres sont mauvais.
Merci encore.
PS: Le code source du programme, les fichiers de config et le .exe se trouve dans kalman_scr-exe.zip (147Ko) et pour lancer l'executable il faut envoyer les parametres suivants:
Code:
kalman.exe measured_position.txt kalman.xml D:\DATABASE\PETS2001\DS01_Te_cam1\
probléme de formulation des equation de filtre de kalman
bonjour ,
je trouve vraiment des difficulté de formuler le vecteur d'état et les matrice de transition pour appliquer le filtre de calman , je vous explique , je travail sur la réalité augmentée donc je doit superposer mon objet 3d virtuel dans une séquence d'image dans le bon endroit , mon objet s'affiche normal , mais il bouge , parce que a chaque 10mseconde je récupère une image depuis mon capteur et les valeur des pixel change due au bruit de capteur , je voulais appliquer le filtre de calman pour stabiliser l'objet dans le bon endroit , j'ai les cordonnées des point caractéristique de mon marquer, donc je peux mesurer la distance ainsi la vitesse ,même j'ai la matrice de pose qui permet de l'objet de s'afficher au centre de marquer , j'ai toute les information qui me permet d'appliquer le filtre de calman , mais je sais pas comment les appliquer pour le filtre ,, aider moi svp ,, urgent svp
