OpenCV Kalman filter

У меня есть три значения гироскопа: тангаж, крен и рыскание. Я хотел бы добавить фильтр Калмана, чтобы получить более точные значения. Я нашел библиотеку opencv, в которой реализован фильтр Калмана, но я не могу понять, как это на самом деле работает.

Не могли бы вы мне помочь? Я не нашел в Интернете связанных тем.

Я попытался заставить его работать для одной оси.

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

in main

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;

И я вызываю это каждый раз, когда получаю данные от гироскопа:

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

Я думал, что в kalman_filter первый параметр - это предыдущее значение, а второй - текущее значение. Я не работаю, и этот код не работает ... Я знаю, что мне предстоит много работать с ним, но я не знаю, как продолжить, что изменить ...