Filtr OpenCV Kalman

Question

Mam trzy wartości żyroskopu, pitch, roll i odchylenie. Chciałbym dodać filtr Kalmana, aby uzyskać dokładniejsze wartości. Znalazłem bibliotekę opencv, która implementuje filtr Kalmana, ale nie mogę zrozumieć, jak to naprawdę działa.

Czy możesz udzielić mi pomocy, która może mi pomóc? Nie znalazłem żadnych powiązanych tematów w Internecie.

Próbowałem sprawić, aby działało na jedną oś.

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

w głównej

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; 

I nazywają to za każdym razem, kiedy odbierać dane z żyroskopu:

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

myślałem w kalman_filter pierwszy parametr jest poprzednia wartość, a druga jest poprawną wartością. Nie jestem i ten kod nie działa ... Wiem, że mam z nim dużo pracy, ale nie wiem jak kontynuować, co zmienić ...

Answer 1

Wygląda na to, że jesteś nadanie zbyt wysokich wartości macierzom kowariancji.

kalman->process_noise_cov jest „szum procesowy covariance matrix” i jest często określane w literaturze jako Q Kalmana. Wynik będzie bardziej płynny przy niższych wartościach.

kalman->measurement_noise_cov jest pomiarowej matrycy kowariancja szumu " i jest często określane w literaturze jako R Kalmana. Wynik będzie bardziej płynny przy wyższych wartościach.

Relacja między tymi dwiema matrycami określa ilość i kształt wykonywanych filtrów.

Jeśli wartość Q jest wysoka, oznacza to, że mierzony sygnał zmienia się szybko, a filtr wymaga dostosowania. Jeśli jest mały, duże różnice zostaną przypisane hałasowi w mierniku.

Jeśli wartość R jest wysoka (w porównaniu do Q), będzie wskazywać, że pomiar jest głośny, więc będzie filtrowany bardziej.

Spróbuj niższych wartości, takich jak q = 1e-5 i r = 1e-1 zamiast q = 2.0 i r = 3.0.