Filtro OpenCV Kalman

Question

Ho tre valori di giroscopio, beccheggio, rollio e imbardata. Vorrei aggiungere il filtro Kalman per ottenere valori più accurati. Ho trovato la libreria opencv, che implementa un filtro di Kalman, ma non riesco a capirlo come funziona davvero.

Potrebbe darmi qualche aiuto che possa aiutarmi? Non ho trovato argomenti correlati su internet.

Ho provato a farlo funzionare per un asse.

const float A[] = { 1, 1, 0, 1 }; 
CvKalman* kalman; 
CvMat* state = NULL; 
CvMat* measurement; 

void kalman_filter(float FoE_x, float prev_x) 
{ 
    const CvMat* prediction = cvKalmanPredict(kalman, 0); 
    printf("KALMAN: %f %f %f\n" , prev_x, prediction->data.fl[0] , prediction->data.fl[1]); 
    measurement->data.fl[0] = FoE_x; 
    cvKalmanCorrect(kalman, measurement); 
} 

nel principale

kalman = cvCreateKalman(2, 1, 0); 
state = cvCreateMat(2, 1, CV_32FC1); 
measurement = cvCreateMat(1, 1, CV_32FC1); 
cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1)); 
memcpy(kalman->transition_matrix->data.fl, A, sizeof(A)); 
cvSetIdentity(kalman->process_noise_cov, cvRealScalar(2.0)); 
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(3.0)); 
cvSetIdentity(kalman->error_cov_post, cvRealScalar(1222)); 
kalman->state_post->data.fl[0] = 0; 

E io chiamo questo ogni volta, quando ricevo dati dal giroscopio:

kalman_filter(prevr, mpe->getGyrosDegrees().roll); 

ho pensato in kalman_filter il primo parametro è il valore precedente e la seconda è il valore del curriculum. Io non sono e questo codice non funziona ... So che ho molto lavoro con esso, ma non so come continuare, cosa cambiare ...

Answer 1

Sembra che tu sia dare valori troppo alti alle matrici di covarianza.

kalman->process_noise_cov è il 'rumore processo covariance matrix' ed è spesso definito in letteratura come Kalman Q. Il risultato sarà più fluido con valori più bassi.

kalman->measurement_noise_cov è la 'misurazione matrice di covarianza del rumore' ed è spesso definito in letteratura come Kalman R. Il risultato sarà più fluido con valori più alti.

La relazione tra queste due matrici definisce la quantità e la forma del filtro che si sta eseguendo.

Se il valore di Q è alto, significa che il segnale che si sta misurando varia rapidamente e che il filtro deve essere adattabile. Se è piccolo, le grandi variazioni saranno attribuite al rumore nella misura.

Se il valore di R è alto (rispetto a Q), indica che la misurazione è disturbata, quindi verrà filtrata di più.

Provare valori inferiori come q = 1e-5 e r = 1e-1 anziché q = 2.0 e r = 3.0.