OpenCV Kalman filter

Question

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

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

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

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

Answer 1

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

kalman->process_noise_cov является «шум процесса covariance matrix» и его часто называют в литературе Калмана, как Q. Результат будет более плавным с более низкими значениями.

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

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

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

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

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