OpenCV Kalman Filter

Question

I have three gyro values, step, roll and yaw. I would like to add a Kalman filter to get more accurate values. I found an opencv library that implements the Kalman filter, but I cannot figure out how this really works.

Could you help me? I did not 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 the 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 it every time I get data from a gyroscope:

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

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

Answer 1

It seems that you give too high values ​​to covariance matrices.

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

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

The relationship between the two matrices determines the amount and form of filtering that you perform.

If the Q value is large, this will mean that the signal you are measuring changes quickly, and you need the filter to be adapted. If it is small, then large changes will be explained by noise in measure.

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

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