Czy ten kalman działa poprawnie?

[qczek]

Witajcie,
na początek chciałbym się przywitać 🙂

Mam pytanie, chciałem sobie trochę polepszyć mojego kalmana którego stosuje w quadrocopterze i niestety mam pewne problemy. Już nawet myślałem że źle go napisałem, wiec skopiowałem jakieś rozwiązanie z sieci, niemniej nadal to samo. Może nie umiem dobrać parametrów?

W skrócie wygląda to tak:

Legenda do wykresu, Niebieski – kąt wyliczony z samych akcelerometrów, Zółty – prędkość kątowa z gyro, Pomarańczowy – estymowany kat.

1) Normalnie obracam w ręce urządzenie i jest dobrze, prędkość kątowa jest, jest zmiana danych z akcelerometru, jest zmiana estymowanego kąta.

2) Teraz sztucznie wymuszam, gdy urządzenie leży bez ruchu, nagły wzrost prędkości kątowej przy tym samym kącie z akcelerometrów. Widać, że na początku jest estymowany kąt, a potem korygowany akcelerometrem. Pytanie, co to za "odbicie" po ustaniu zakłócenia prędkości kątowej?

3) I to co mnie najbardziej boli, bo quadrocopter przy opadaniu zaczyna się bujać na boki.

Znowu przy nieruchomym urządzeniu wprowadzam zmienę, na przyspieszeniu ziemskim na osi yaw, czyli jak by nagle zaczął przyspieszać pionowo z 0.5g. I jest źle, niebieski to moje zakłucenie kąta z akcelerometru, a zaraz za nim podąża estymowany kąt 🙁

I teraz jak z tym walczyć, próbowałem, na tak zwaną pałę zmieniać współczynniki kowariancji, ale jak jest lepiej, to całość staje się bardzo mułowata...

Z góry dzięki za jakieś wskazówki.

Krzysiek

[GAndaLF]

Filtr Kalmana to nie jest taka prosta sprawa. Żeby uzyskać pomoc musisz bardziej precyzyjnie opisać problem.

Po pierwsze przydało by się wiedzieć jak wygląda twój model obiektu:

- jakie zmienne stanu wykorzystujesz

- czy używasz liniowego modelu

- czy twój model uwzględnia pracę silników czy wszystkie zmiany pozycji/prędkości/przyspieszenia traktowane są jako szum

- jaką postać mają macierze A B i C modelu

- jaki jest czas próbkowania - czy dla wszystkich pomiarów jest taki sam i czy obliczasz stany pośrednie pomiędzy pomiarami

- w jaki sposób dobrałeś macierze szumu pomiarowego i procesowego

- jakie użyłeś wartości początkowe stanu i wariancji

Mógł byś również podać wzory wykorzystane do implementacji. Niby są one powszechnie dostępne, ale piszesz, że możesz mieć błędy w implementacji. Poza tym możesz używać zwykłego Kalmana, EKF albo jedną z wielu innych wersji algorytmu. Żeby to sprawdzić potrzebne są fragmenty kodu i zastosowane wzory.

Co dokładnie jest twoim problemem i co dokładnie badasz w testach, które pokazujesz na wykresach. Z twojego opisu ciężko wywnioskować, czy w testach zadajesz te skokowe wartości jakoś sztucznie offsetem i czy zmiana jest odzwierciedlona w ten sam sposób na wszystkich wykorzystywanych czujnikach. Dlaczego uważasz, że algorytm nie działa poprawnie. Możliwe, że działa tak jak powinien tylko testy nie sprawdzają tego co chcesz.

