Elektroda.pl
Elektroda.pl
X
Proszę, dodaj wyjątek www.elektroda.pl do Adblock.
Dzięki temu, że oglądasz reklamy, wspierasz portal i użytkowników.

STM32F4 - Implementacja filtru Madgwicka

KFplay4free 09 Sie 2016 13:36 2646 14
  • #1 09 Sie 2016 13:36
    KFplay4free
    Poziom 3  

    Witam forumowiczów,
    Od pewnego czasu staram się zaimplementować filtr Madgwicka (MARG) dla układu AltIMU10 v4. STM32F4 - Implementacja filtru Madgwicka W Internecie nie ma niestety zbyt wiele informacji na ten temat, a ja nie mam doświadczenia w tego typu algorytmach. Wartości Pitch, Roll i Yaw, które otrzymuję, zdają się być wręcz niezależnie od położenia i często zatrzymują się na konkretnych, nieprawidłowych wartościach. Zanim zamieszczę kod, zapewnię, że przekonwertowałem dane z żyra, akcelerometru i magnetometru na odpowiednio: dps, g i uT. Zwracam się z uprzejmą prośbą do ekspertów o wskazówki na co zwrócić uwagę, żeby rozwiązać ten problem.

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Pozdrawiam i z góry serdecznie dziękuję za wszelką pomoc.

    0 14
  • #3 09 Sie 2016 18:22
    KFplay4free
    Poziom 3  

    Dzięki za link, ale z tego co widzę, w tamtej implementacji nie są w ogóle zwracane nachylenia. Poza tym, wszystko wykonane bardzo podobnie. Szczerze mówiąc, myślę, że popełniam błąd w konwersji danych wejściowych, w podawaniu argumentów funkcji (nie wiem, czy nie powinienem podawać argumentów przeciwnych), albo kalibracji (a właściwie jej braku) :(

    0
  • #4 10 Sie 2016 13:02
    KFplay4free
    Poziom 3  

    PS. Jeśli ktoś miałby chwilkę i chęć pomocy, mógłby zajrzeć do kodu i doradzić mi w skrócie - co po kolei zrobić z danymi do filtru? Dziękuję ;)
    Odnoszę się do następującej dyskusji https://www.elektroda.pl/rtvforum/viewtopic.php?p=15257292#15257292
    Chodzi o filtrację magnetometru i odpowiedni znak przed argumentami funkcji.

    0
  • #5 11 Sie 2016 14:11
    kriss68
    Poziom 20  

    Sprawdź sobie czy osie wszystkich czujników są ustawione tak jak myślisz. Ja na to sporo czasu straciłem.

    0
  • #6 11 Sie 2016 17:21
    KFplay4free
    Poziom 3  

    No właśnie o to mi chodzi. STM32F4 - Implementacja filtru Madgwicka STM32F4 - Implementacja filtru Madgwicka
    Po lewej jest akcelerometr i magnetometr, po prawej żyrko. Sprawdziłem i scalaki są tak ustawione, że osie się pokrywają. Upewnię się jeszcze, czy nie otrzymuję przeciwnych danych.
    Edit: Okazało się, że jeśli siła działa zgodnie ze zwrotem osi, czujnik zwraca wartość ujemną. Przynajmniej na akcelerometrze. Żyroskop poprawnie wskazuje obrót wokół osi X i Z, natomiast "dodatni" kierunek obrotu w nocie jest przeciwny do rzeczywistego. Magnetometru nie za bardzo potrafię przebadać. Coś pokazuje, to wiem na pewno :D . Tyle udało mi się ustalić. Tylko co w związku z tym zrobić w kodzie?

    0
  • Pomocny post
    #7 12 Sie 2016 17:22
    kriss68
    Poziom 20  

    Musisz tak pozamieniać między sobą ax ay az, mx my mz, gx gy gz żeby odpowiadały układowi który jest przyjęty w algorytmie. Jeśli znak się nie zgadza to chyba logiczne co trzeba zrobić. Co do magnetometru powinieneś dostać wektor wskazujący północ magnetyczną, skierowany lekko w dół (inklinacja magnetyczna). Polecam małymi kroczkami, najpierw wrzuć dane z akcelerometru a resztę ustaw na 0 i zobacz co się dzieje, potem dodaj do tego żyro, jeśli zacznie wariować to są pomieszane osie. Jeśli działa dodaj magnetometr, chociaż tutaj bez kompensacji hard iron będą głupoty, kompensacja soft iron też by się przydała ale to już bardziej złożone zagadnienie.

    0
  • Pomocny post
    #8 16 Sie 2016 18:00
    alexx600
    Poziom 9  

    KFplay4free napisał:
    Okazało się, że jeśli siła działa zgodnie ze zwrotem osi, czujnik zwraca wartość ujemną. Przynajmniej na akcelerometrze


    Wynika z faktu, że akcelerometr, nie mierzy przyśpieszenia, a siłę oddziaływania podłoża. Bardzo łatwo jest się, o tym przekonać jeżeli taki sensor zostanie upuszczony. W przypadku spadku swobodnego wskazania akcelerometru wynoszą zero ( chociaż siła grawitacji działa cały czas).

    Tutaj http://www.ni.com/cms/images/devzone/tut/image4795360507131540722.jpg , siła grawitacji (w domyśle ) skierowana jest w dół, oś Z akcelerometru skierowana jest ku górze. Wskazania wskazują +1 g, a nie -1g.

    Dodano po 38 [minuty]:

    KFplay4free napisał:
    Zanim zamieszczę kod, zapewnię, że przekonwertowałem dane z żyra, akcelerometru i magnetometru na odpowiednio: dps, g i uT.


    Odnośnie braku wymogu skalowania wskazań akcelerometru i magnetometru napisałem w prywatnej wiadomości jak i poście (w moim wątku);)

    Jeżeli chodzi o żyroskop.
    1. MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); kod ze strony https://github.com/kriswiner/MPU-9250/blob/master/MPU9250BasicAHRS.ino
    2. #define RATIO_GYRO ((1000./32767.)*(M_PI/180.)) fragment plik mainwindow.c ( archiwum mpu9250 OpenGL app for Window) ze strony http://www.lucidarme.me/?p=5057
    3. Notka na blogu na temat konwersji danych z żyroskopu na kwaternion http://www.lucidarme.me/?p=5299

    Nie implementowałem filtru Magdicka, a Mahonego. Po przytoczonych przykładach wydaje mi się, że na 99% w Madgwicku również powinno myć rad/s a nie dps:)

    Jeżeli do funkcji przekazywane są dane w dps (nie rad/), to zapewne "miota nim jak szatan".

    0
  • #9 21 Sie 2016 15:42
    KFplay4free
    Poziom 3  

    alexx600 napisał:
    miota nim jak szatan
    Oj miota miota :D Zaraz zupdatuję na rad/s i zrelacjonuję.

    Dodano po 3 [godziny] 43 [minuty]:

    A więc tak. Rzeczywiście do funkcji najprawdopodobniej powinny być podane wartości w rad/s. Gdy takie dane podałem, uzyskałem w pitch`u zakres do 90 stopni. Roll skacze ze 180 do -180 (?), a na Yaw nawet nie patrzę, bo nie podaje do funkcji magnetometru (na razie). Zrobiłem też tak jak kolega Kriss poradził i podałem do funkcji sam akcelerometr- na pitch`u mam około 0, roll 173, yaw 62. W takim razie wstawić odwrotny znak do osi Z?

    EDIT: Tak, zmieniłem znak i mam 0 , 0 , 180. Jestem na dobrej drodze...

    Dodano po 5 [minuty]:

    Ale gdy dodałem do tego żyro, wszystko oszalało. Pitch i Roll przy obrotach zaczynają dryfować, a Yaw ma wartość 272. Co teraz zrobić?

    0
  • Pomocny post
    #11 22 Sie 2016 20:14
    alexx600
    Poziom 9  

    KFplay4free napisał:
    Gdy takie dane podałem, uzyskałem w pitch`u zakres do 90 stopni. Roll skacze ze 180 do -180 (?), a na Yaw nawet nie patrzę, bo nie podaje do funkcji magnetometru (na razie).


    Hmh, właśnie z kątami Eulera są pewne niedogodności. Prawie zawsze jest inaczej począwszy od kolejności wykonywanych obrotów ( XYZ, ZYX, etc..) kończąc na zakresie poszczególnych liczb. Jest jeszcze kwestia intrinsic i extrinsic...

    Generalnie korzystając z funkcji asin(), atan(), acos() otrzyma się kąty w zakresie ±90°,aby uzyskać pełny zakres korzysta się z funkcji atan2() zakres ±180°.

    Tutaj Kwaterniony w praktyce

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Tutaj OpenFrameworks
    Kod: c
    Zaloguj się, aby zobaczyć kod

    A tutaj
    LM303DLH
    Kod: c
    Zaloguj się, aby zobaczyć kod


    W kodzie z pierwszego postu jest
    Kod: c
    Zaloguj się, aby zobaczyć kod


    Więc zakresy stopni się zgadzają.

    KFplay4free napisał:
    Roll skacze ze 180 do -180 (?).

    Nie wiem czy odpowiednio to rozumiem. Roll raz wynosi -180° a następnym razem +180°, to przecież do samo. Ale coś duża ta wartość.

    Ja na początek spróbowałbym wyobrazić sobie te dane, a właściwie zobrazował je
    Tutaj GoogleTechTalks w 44:42 masz kod odpowiedzialny za wyświetlanie i rotację w opengl z wykorzystaniem quaternionu. Sposób uniewrsalny może zostać wszędzie zaimplementowany.

    Dodano po 30 [minuty]:

    KFplay4free napisał:
    Zrobiłem też tak jak kolega Kriss poradził i podałem do funkcji sam akcelerometr


    Osobiście nigdy nie testowałem, więc nie jestem pewny czy to zadziała, ale jeżeli jest tego potwierdzenie to zapewne tak jest:)


    Sprawdziłem, kod w pliku MadgwickAHRS.m wygląda tak
    Kod: matlab
    Zaloguj się, aby zobaczyć kod


    W przypadku podania zerowych wartości do danych żyroskopu qDot zależy wyłącznie od F oraz J.
    F zależy od wskazań akcelerometru (Magiczne obliczenia w F wynikają z kompensacji grawitacji gravity compensation).
    Od czego zależy J, nie wiem, może kolega Kriss się orientuje?:)

    KFplay4free napisał:
    podałem do funkcji sam akcelerometr- na pitch`u mam około 0, roll 173, yaw 62. W takim razie wstawić odwrotny znak do osi Z?

    EDIT: Tak, zmieniłem znak i mam 0 , 0 , 180. Jestem na dobrej drodze...


    Jeżeli na wejście filtru podawany jest wyłącznie akcelerometr, niczego sensownego w Yaw nie można się spodziewać. Akcelerometr nie umożliwia wyznaczenia kąta obrotu wokół osi globalnej Z.

    Ja zacząłbym od samego akcelerometru, dopiero później dołączył żyroskop. Można spróbować również skorzystać z takiej sztuczki Link. W tym wypadku aby otrzymać kwaternion, należy podać dwa wektory jeden globalne przyśpieszenie (0,0,1) oraz znormalizowany wektor (ax,ay,az).

    0
  • #12 24 Sie 2016 15:09
    KFplay4free
    Poziom 3  

    Powiem szczerze, że chyba nie jestem w stanie rozwiązać tego problemu. Próbowałem przestudiować kwaterniony, ale nieznajomość "niższych" zagadnień (idę dopiero do liceum) sprawia, że jest to dla mnie czarna magia, a drogą eksperymentowania daleko nie zajdę. :( Za to obejrzałem wykład z GoogleTechTalks i trzeba przyznać, że był znakomity. Tak naprawdę mógłbym podjąć się napisania takiego filtru całkującego dane z żyroskopu i kompensującego dryf wektorem obliczonym z akcelerometru, ale nie wiem czy jest sens robienia tego na kątach Eulera?
    Pozdrawiam

    0
  • Pomocny post
    #13 25 Sie 2016 15:57
    alexx600
    Poziom 9  

    KFplay4free napisał:
    Powiem szczerze, że chyba nie jestem w stanie rozwiązać tego problemu.


    Proponowałbym zacząć od samego akcelerometru i od wysłania kwaternionu do komputera. Tak aby mieć możliwość zobrazowania obrotu.
    Rozbiłbym to na dwa zagadnienia.
    1. Tworzenie kwaternionu
    2. Wizualizacja
    Ad 1. Quaternion z danych pochodzących samego akcelerometru
    Przykład z Tutaj
    Kod: c
    Zaloguj się, aby zobaczyć kod

    Pierwszy z wektorów stanowi wektor globalny przyśpieszenia u=(0,0,1).
    Wektor v to znormalizowany wektor (wersor) przyśpieszenia. Oznacza to tyle, że
    należy obliczyć długość wektora,
    Kod: c
    Zaloguj się, aby zobaczyć kod

    ,a następnie każdy z elementów wektora przyśpieszenia (ax,ay,az) podzielić przez obliczoną długość
    Kod: c
    Zaloguj się, aby zobaczyć kod

    dot product i cross produkt to odpowiednio iloczyny skalarny i wektorowy. Dopiero w liceum się o tym mówi.
    Link
    czyli przykładowo
    dot ( u,v)=u(1)*v(1)+u(2)*v(2)+u(3)*v(3);
    w naszym przypadku;
    dot ( u,v)=0*ax+0*ay+1*az=az;
    iloczyn wektorowy jest bardziej skomplikowany
    ogólnie wygląda tak
    cross(u,v)
    w(1) = u(2)*v(3)-u(3)*v(2);
    w(2) = u(3)*v(1)-u(1)*v(3);
    w(3) = u(1)*v(2)-u(2)*v(1);
    w analizowanym przypadku
    w.x = -1*ay
    w.y = 1*ax
    w.z = 0
    Ogólnie, tak jak widać dużo się poskraca. Myślę, że bez problemu można to zaimplementować w czystym C, bez pseudokodu.
    Na koniec taki quaternion musi zostać znormalizowany. Postępowanie podobnie jak z wcześniejszym wektorem v.
    Kod: c
    Zaloguj się, aby zobaczyć kod

    Następnie wysłanie quaternionu w postaci
    Kod: c
    Zaloguj się, aby zobaczyć kod

    Przy pomocy funkcji cross i dot, zostają pominięte funkcje trygonometryczne. Kosztem wykorzystania oczywiście funkcji pierwiastkowania.
    Można również robić inaczej, tzn. stworzyć kwaternion na podstawie kątów Eulera. W tym wypadku z funkcji trygonometrycznych należałoby wyznaczyć kąty Pitch oraz Roll. Na ich podstawie obliczyć kwaterniony. Następnie je wymnożyć. Opisywany wcześniej proces z wykorzystaniem dwóch wektorów jest znacznie prostszy.

    AD 2. Wizualizacja danych
    Na hackaton w Gdańsku wraz z przyjaciółmi przerobiliśmy na szybko przykład z lucidarme. Było mało czasu, więc projekt nie jest ładny-ale działał. Zamieszczę go w załączniku.

    Kod jest przerobiony tak, aby otrzymywać dane z serial w opisywanej wcześniej postaci.

    KFplay4free napisał:
    Za to obejrzałem wykład z GoogleTechTalks i trzeba przyznać, że był znakomity.
    Film nie jest zły i tłumaczy rzeczy w przystępny sposób, też mi się podobał.
    KFplay4free napisał:
    idę dopiero do liceum
    Trochę będziesz musiał się sam pouczyć. Daj znać czy coś wyszło:)

    0
  • #14 25 Sie 2016 21:45
    KFplay4free
    Poziom 3  

    Bardzo dziękuję Ci za wytłumaczenie tego sposobu z wektorami. Właściwie to wcale nie takie skomplikowane, jak ma się solidny materiał do nauki. Spróbuję wysłać kwaterniony z akcelerometru do PC i pokażę co uzyskałem. Małymi kroczkami i do celu. Serdecznie pozdrawiam, i jeszcze raz wielkie dzięki za poświęcenie mi tyle czasu.

    0
  • #15 30 Sie 2016 18:44
    KFplay4free
    Poziom 3  

    Odebrałem kwaterniony:

    Code:
    0.024229   -0.86446   0.502113   0
    
    0.022783   -0.99555   0.091483   0
    0.022616   -0.98688   0.159885   0
    0.029931   -0.92663   0.374772   0
    0.019435   -0.95165   -0.30657   0
    0.034155   -0.99942   0   0
    0.018643   -0.75865   0.651231   0
    0.036383   -0.96461   0.261175   0
    0.010495   -0.9983   -0.05737   0
    0.028228   -0.99952   -0.01276   0

    Kod programu:
    Kod: c
    Zaloguj się, aby zobaczyć kod

    Kod: c
    Zaloguj się, aby zobaczyć kod

    Pętla główna:
    Kod: c
    Zaloguj się, aby zobaczyć kod

    0