OpenCV Filtro de Kalman

Question

Tengo tres valores de giroscopio, cabeceo, balanceo y guiñada. Me gustaría agregar el filtro de Kalman para obtener valores más precisos. Encontré la biblioteca opencv, que implementa un filtro de Kalman, pero no puedo entender cómo funciona realmente.

¿Me podría dar alguna ayuda que pueda ayudarme? No encontré ningún tema relacionado en internet.

He intentado hacer que funcione para un eje.

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); 
} 

en el principal

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; 

Y yo llamo a esto cada vez, cuando recibo los datos de giróscopo:

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

pensé en kalman_filter el primer parámetro es el valor anterior y el segundo es el valor correcto No estoy y este código no funciona ... Sé que tengo mucho trabajo con él, pero no sé cómo continuar, qué cambiar ...

Answer 1

Parece que eres dando valores demasiado altos a las matrices de covarianza.

kalman->process_noise_cov es el 'ruido del proceso covariance matrix' y que a menudo se conoce en la literatura como Kalman Q. El resultado será más suave con valores más bajos.

kalman->measurement_noise_cov es la 'matriz de covarianza de ruido de medición' y que a menudo se denomina en la literatura Kalman como R. El resultado será más suave con valores más altos.

La relación entre esas dos matrices define la cantidad y la forma del filtrado que está realizando.

Si el valor de Q es alto, significará que la señal que está midiendo varía rápidamente y necesita que el filtro sea adaptable. Si es pequeño, entonces se atribuirán grandes variaciones al ruido en la medida.

Si el valor de R es alto (en comparación con Q), indicará que la medición es ruidosa por lo que se filtrará más.

Pruebe valores más bajos como q = 1e-5 y r = 1e-1 en lugar de q = 2.0 y r = 3.0.