Jeżeli chodzi o strojenie filtru, to możesz manipulować głównie macierzą V (kowariancja szumu procesowego). Małe wartości oznaczają, że model jest wiarygodny przez co odpowiedź na gwałtowne zmiany jest wolniejsza. Duże wartości z kolei utrudniają ustabilizowanie się estymowanych wartości. Istnieją ogólne strategie doboru kowariancji w zależności od dynamiki ruchu obiektu. Kowariancje dla poszczególnych zmiennych stanu mogą być różne i jeżeli chcesz dokładnie wystroić filtr musisz się nimi pobawić. Możesz jednak dać taką samą wartość ( większą niż przy oddzielnym strojeniu) dla wszystkich zmiennych i filtr powinien działać dobrze. Jakiś wpływ ma też wybór początkowych wartości wektora stanu i macierzy kowariancji. Wpływ wartości początkowych jest ważny tylko na początku działania filtru. Macierz W (kowariancja szumu pomiarowego) jest determinowana przez użyte czujniki i raczej nie powinna być zmieniana.

[qczek]

cześć,
Dzięki za odpowiedz. Faktycznie muszę się trochę bardziej teoretycznie wziąć za to 🙂

Na razie zrobiłem intuicyjnie coś co działa dobrze. Wyszedłem z założenia, ze odczyt z akcelerometrów jest dobry i "pozytywny" tylko wtedy kiedy obiekt porusza się z przyspieszeniem równym zero, to znaczy że długość wektora siły ciężkości jest 1g.

Wobec tego koryguje współczynnik wzmocnienia tym mocniej im bardziej długość wektora odczytanego z akcelerometrów odbiega od 1g

kf_val_t k_acc_cor = 1/exp(qsystem_parameters.kf_roll_pitch_params.gmodgain * (acc_mod-1)*(acc_mod-1));

a w całej procedurze updatu stanu (prostsza wersja) wygląda to tak

void kf_roll_pitch_update(filter_kalman_svbias_str * state, kf_val_t measurement_angle, kf_val_t measurement_angvel, kf_val_t acc_mod)
{

// caluclate kalman gain's correction based on acc_mod
kf_val_t k_acc_cor = 1/exp(qsystem_parameters.kf_roll_pitch_params.gmodgain * (acc_mod-1)*(acc_mod-1));


//prediction update - predykcja
// stan
state->angvel = measurement_angvel - state->vbias;
state->angle = state->angle + state->angvel * KF_ROLL_PITCH_STIME;
// of course state->vbias = state->vbias;


// calculate P matrix
state->p[0][0] += KF_ROLL_PITCH_STIME * (KF_ROLL_PITCH_STIME * state->p[1][1] - state->p[0][1] - state->p[1][0] + state->qacc);
state->p[0][1] -= KF_ROLL_PITCH_STIME * state->p[1][1];
state->p[1][0] -= KF_ROLL_PITCH_STIME * state->p[1][1];
state->p[1][1] += state->qbias * KF_ROLL_PITCH_STIME;

// calculate kalman gain
state->s = state->p[0][0] + state->r;
state->k[0] = state->p[0][0] / state->s;
state->k[1] = state->p[1][0] / state->s;


   // and covariance
   state->p[0][0] -= state->k[0] * state->p[0][0];
   state->p[0][1] -= state->k[0] * state->p[0][1];
   state->p[1][0] -= state->k[1] * state->p[0][0];
   state->p[1][1] -= state->k[1] * state->p[0][1];


// korekcja w zaleznosci od modułu g
state->k[0] = k_acc_cor * state->k[0];
state->k[1] = k_acc_cor * state->k[1];

// calculate angle and new bias
   state->y = measurement_angle - state->angle;
   state->angle += state->k[0] * state->y; //corection
   state->vbias += state->k[1] * state->y;


}

Robiłem także wersję z estymacją prędkości kątowej ale podobne rezultaty, być może przez brak strojenia.

Mam jeszcze tak zrobione, że odczyty w sumie napięć z acc/gyro sa puszczane przez osobne kalmany, w celu wywalenia szumu, bez wprowadzania opóźnień jak w przypadku filtrów lowpass.

Jak dokończę inną cześć projektu to wrócę do tematu z bardziej konkretnymi informacjami.

Pozdrawiam

Krzysiek