OpenCV фильтр Калмана - PullRequest
Новая
       12

OpenCV фильтр Калмана

9 голосов
/ 19 сентября 2010

У меня есть три значения гироскопа, тангаж, крен и рыскание. Я хотел бы добавить фильтр Калмана, чтобы получить более точные значения. Я нашел библиотеку 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);
}

в основном 

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 первый параметр - это предыдущее значение, а второй - точное значение. Это не так, и этот код не работает ... Я знаю, у меня много работы с ним, но я не знаю, как продолжить, что изменить ...

1 Ответ

21 голосов
/ 19 сентября 2010

Похоже, вы даете слишком высокие значения ковариационным матрицам.

kalman->process_noise_cov - это 'технологический шум ковариационная матрица ' , и частоупоминается в калманской литературе как Q.Результат будет более гладким с более низкими значениями.

kalman->measurement_noise_cov - это 'ковариационная матрица шума измерений' , и в литературе по Кальману ее часто называют R.Результат будет более гладким с более высокими значениями.

Отношение между этими двумя матрицами определяет количество и форму фильтрации, которую вы выполняете.

Если значение Q высокое, это будет означать, что измеряемый сигнал быстро меняется, и вам нужен фильтр для адаптации.Если оно небольшое, то большие колебания будут связаны с шумом в измерении.

Если значение R высокое (по сравнению с Q), это будет означать, что измерение является шумным, поэтомубудет отфильтровано больше.

Попробуйте использовать более низкие значения, такие как q = 1e-5 и r = 1e-1 вместо q = 2.0 и r = 3.0.

...