OpenCV Kalman filter

Question

I have three gyroscope values, pitch, roll and yaw. I would like to add Kalman filter to get more accurate values. I found the opencv library, which implements a Kalman filter, but I can't understand it how is it really work.

Could you give me any help which can help me? I didn't find any related topics on the internet.

I tried to make it work for one axis.

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;

And I call this everytime, when I receive data from gyro:

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

I thought in kalman_filter the first parameter is the previous value and the second is the currect value. I'm not and this code doesn't work... I know I have a lot of work with it, but I don't know how to continue, what to change...

Answer 1

It seems like you are giving too high values to the covariance matrices.

kalman->process_noise_cov is the 'process noise covariance matrix' and it is often referred in the Kalman literature as Q. The result will be smoother with lower values.

kalman->measurement_noise_cov is the 'measurement noise covariance matrix' and it is often referred in the Kalman literature as R. The result will be smoother with higher values.

The relation between those two matrices defines the amount and shape of filtering you are performing.

If the value of Q is high, it will mean that the signal you are measuring varies quickly and you need the filter to be adaptable. If it is small, then big variations will be attributed to noise in the measure.

If the value of R is high (compared to Q), it will indicate that the measuring is noisy so it will be filtered more.

Try lower values like q = 1e-5 and r = 1e-1 instead of q = 2.0 and r = 3.0.