Filtr Kalmana - jak zacząć?

[scilent]

Witam,
zabrałem się za budowę układu sterowania silnikami bezszczotkowymi w oparciu o MPU6050 (typowy quadro). Do tego zadania najlepiej sprawdzi się prosty i skuteczny filtr Kalmana. Pomimo, że mam trochę wiadomości z teorii sterowania, to potrzebuję pomocy w toku rozumowania.

Znalazłem taki zestaw pytań, na który przydaje się odpowiedzieć zabierając się za Kalmana:

- jakie zmienne stanu wykorzystujesz: chcę uzyskać trzy kąty obrotu: dla każdej osi prędkość obrotowa z żyro i odpowiadające przyspieszenie liniowe (jako wpływ grawitacji) oraz ich szumy; na forum można znaleźć kilka przykładów i tym będę się posiłkował.

- czy używasz liniowego modelu: dobre pytanie 🤣 jeśli dobrze rozumiem, obecność sinusów i cosinusów oznacza negatywną odpowiedź, ale jeśli w tym rzecz, to myślę tak - robot nie pochyli się o więcej niż 20stopni, dla których sin(x)=x z błędem poniżej 5% 😅

- czy twój model uwzględnia pracę silników czy wszystkie zmiany pozycji/prędkości/przyspieszenia traktowane są jako szum: no właśnie, w jaki sposób uwzględnić pracę silników? Oczywiście zmiany orientacji są pożądane. Przykładowo, do równania orientacji kątowej dodać +k*m, gdzie k to stała do dopasowania, a m to np obroty silników odpowiadających za obrót? Oczywiście nie mam informacji o obrotach, posiadam jakąś wartość sygnału podawanego na silniki.

- jaką postać mają macierze A B i C modelu, w jaki sposób dobrałeś macierze szumu pomiarowego i procesowego: chyba za wcześnie na to pytanie 🤣 muszę być pewien, że wszystko gra i nie pomijam żadnych zmiennych.

- jaki jest czas próbkowania - czy dla wszystkich pomiarów jest taki sam i czy obliczasz stany pośrednie pomiędzy pomiarami: czas próbkowania zawsze taki sam dzięki używaniu timerów, stanów pośrednich nie obliczam i nie widzę nawet sensu, gdyż dane z Kalmana i tak posłużą jako wejście PIDa (i wtedy będę się martwił)

- jakie użyłeś wartości początkowe stanu i wariancji: po uruchomieniu zeruję żyroskop i akcelerometr, wtedy jest czas na wszystkie wartości początkowe, że tak powiem; jedyny problem to ustawienie quadro na idealnie poziomej płaszczyźnie 😕

Podsumowując, na razie mam problem, żeby w ogóle ruszyć z rozpisaniem równań, uwzględniających wszystko, co potrzebne do quadro. Dlatego za wszelką pomoc bardzo dziękuję. Mile widziane materiały rozjaśniające temat, albo same przykłady zarówno w języku C jak w języku matematyki 🙂🙂

Być może czegoś jeszcze nie rozumiem, gdzieś napisałem totalną głupotę, ale o to chodzi, żeby się uczyć na błędach. 😅

[Lukaszm]

czy twój model uwzględnia pracę silników czy wszystkie zmiany pozycji/prędkości/przyspieszenia traktowane są jako szum: no właśnie, w jaki sposób uwzględnić pracę silników? Oczywiście zmiany orientacji są pożądane. Przykładowo, do równania orientacji kątowej dodać +k*m, gdzie k to stała do dopasowania, a m to np obroty silników odpowiadających za obrót? Oczywiście nie mam informacji o obrotach, posiadam jakąś wartość sygnału podawanego na silniki.

Myślisz w dobrym kierunku. System (filtr, a w zasadzie obserwator) będzie się lepiej zachowywał jeśli uwzględnisz w nim wejścia układu (które znasz). Problem jest taki, że musiałbyś znać siły generowane przez silniki, co może być kłopotliwe i co najwyżej uda się uzyskać tylko przybliżone wartości.

jakie użyłeś wartości początkowe stanu i wariancji: po uruchomieniu zeruję żyroskop i akcelerometr, wtedy jest czas na wszystkie wartości początkowe, że tak powiem; jedyny problem to ustawienie quadro na idealnie poziomej płaszczyźnie 😕

Początkowa estymata stanu a jakiekolwiek kalibracje/zerowanie sensorów to dwie różne rzeczy. Estymatę (czyli 3 kąty jak rozumiem) w chwili 0 możesz sobie spokojnie przyjąć jako zera. Wariancja estymacji - to już eksperymentalnie, ale i tak w momencie startu systemu copter sobie stoi na podłożu, więc nie powinno być tutaj jakichś problemów.

[scilent]

Dzęki za odpowiedź. Niestety musiałem zrezygnować z pomysłu, gdyż mam określony czas na obliczenia na arduino nano (rzędu kilku ms), a liczba użytych zmiennych float w Kalmanie powoduje, że mocno ten limit przekraczam 😃 .

Wykorzystałem więc filtr komplementarny, wyniki są zadowalające w moim przypadku - steruję dużym dronem i nie planuję robić beczki 🙂 . Dzięki temu obliczenia na zmiennych float zajmują niecałą milisekundę (procek 16MHz).