explosion95

Lukaszm, bardzo dziękuję i za odpowiedź, i za propozycje! Jednakże zanim do nich przystąpiłem, wydaje mi się, że odnalazłem potencjalne źródło błędów! A przyczyna (jeśli to ta) okazała się taka prosta... w wzorach używanych w filtrze przeliczam wszystko z użyciem wartości deltaT, tylko że mam ją w mikrosekundach... stąd możliwe że wartości są akurat 10^6 większe niż poprawne. Po poprawce chyba jest w porządku W tym momencie chciałbym spytać, jako że w ogóle nie mam wyczucia i doświadczenia, czy są jakieś ogólne wskazówki co do oszacowania lub dobrania wartości macierzy kowariancji szumów Q i R (albo według innych zródeł V i W)? Czy R powinno być duże i czy Q powinno mieć wartości tylko na przekątnej? Czy wartości na przekątnej Q mają być takie same czy różne? Spotkałem się z wieloma sposobami Pytam i proszę gdyż gdyż filtr zachowuje się bardzo dziwnie, wpada w sinusoidalne oscylacje o zmieniającej się samoistnie częstotliwości. Na obecną chwilę nie potrafię określić reguły jaka odpowiada za takie zachowanie gdy zmieniam parametry szumów..

Cześć Na swojej płytce Arduino UNO wraz z dołączonym modułem MPU-6050 zaimplementowałem ostatnio filtr Kalmana wyliczający odchylenie kątowe kąta Pitch, w sumie wyszło pewnie identycznie do jednego z przykładów zamieszczonych w publikacji pana Kędzierskiego z PWR. Problem w tym, że filtr nie działa całkowicie poprawnie a nie umiem znaleźć przyczyny. Poniżej pozwolę sobie wkleić całość kodu, który zawiera m.in. dokładnie opisane modele we/wy, macierze itd, a prócz tego jakieś zrzuty ekranu z tego co dzieje się obliczonym w FK kątem. Przepraszam, że kod jest długi, ale jest chyba całkiem znośnie skomentowany, filtr znajduje się mniej więcej od połowy w dół, a zamieściłem cały kod gdyż może błąd jest gdzieś poza filtrem? Tylko że filtr komplementarny, który też tam mam działa poprawnie... /* *Kod pobierania danych z modulu MPU-6050 bez uzycia zewnetrznych bibliotek *Jedyna stosowana biblioteka to <Wire.h> do komunikacji I2C z modułem pomiarowym * *Zakres pomiarowy zyroskopu ustawiam chyba na +/- 500deg/s *Zakres pomiarowy akcelerometru ustawiam chyba na +/-4g * *Moduł MPU-6050 pozwala też na pomiar i odczytywanie temperatury *Wartosci są schowane w rejestrach 0x41 i 0x42 * *Wszelkie dane odnosnie dzialania i inicjowania pracy modulu, mapy rejestrow dostepne *są w nocie katalogowej MPU-6050 */ //////////////////////// // POCZATEK PROGRAMU // //////////////////////// #include <Wire.h> int kalibracja = 0; // zmienna potrzebna do kalibracji zyroskopu float deltaT=0; // wazna zmienna, definiuje czas miedzy pomiarami int zliczanie =0; long raw_acc_X, raw_acc_Y, raw_acc_Z; // surowe dane z rejestrow akcelerometru long sum_gyro_X, sum_gyro_Y, sum_gyro_Z; // zmienne do sumowania odczytow z zyroskopu //long loop_timer; // do mierzenia czasu trwania petli long acc_vector; // zmienna do liczenia Pitagorasa z skladowych akcelerometru unsigned long czas1, czas2 =0; // do mierzenia czasu trwania petli i liczenia wartosci deltaT int raw_gyro_X, raw_gyro_Y, raw_gyro_Z; // surowe dane z rejestrow gyroskopu float gyro_pitch, gyro_roll, gyro_yaw; // zmienne na katy wyliczone z pomiarow zyroskopu float acc_pitch, acc_roll, acc_yaw; // zmienne na katy wyliczone z pomiarow akcelerometru float output_pitch, output_roll, output_yaw; // zmienne przechowujace ostatecznie wyliczone katy float pitch_spirit_level = 2.5; // 1.34SPRAWDZ odczyty z acc na poziomicy float roll_spirit_level = -3.0; //-4.33 i wpisz do tych zmiennych!!!! boolean first_cycle = true; // przy pierwszym wzieciu danych kat zyroskopu jest // kątem akcelerometru double liczba = 0.000000000026645; //////////////////////////////// // ZMIENNE DO FILTRU KALMANA // // ///////////////////////////////////////////////////////////////////////////////////// float kalman_pitch = 0; float acc_pitch1, gyro_drift2, gyro_drift1 = 0; // wyzerowane zmienne dla pierwszej petli obliczen float P1_11, P1_22 = 1; // elementy macierzy kowariancji P a posteriori float P1_12, P1_21 = 0; float Q_11, Q_22 = 0.0001; // elementy macierzy kowariancji szumu procesowego float Q_12, Q_21 = 0; float A_11, A_T_12 = 1; //elementy macierzy wejścia A oraz macierzy transponowanej wejścia A^T float A_12, A_T_11 = -deltaT; float A_21, A_T_22 = 0; float A_22, A_T_21 = 1; float P2_11, P2_12, P2_21, P2_22 = 0; // elementy macierzy kowariancji P a priori float R = 10; //kowariancja szumu pomiarowego float H_1, H_T_2 = 1; //elementy macierzy przejścia H i macierzy transponowanej przejścia H^T float H_2, H_T_1 = 0; float K_1, K_2 = 0; //wektor wzmocnienia Kalmana, jest 2x1 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////// // PETLA JEDNORAZOWA // ///////////////////////// void setup() { Serial.begin(115200); // rozpoczecie komunikacji szeregowej Wire.begin(); // zainicjowanie komunikacji Wire SetupMPU(); // zainicjowanie stanu MPU przed praca GyroCalibration(); // kalibracja zyroskopu, usrednienie 2000 wynikow } //////////////////////// // PETLA GLOWNA // //////////////////////// void loop() { zliczanie = zliczanie +1; czas1=micros(); RecordGyroRegisters(); // pobierz dane z zyroskopu raw_gyro_X=raw_gyro_X - sum_gyro_X; // odejmij od pobranych danych wartosci raw_gyro_Y=raw_gyro_Y - sum_gyro_Y; // obliczone przy kalibracji raw_gyro_Z=raw_gyro_Z - sum_gyro_Z; ProcessGyroData(); // przelicz dane z zyroskopu // na cos wartosciowego RecordAccelRegisters(); ProcessAccelData(); ComplementaryFilter(); KalmanFilter(); if(zliczanie == 5) { PrintData(); zliczanie = 0; } czas2=micros(); // wazna linijka, tu sie oblicza deltaT=czas2-czas1; // czas wykonania petli sterujacej } //////////////////////// // KONIEC PETLI // //////////////////////// void SetupMPU() // funkcja ustawienia wartosci poczatkowych w MPU { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x6B); // ustawiamy dostęp do rejestru 6B by ustawić tryb pracy Wire.write(0b00000000); // rejestr czuwania wylaczony itp Wire.endTransmission(); // zakonczenie transmisji Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x1B); // rejestr 1B do konfiguracji pracy zyroskopu Wire.write(0b00001000); // tryb pracy do +/- 500 deg/s Wire.endTransmission(); // zakonczenie transmisji Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x1C); // rejest 1C do konfiguracji pracy akcelerometru Wire.write(0b00001000); // tryb pracy do +/- 4g Wire.endTransmission(); // zakonczenie transmisji } void GyroCalibration() { for (int kalibracja = 0; kalibracja < 1000 ; kalibracja ++) { RecordGyroRegisters(); // w każdej petli odczytaj wartosci z zyroskopu sum_gyro_X = sum_gyro_X+raw_gyro_X; // w kazdej petli w zmiennej sum_gyro_X sumuj kolejne pomiary sum_gyro_Y = raw_gyro_Y+sum_gyro_Y; // w kazdej petli w zmiennej sum_gyro_Y sumuj kolejne pomiary sum_gyro_Z = raw_gyro_Z+sum_gyro_Z; // w kazdej petli w zmiennej sum_gyro_Z sumuj kolejne pomiary delay(3); // Delay 3us to simulate the 250Hz program loop } sum_gyro_X = sum_gyro_X/1000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X sum_gyro_Y = sum_gyro_Y/1000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X sum_gyro_Z = sum_gyro_Z/1000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X } void RecordAccelRegisters() { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x3B); // ustawiam rejest 0x3B do danych akcelerometru Wire.endTransmission(); // zakonczenie transmisji Wire.requestFrom(0b1101000,6); // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc while(Wire.available()<6); raw_acc_X=Wire.read()<<8|Wire.read(); // dane acc_x/y/z przechowywane sa w 8bitowych rejestrach raw_acc_Y=Wire.read()<<8|Wire.read(); // od adresu 0x3B do 0x40 wlacznie i wlasnie je pobieramy raw_acc_Z=Wire.read()<<8|Wire.read(); // i zapisujemy do zmiennych, na kazda os 2 bajty danych } void ProcessAccelData() { //z Pitagorasa przeliczam dlugosc calkowita wektora sily ciezkosci acc_vector = sqrt((raw_acc_X*raw_acc_X)+(raw_acc_Y*raw_acc_Y)+(raw_acc_Z*raw_acc_Z)); acc_pitch = asin((float)raw_acc_Y/acc_vector)* 57.296; acc_roll = asin((float)raw_acc_X/acc_vector)* -57.296; // wzory obok sa by obliczyc kąty pomiedzy skladowymi sil // a te katy to jednoczesnie kąty obrotu modulu acc_pitch = acc_pitch - pitch_spirit_level; acc_roll = acc_roll - roll_spirit_level; } void RecordGyroRegisters() { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x43); // ustawiam rejest 0x43 do danych zyroskopu Wire.endTransmission(); // zakonczenie transmisji Wire.requestFrom(0b1101000,6); // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc while(Wire.available()<6); raw_gyro_X=Wire.read()<<8|Wire.read(); // dane gyro_x/y/z przechowywane sa w 8bitowych rejestrach raw_gyro_Y=Wire.read()<<8|Wire.read(); // od adresu 0x43 do 0x48 wlacznie i wlasnie je pobieramy raw_gyro_Z=Wire.read()<<8|Wire.read(); // i zapisujemy do zmiennych, na kazda os 2 bajty danych } void ProcessGyroData() // deltaT to czas probkowania ~2500us = 2.5ms = 0.0025s { // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji gyro_pitch=gyro_pitch+(raw_gyro_X * (deltaT/65500000)); // robimy calkowanie by dostac wartosc kąta deg //sprawdz wzory uwzgledniajac deltaT!!!! gyro_roll=gyro_roll+(raw_gyro_Y * (deltaT/65500000)); // gyro_yaw=gyro_yaw+(raw_gyro_Z * (deltaT/65500000)); // wyniki bedace wychyleniem katowym z pomiarow zyroskopu gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*deltaT*liczba); // gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*deltaT*liczba); // tymi obliczeniami likwidujemy gimbal lock (nieprawda ale skrot myslowy) } void ComplementaryFilter() { if (first_cycle==true) // warunek spełniony wylacznie raz // to wczesniej bylo w funkcji ComplementaryFilter { // w pierwszym obiegu sterowania // na samym poczatku gyro_pitch = acc_pitch; // zeby wpisac do gyro polozenie poczatkowe gyro_roll = acc_roll; // z pomiarow akcelerometru first_cycle = false; output_pitch = gyro_pitch; // w pierwszej petli wpisujemy to output_roll = gyro_roll; // bezposrednio na wyjscie } // FILTR KOMPLEMENTARNY DLA KĄTÓW PITCH I ROLL output_pitch = 0.97 * output_pitch + 0.03 * acc_pitch; output_roll = 0.97 *output_roll + 0.03 * acc_roll; output_yaw = gyro_yaw; } // FILTR KALMANA DLA ESTYMACJI KĄTA PITCH void KalmanFilter() { ////////////////////////////////////////////////////////////////////////// // OPIS GŁÓWNY RÓWNAN I MACIERZY UZYTYCH DO FILTRU KALMANA // // FILTR TEN MA ZA ZADANIE ESTYMOWAC KĄT OBROTU PITCH NA PODSTAWIE // // WSKAZAN AKCELEROMETRU I ZYROSKOPU MPU 6050 ///////////////////////////////////////////////////////////////////////////////////// // // Adnotacja i komentarze: starałem sie jak moglem wszystko opisać ale nie da sie, ponizej znajdują sie wzory wykorzystywane na kazdym etapie // obliczen ale takze wymnażane recznie "na piechotę" przez siebie poszczególne elementy różnych macierzy. To dlatego bo uC nie ma // zaimplementowanych bibliotek do liczenia macierzy wiec trzeba to robić recznie // jakby cos, to wszystko robilem na kartce, i tam tez upraszczalem, stad niektore wzory (np na elementy macierzy kowariancji P2) // moga byc niejasne, bo po prostu są juz uproszczone, wynikają mimo to z poprawnych (oby) przeksztalcen wedlug zasad działan na macierzach // // // x2 = A*x1 + B*u + w; // w to szum procesowy // z = H*x2 + v; // x2 to stan x a priori, A i B to macierze (wejscia i sterowania), u to wektor sterowania, x1 to stan a posteriori // // v to szum pomiarowy, H to macierz przejscia (wyjścia) z procesu do pomiaru, z to pomiar // // // x to stan, macierz [acc_pitch2; gyro_drift2] stosujemy trick ze dryft jest nasza zmienna stanu // // //RÓWNANIA NASZEGO STANU // // kalman_pitch = acc_pitch1 +gyro_pitch*deltaT - gyro_drift1*deltaT; // rownanie estymowanego kąta obrotu // gyro_drift2 = gyro_drift1; // zakladamy ze dryft jest staly // // x = [kalman_pitch; gyro_drift2] // to jest stan naszego systemu // // dany wektorem z dwiema zmiennymi stanu // // czyli kątem i dryftem // // u = gyro_pitch // predkosc katowa z zyroskopu jest // // wektorem sterujacym układem // // A = | 1 -deltaT | // macierz wejścia A // | 0 1 | // // // B = | deltaT | // macierz sterowania B // | 0 | // // H = | 1 0 | // macierz wyjścia H // // R = | 1 | // macierz kowariancji szumu pomiarowego // // Q = | 1 0 |*q // Q to macierz kowariancji szumu procesowego // | 0 1 | // q doświadczalnie 0.0001 // // P = | P_11 P_12 | // macierz kowariancji // | P_21 P_22 | // /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////// // ETAP PREDYKCJI // // /////////////////////////////////////////////////////////////////////////////////////////// // // x2 = A * x1 + B * u; //predykcja nastepnego stanu systemu na podstawie poprzedniej estymaty // P2 = A * P1 * A^T + Q; //predykcja nowej macierzy kowariancji na podstawie poprzedniej // //macierzy kowariancji szumu procesowego Q // // RÓWNANIE PREDYKCJI NA POLOZENIE KATOWE WOKOL OSI X (PITCH) kalman_pitch = acc_pitch1 +raw_gyro_X*deltaT/65.5 - gyro_drift1*deltaT; // predykcja nowego kąta pitch na podstawie poprzedniego gyro_drift2 = gyro_drift1; // oraz predkosci pochodzącej z zyroskopu // raw_gyro_X/65.5 to predkosc obrotowa w deg/s wg noty // // katalogowej MPU 6050 // P1_11 = 1; Q_11 = 0.0001; // P1_12 = 0; Q_12 = 0; // P1_21 = 0; Q_21 = 0; // P1_22 = 1; Q_22 = 0.0001; // // A_11 = 1; A_T_11 = -deltaT; // A_12 = -deltaT; A_T_12 = 1; // A_21 = 0; A_T_21 = 1; // A_22 = 1; A_T_22 = 0; // // // RÓWNANIE PREDYKCJI NA NOWA MACIERZ KOWARIANCJI (rozpisane na kartce a poniżej juz uproszczone) // P2_11 = ((A_11*P1_11 + A_12*P1_21)*A_T_11 + (A_11*P1_12 + A_12*P1_22)*A_T_21) + Q_11; // P2_12 = ((A_11*P1_11 + A_12*P1_21)*A_T_12 + (A_11*P1_12 + A_12*P1_22)*A_T_22) + Q_12; // P2_21 = ((A_21*P1_11 + A_22*P1_21)*A_T_11 + (A_21*P1_12 + A_22*P1_22)*A_T_21) + Q_21; // P2_22 = ((A_21*P1_11 + A_22*P1_21)*A_T_12 + (A_21*P1_12 + A_22*P1_22)*A_T_22) + Q_22; // ELEMENTY MACIERZY KOWARIANCJI PO ETAPIE PREDYKCJI P2_11 = ((P1_11 + (A_12*P1_21))*A_T_11 + (P1_12 + (A_12*P1_22))) + Q_11; P2_12 = P1_11 + (A_12*P1_21); P2_21 = P1_21*A_T_11 + P1_22; P2_22 = P1_21 + Q_22; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ////////////////////////////////////////////////////////////////////// // // // ETAP KOREKCJI // // ////////////////////////////////////////////// // // // OBLICZENIA WZMOCNIENIA KALMANA // // K = P2 * H^T * (H * P2 * H^T + R)^1 // wzór na okreslenie wzmocnienia Kalmana dla // x2 = x2 + K*(z - H*x2) // obecnej iteracji // P2 = ( I - K*H) *P2 // z to acc_pitch czyli odczyt polozenia z acc // // // K_1 = P2_12 * (1/(P2_12+R)); // juz uproszczone wzory na poszczegolne elementy K_2 = P2_22 * (1/(P2_12+R)); // macierzy wzmocnienia Kalmana // ETAP KOREKCJI I RÓWNANIA KOREKCYJNE DLA STANU UKŁADU kalman_pitch = kalman_pitch + K_1*(acc_pitch-kalman_pitch); // etap korekcji i okreslenie nowego stanu gyro_drift2 = gyro_drift2 + K_2*(acc_pitch-kalman_pitch); // wedlug oficjalnych wzorów dla filtru Kalmana // ETAP KOREKCJI I RÓWNANIE KOREKCYJNE MACIERZY KOWARIANCJI P2_21 = K_2*P2_11 - P2_21; P2_11 = (K_1 - 1)*P2_11; P2_12 = (K_1 - 1)*P2_12; P2_22 = -P2_22; // NA KONIEC AKTUALIZACJA ZMIENNYCH I ZAMKNIECIE PETLI P1_11 = P2_11; P1_12 = P2_12; // nasza najnowsza macierz kowariancji P1_21 = P2_21; // staje się poprzednia i idzie do P1_22 = P2_22; // nowej petli obliczen acc_pitch1 = kalman_pitch; // nasz najnowszy najlepszy stan gyro_drift1 = gyro_drift2; // staje sie poprzednim stanem // i trafia do nowej petli obliczeń } //KONIEC FILTRU KALMANA void PrintData() { // funkcja do wyswietlania wartosci na monitor Serial.print(kalman_pitch); Serial.println(); } Nie rozumiem powodu dla którego filtr oblicza końcowy wynik na tak ogromną liczbę, która nie mieściła się w zmiennej float: Natomiast po zmianie wszystkich zmiennych używanych w filtrze na zmienne typu long otrzymałem wykresy: Skąd wzięły się tak duże wartości kąta? Jak widać na powyższym rysunku, charakter zmian kąta jest zgodny z ruchem modułu, tylko wartości są rzędu 10^7, a ponadto na ostatnim zdjęciu widać, że w bezruchu filtr wykazywał stałe płynięcie i wzrost wartości kąta. Czy jest szansa, by zechciał ktoś zerknąć..? Skoro wartości zmieniają się zgodnie z ruchem modułu, to może rozwiązanie jest proste? Czy może być to kwestia źle ustawionych wartości początkowych? Albo źle wpisanych wartości do macierzy szumów Q i R?

EasyPeasy_, dzięki za angielski link, treść wygląda bardzo przystępnie a publikacja Pana Kędzierskiego jest mi już dość dobrze znana, więc zajmę się nim w pierwszej kolejności no i oczywiście przypilnuję rzeczy, na które zwróciłeś uwagę Pozdrawiam, bardzo dziękuję Wam za wszelką pomoc!

No przecież, że tak... dzięki Ci bardzo! Niby drobnostka, a ważna Dobrze wiedzieć, muszę bliżej zapoznać się z notą gdyż najwyraźniej nie rozumiem działania układu, zajmując się kilkoma aspektami naraz czasami zakładam, że coś działa i już, nie zawsze mam czas by dojść jak coś funkcjonuje, szczególnie gdy do obecnych potrzeb wystarcza to co wiem Tak jak wyżej, z niewiedzy założyłem, że taktowanie MPU jest zależne od częstotliwości pobierania danych przez uC w pętli sterującej. W sumie to totalnie nie miało sensu znajdę chwilkę to nadrobię braki żeby lepiej wszystko zrozumieć, szkoda Twojego czasu na poprawianie wszystkich moich błędów EasyPeasy_ No pewnie! Od dawna wiedziałem, że prędzej czy później zajmę się tym filtrem więc bardzo chętnie! Do tej pory zrozumienie FK mnie przerastało.. Kilka razy się do tego zabierałem, nawet korzystałem z przystępnych artykułów na forbocie, ale słabo mi poszło, a teoria i wzory mogą faktycznie troszkę odstraszyć Wszystko się zgadza, akurat brak zewnętrznego magnetometru spowodował, że nie mam żadnej kompensacji dryftu na tym kącie, chociaż nie jest on krytyczny z perspektywy moich obecnych i najbliższych potrzeb. Jednak mam świadomość, że MPU-6050 umożliwia wpięcie go do układu i wykorzystanie jego danych do estymacji kąta Yaw Dzięki za pomocne odpowiedzi!

Taaak, spotkałem się z nią, bardzo popularna, szkoda tylko że na moim arduino nie chciała się kompilować... ale nic straconego, więcej się nauczyłem Ooo, to chyba nie jest mi znane. Na bazie przerwań z MPU? to nie są one regulowane linią zegarową SCL wychodzącą z arduino? czyli tożsamą z częstotliwością wykonywania pętli? Jeśli mowa o dodatkowym wyjściu modułu INT to zwarłem go do masy. Obecny kod mam rozpisany na zmienny czas wykonania pętli i na obecną chwilę po obliczeniach kątów i filtracji wychodzi około 2600us, czyli mam jeszcze sporo zapasu na następne obliczenia Jak zwykle masz rację, różnica między stosowaniem funkcji a niestosowaniem jest znikoma, właściwie żadna a obliczenia na floatach czy zmiennych double istotnie trwa dość długo, lepiej kiedy tylko się da to takich obliczeń unikać void ProcessGyroData() // deltaT to czas probkowania ~2500us = 2.5ms = 0.0025s { // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji gyro_pitch=gyro_pitch+(raw_gyro_X * (deltaT/65500000)); // robimy calkowanie by dostac wartosc kąta deg //sprawdz wzory uwzgledniajac deltaT!!!! gyro_roll=gyro_roll+(raw_gyro_Y * (deltaT/65500000)); // gyro_yaw=gyro_yaw+(raw_gyro_Z * (deltaT/65500000)); // wyniki bedace wychyleniem katowym z pomiarow zyroskopu gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*deltaT*liczba); // gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*deltaT*liczba); // tymi obliczeniami likwidujemy gimbal lock } void ComplementaryFilter() { if (first_cycle==true) // warunek spełniony wylacznie raz { // w pierwszym obiegu sterowania gyro_pitch = acc_pitch; // zeby wpisac do gyro polozenie poczatkowe gyro_roll = acc_roll; // z pomiarow akcelerometru first_cycle = false; output_pitch = gyro_pitch; // w pierwszej petli wpisujemy to output_roll = gyro_roll; // bezposrednio na wyjscie } // FILTR KOMPLEMENTARNY DLA KĄTÓW PITCH I ROLL output_pitch = 0.97 * output_pitch + 0.03 * acc_pitch; output_roll = 0.97 *output_roll + 0.03 * acc_roll; output_yaw = gyro_yaw; } void PrintData() { // funkcja do wyswietlania wartosci na monitor Serial.print(acc_pitch); Serial.print("\t"); Serial.print(acc_roll); Serial.print("\t"); Serial.print(output_roll); Serial.print("\t"); Serial.print(output_pitch); Powyżej zawarty jest fragment odpowiedzialny za filtr komplementarny. Zamieszczam screeny z kreślarki, bo chciałbym spytać o jakość filtracji wyników, które widać na wykresach: Kąt Roll dla układu w bezruchu Kąt Roll dla układu poruszanego Kąt Pitch dla układu w bezruchu Kąt Pitch dla układu poruszanego Dla mnie jako amatora filtracja wydaje się być całkiem ładna choć są opóźnienia w reakcji filtra, nie wiem tylko jak bardzo szkodliwe na ogół im bardziej zwiększam wpływ acc w filtrze, tym odpowiedź jest szybsza ale bardziej zaszumiona, a obecne nastawy (0.97 gyro, 0.03 acc) dobrałem doświadczalnie

shakixp, przepraszam, że odpisuję Ci dopiero po takim czasie jednak życie i obowiązki skutecznie mnie zatrzymało a chciałem przeanalizować Twoje uwagi z należytą starannością dzięki za obszerny post! Po pierwsze, niewiele umiem informacji wyciągnąć z wykresów, które wrzuciłeś, choć ogólnie zdaje mi się, że 1) wraz z wzrostem temperatury jest ogólna tendencja do lepszych wyników, 2) baaardzo ogólnie to w okolicach 20-25st C chyba te wyniki ogółem są najbardziej poprawne. Swoją drogą słusznie ciekawe, że wartości LSB z czujnika temperatury nie rosną liniowo, proporcjonalnie do wzrostu temperatury rzeczywistej. Po drugie i bardzo ważne: Muszę oddać honor i przyznać się do błędu - ponownie zrobiłem test na czas trwania pętli sterowania i nie jest to 4ms /250Hz... Pętla w istocie zawiesza działanie na czas zwolnienia miejsca w buforze UARTa. Poniżej kod pętli dla baud rate = 9600baud: void loop() { czas1=micros(); recordGyroRegisters(); raw_gyro_X=raw_gyro_X - sum_gyro_X; raw_gyro_Y=raw_gyro_Y - sum_gyro_Y; raw_gyro_Z=raw_gyro_Z - sum_gyro_Z; processGyroData(); processAccelData(); recordAccelRegisters(); Serial.println(czas3); while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop czas2=micros(); czas3=czas2-czas1; loop_timer = micros(); //Reset the loop timer } Trochę wstyd, że twierdziłem wyżej inaczej, a wszystko z błędnego pomiaru. Przy prędkości 9600baud pierwsze kilkanaście pętli robiło się faktycznie w 4000us, po czym czas ten wzrastał na stałą wartość ~6200us. Chwilę zajęło mi dojście do tego, że po prostu bufor dość szybko się zapycha, stąd większy czas w późniejszych obiegach. Przy innych prędkościach przesyłu jak również przy innym "obciążeniu" portu wysyłanymi danymi ta tendencja również jest widoczna. Przepraszam za pomyłkę Zastanawiałem się, czy może dobrym pomysłem byłoby zmniejszenie częstotliwości działania pętli sterującej? Na przykład do 200Hz/5ms? Jaki to może mieć wpływ na szybkość reakcji konstrukcji drona w dalekiej przyszłości? Funkcji w pętli będzie tylko dochodzić, czas jej trwania będzie się tylko wydłużać, to może większy bufor czasowy na obliczenia się przyda...? Drugą z opcji, którą rozważam jest to co wyżej fajnie mi zaproponowałeś, czyli każdorazowy pomiar czasu trwania każdego obiegu, tak by czas wykonania każdej pojedynczej pętli był uwzględniany jako chwilowe "deltaT" w całkowaniu. Muszę jednak sprawdzić, jak funkcja micros() obciąża mikroprocesor, bo nie chciałbym by okazało się, że mierzenie w pętli tego czasu na jej początku, końcu i odejmowanie wartości było cięższe od uzupełniania czasu do 4000us poprzez pętlę while. shakixp, byłbyś w stanie ocenić oba rozwiązania? Swoją drogą jeszcze sprawdzę, czy jest jakaś różnica czasowa, jeśli pętla sterująca wywołuje kolejno funkcje obliczające, czy jeśli usunę te funkcje a ich treści wsadzę bezpośrednio do kodu pętli głównej. Pozdrawiam!

shakixp, na początek kurczę no, muszę to napisać: otóż nawet nie wiesz jak mi miło, że chce Ci się w sobotę przed północą pomagać jakiemuś laikowi na forum pytającemu o banały ja chyba jeszcze nawet nie wiem ile nie wiem :D a skoro o tym mowa, wielu rzeczy wciąż nie rozumiem.. jeśli mogę nagiąć choć raz jeszcze Twoją uprzejmość... A tak poważnie, wracając do propozycji Elvis, by rozrzedzić częstotliwość wysyłania danych przez UART na monitor, zrobiłem to (1Hz) i nagle okazało się, że odczyty dla żyroskopu stały się w porządku. Ponadto zapoznałem się z problemem zapychania bufora portu szeregowego z podanego wcześniej linku do innego tematu (warto było, dziękuję ) i moim wnioskiem jest, że wypisywanie danych na monitor w moim programie musiało zapchać bufor portu szeregowego praktycznie od razu... czy to znaczy że wystarczy nie wysyłać tych danych, bądź wysyłać je stopniowo? Ot, cała sztuczka? Chciałem je monitorować by widzieć czy wszystko gra, ich ciągłe wypisywanie nie jest mi potrzebne Jeśli udowadniam teraz swoją ignorancję bądź niewiedzę - ehh trudno: Rozumiem w takim razie, że to właśnie dość szybko zapchany bufor powoduje, że program zawiesza działanie do czasu każdorazowego zwolnienia się w nim jakiegoś miejsca. Z tego powodu radykalnie zmniejsza się częstotliwość pobierania danych z gyro, a więc te nieliczne wzięte do obliczeń są pomnożone przez "deltaT" (swoją drogą o wiele mniejsze od rzeczywistego czasu pomiędzy pobranymi pomiarami) i ostatecznie dają znacznie mniejszy wynik w całce. Tak to działa? Nawet jeśli pętla dalej trwa przez ślicznie ustalone 250ms to po prostu nie w każdej zostaje obliczony nowy kąt do zsumowania, tak? Trudno mi powiedzieć, do zrobienia screena oczywiście obracałem modułem ale chyba zrobiłem zrzut dla płytki postawionej do góry nogami więc mogła się już nie ruszać (dla podkreślenia, jak proporcjonalnie mniejsze są wyniki odczytane od obrotu rzeczywistego). Niepokojąco stabilne? Do czego dążysz? bo chyba jest tu kolejna rzecz której najwyraźniej nie widzę (zaczyna mi być głupio tu pisać) ale skoro gyro liczy prędkość względną, a poprzez całkowanie sumuję drobne (względne) kąty wykonane co "deltaT" miedzy odczytami, to nawet jeśli gyro się nie rusza ( 0deg/s*dT =0) to kąty wyświetlają się poprawnie (pomijam dryft). Wiem że piszę Ci coś oczywistego, ale może akurat wyjdzie to czego nie rozumiem Troche offtopic, ale każdy błąd który mi wytkniesz jest dla mnie bardzo cenny a na pewno jest ich wiele, tylko coraz bardziej stracham się wykorzystywać Twoją uprzejmość Dzięki za link. Jak od dłuższej chwili staram się zrozumieć to zjawisko to tak sobie myślę, że te proste obliczenia (niby na gimbal locka właśnie) w moim kodzie wzięte prosto od gościa z YT, który z powodzeniem (!) wprowadził to do swojego quadcoptera, są "troszkę" śmieszne. Przyznaję, wziąłem je bez przemyślenia... facet to nawet tłumaczy skąd je wziął, więc gdybyś był ciekaw, podam link na pewno więcej Ci to powie, niż mi Co ciekawsze (i dla mnie trochę zaskakujące) to to, że kąty Eulera myślałem, że są mi znane z studiów automatyki i robotyki gdzie przerabialiśmy różne obroty względem układów lokalnych lub układu bazowego, jednakże... po 1) nijak mi to nie pomaga zrozumieć GL (ale już się za to biorę - szanuję bardzo Twoje powyższe wytłumaczenie) po 2) nikt nam o tym nie powiedział a sam obracając wiele układów współrzędnych nie zauważyłem tego problemu. Sam już nie wiem jak źle to świadczy o mojej (nie)wiedzy. Aha, przedłużam już ale kurczę, Ty tu shakixp do mnie górnolotne tematy a ja zwróciłem dopiero uwagę czy ja w ogóle odbieram dobre dane z modułu...? Poniżej screen przy leżącym płasko MPU i odczyty z acc: Troszkę duże te liczby dla zakresu +/-8g (4096), szczególnie dla osi X, gdzie siły w ogóle nie ma... nie wiem jak bardzo przymrużyć na to oko, jednakże nigdzie na YT nie widziałem takich błędów u ludzi w tutorialach. Pozdrawiam i wielkie dzięki za cierpliwość

Ojej, Panowie, bardzo dziękuję za odpowiedzi i pomysły, tyle ich jest że nie odwołam się do wszystkiego odrazu zabieram się więc do testów! Na szybko kilka spraw: Surowe dane z gyro chyba są poprawne i oczywiście była to pierwsza z rzeczy które sprawdzałem. Nieskalibrowane sięgają ~ +/-32768, a podzielone przez 65.5 -> +/-500deg/s czyli zgodnie z ustawionym zakresem w rejestrze. Co do kodu, nie dziwię się że jest podobny gdyż wygląda na wzięty wprost od pewnego pana z YT który zrobił dość popularny DIY quadcopter na arduino chyba, że było dokładnie odwrotnie, ale to nieważne Długość trwania pętli sprawdzałem w każdym obiegu poprzez odjęcie wartości wyrzucanej przez funkcję micros() tuż przed wykonaniem pętli oraz wartości wyrzuconej przez tę funkcję tuż po wykonaniu, no i po odjęciu wychodziło jakoś 4004 microseconds czyli 4ms, wydaje mi się że zrobiłem to poprawnie.. niestety z programowania jestem cienki więc chciałem uniknąć kwestii wprowadzania kwaternionów.. jednakże skoro o tym mówisz, czy to znaczy ze sposób na likwidację tego zjawiska jest u mnie w programie zrobiony błędnie? void loop() { recordGyroRegisters(); raw_gyro_X=raw_gyro_X - sum_gyro_X; raw_gyro_Y=raw_gyro_Y - sum_gyro_Y; raw_gyro_Z=raw_gyro_Z - sum_gyro_Z; processGyroData(); recordAccelRegisters(); processAccelData(); printData(); while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop loop_timer = micros(); //Reset the loop timer } Oki, to zanim zabiorę się do analizy Waszych propozycji (mega dziękuję!!!) to jeszcze pytanie (chyba że rozwieje się ono przy czytaniu): Powyżej cała pętla sterująca. Skoro Serial.print w funkcji printData() zaburza obliczenia to czy wyniki pitch/roll z akcelerometru (funkcja processAccelData() ) tez nie powinna zwracać złych wartości..? Jest wywoływana w pętli sterującej tak samo jak gyro a pięknie w Serial.princie wyrzuca dokładne wartości kątów Pozdro i dziękuję za pomoc! zabieram się do czytania

Wyświetlana wartość kąta pitch jest już w stopniach, więc poprawnie działający program powinien pokazać na screenie 180deg przy takim samym obrocie płytki. Pokazuje obecnie oczywiście proporcjonalnie mniej, to już zależy ile tych Serial.printów wcisnąłem doświadczalnie do kodu. Jeśli chodzi o działanie pętli to w niej są wykonywane także obliczenia tych samych kątów roll i pitch ale dla wskazań akcelerometru i tutaj nie ma żadnych błędów - wyniki są bardzo dokładne.. no a to wciąż ta sama pętla przecież. Dlatego tym bardziej nie rozumiem całej sytuacji, a proces przeliczania danych dla gyro sprawdzałem wielokrotnie.. edit, odnośnie czasu trwania pętli zrobiłem testy i trwa ona zawsze 4ms, czyli wykonuje się z poprawną częstotliwością 250Hz niezależnie ile jej na potrzeby testu dołożyłem Serial.printów

Dzień dobry! Słuchajcie, witam na forum Forbota, gdyż mimo, że bywam tu już od dawna jako obserwator, dziś pierwszy raz zmuszony zostałem by prosić sam o pomoc zajmuję się modułem gyro+acc mpu6050, z którego wyciągam dane i przerabiam, by były nieco bardziej przydatne poniższy kod opiera się głownie na gotowych przykładach z YT. Jednakże od samego początku gdzieś w kodzie mam błąd, którego nie potrafię wyłapać. Otóż przeliczone (według mnie logicznie i poprawnie) wartości z żyroskopu na monitorze szeregowym nie ukazują się jako wartości poprawne. Co ciekawe jednak, nie są to wartości oderwane od rzeczywistości, gdyż w miarę kręcenia modułem, kąty narastają bądź maleją, jednakże w mega wolnym tempie. Dla wyjaśnienia, obrót płytką o dany kąt daje proporcjonalnie mniejszy wynik na ekranie (na załączonym screenie widać wartość dla kąta Roll = ~2.74deg przy obrocie płytką o 180deg). Na dole zamieszczam kod programu Rozumiem chyba całkowicie działanie programu a mimo to nie rozumiem tego zachowania. Ale jedyna zależność, którą zauważyłem (na screenie po lewej widać ogromną masę funkcji wypisujących na ekran - nie przypadkowo) to to, że dokładanie funkcji Serial.print do funkcji void printData() umieszczonej w pętli głównej wpływa coraz gorzej na wyświetlanie się wartości kątowych pitch, roll i yaw (są jeszcze mniejsze przy tym samym rzeczywistym obrocie płytki). Pewnie mało zrozumiale napisałem swój problem, ale to pewnie z braku snu bardzo proszę Was o pomoc, gdyby tylko ktoś z Was zechciał przeglądnąć kod i wyjaśnić skąd się biorą te błędy... matematyka jest zrobiona chyba poprawnie, a pętla główna ustawiona jest na 250Hz stałego wykonania. Przepraszam za ewentualne błędy albo banalność problemu którego nie dostrzegam.. gdybym mógł jakoś pomóc, to chętnie odpowiem na pytania pozdrawiam! /* *Kod pobierania danych z modulu MPU-6050 bez uzycia zewnetrznych bibliotek *Jedyna stosowana biblioteka to <Wire.h> do komunikacji I2C z modułem pomiarowym * *Zakres pomiarowy zyroskopu ustawiam chyba na +/- 500deg/s *Zakres pomiarowy akcelerometru ustawiam chyba na +/-8g * *Moduł MPU-6050 pozwala też na pomiar i odczytywanie temperatury *Wartosci są schowane w rejestrach 0x41 i 0x42 * *Wszelkie dane odnosnie dzialania i inicjowania pracy modulu, mapy rejestrow dostepne *są w nocie katalogowej MPU-6050 */ //////////////////////// // POCZATEK PROGRAMU // //////////////////////// #include <Wire.h> int kalibracja = 0; // zmienna potrzebna do kalibracji zyroskopu long raw_acc_X, raw_acc_Y, raw_acc_Z; //surowe dane z rejestrow akcelerometru long int raw_gyro_X, raw_gyro_Y, raw_gyro_Z; //surowe dane z rejestrow gyroskopu long sum_gyro_X, sum_gyro_Y, sum_gyro_Z; // zmienne do sumowania odczytow z zyroskopu long loop_timer; //do mierzenia czasu trwania petli float gyro_pitch, gyro_roll, gyro_yaw; // zmienne na katy wyliczone z pomiarow zyroskopu float acc_pitch, acc_roll, acc_yaw; // zmienne na katy wyliczone z pomiarow akcelerometru long acc_vector; // zmienna do liczenia Pitagorasa z skladowych akcelerometru ///////////////////////// // PETLA JEDNORAZOWA // ///////////////////////// void setup() { Serial.begin(9600); // rozpoczecie komunikacji szeregowej Wire.begin(); // zainicjowanie komunikacji Wire setupMPU(); // zainicjowanie stanu MPU przed praca GyroCalibration(); // kalibracja zyroskopu, usrednienie 2000 wynikow loop_timer = micros(); // do zmiennej loop_timer zostaje wpisana wartosc czasu // od załączenia programu } //////////////////////// // PROGRAM WLASCIWY // //////////////////////// void loop() { recordGyroRegisters(); raw_gyro_X=raw_gyro_X - sum_gyro_X; raw_gyro_Y=raw_gyro_Y - sum_gyro_Y; raw_gyro_Z=raw_gyro_Z - sum_gyro_Z; processGyroData(); recordAccelRegisters(); processAccelData(); printData(); while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop loop_timer = micros(); //Reset the loop timer } void setupMPU() // funkcja ustawienia wartosci poczatkowych w MPU { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x6B); // ustawiamy dostęp do rejestru 6B by ustawić tryb pracy Wire.write(0b00000000); // rejestr czuwania wylaczony itp Wire.endTransmission(); // zakonczenie transmisji Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x1B); // rejestr 1B do konfiguracji pracy zyroskopu Wire.write(0b00001000); // tryb pracy do +/- 500 deg/s Wire.endTransmission(); // zakonczenie transmisji Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x1C); // rejest 1C do konfiguracji pracy akcelerometru Wire.write(0b00010000); // tryb pracy do +/- 8g Wire.endTransmission(); // zakonczenie transmisji } void GyroCalibration() { for (int kalibracja = 0; kalibracja < 2000 ; kalibracja ++) { recordGyroRegisters(); // w każdej petli odczytaj wartosci z zyroskopu sum_gyro_X = sum_gyro_X+raw_gyro_X; // w kazdej petli w zmiennej sum_gyro_X sumuj kolejne pomiary sum_gyro_Y = raw_gyro_Y+sum_gyro_Y; // w kazdej petli w zmiennej sum_gyro_Y sumuj kolejne pomiary sum_gyro_Z = raw_gyro_Z+sum_gyro_Z; // w kazdej petli w zmiennej sum_gyro_Z sumuj kolejne pomiary delay(3); // Delay 3us to simulate the 250Hz program loop } sum_gyro_X = sum_gyro_X/2000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X sum_gyro_Y = sum_gyro_Y/2000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X sum_gyro_Z = sum_gyro_Z/2000; // uśrednij wyniki pomiarow i zapisz do sum_gyro_X } void recordAccelRegisters() { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x3B); // ustawiam rejest 0x3B do danych akcelerometru Wire.endTransmission(); // zakonczenie transmisji Wire.requestFrom(0b1101000,6); // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc while(Wire.available()<6); raw_acc_X=Wire.read()<<8|Wire.read(); // dane acc_x/y/z przechowywane sa w 8bitowych rejestrach raw_acc_Y=Wire.read()<<8|Wire.read(); // od adresu 0x3B do 0x40 wlacznie i wlasnie je pobieramy raw_acc_Z=Wire.read()<<8|Wire.read(); // i zapisujemy do zmiennych, na kazda os 2 bajty danych } void processAccelData() { //z Pitagorasa przeliczam dlugosc calkowita wektora sily ciezkosci acc_vector = sqrt((raw_acc_X*raw_acc_X)+(raw_acc_Y*raw_acc_Y)+(raw_acc_Z*raw_acc_Z)); acc_pitch = asin((float)raw_acc_Y/acc_vector)* 57.296; acc_roll = asin((float)raw_acc_X/acc_vector)* -57.296; // wzory obok sa by obliczyc kąty pomiedzy skladowymi sil } // a te katy to jednoczesnie kąty obrotu modulu void recordGyroRegisters() { Wire.beginTransmission(0b1101000); // rozpoczecie transmisji, inaczej 0x68, czyli adres MPU Wire.write(0x43); // ustawiam rejest 0x43 do danych zyroskopu Wire.endTransmission(); // zakonczenie transmisji Wire.requestFrom(0b1101000,6); // od 0x3B ma sie zaczac pobieranie 6 bajtow danych o acc while(Wire.available()<6); raw_gyro_X=Wire.read()<<8|Wire.read(); // dane gyro_x/y/z przechowywane sa w 8bitowych rejestrach raw_gyro_Y=Wire.read()<<8|Wire.read(); // od adresu 0x43 do 0x48 wlacznie i wlasnie je pobieramy raw_gyro_Z=Wire.read()<<8|Wire.read(); // i zapisujemy do zmiennych, na kazda os 2 bajty danych } void processGyroData() // 250Hz to czestotliwosc calkowania pomiarow { // 65.5 to wartosc dla +/-500deg/s wzieta z dokumentacji gyro_pitch=gyro_pitch+(raw_gyro_X * 0.0000611); // robimy calkowanie by dostac wartosc kąta deg gyro_roll=gyro_roll+(raw_gyro_Y * 0.0000611); // 0.0000611 = 1/(250*65.5) gyro_yaw=gyro_yaw+(raw_gyro_Z * 0.0000611); // wynik bedacy wychyleniem katowym z pomiarow zyroskopu gyro_pitch=gyro_pitch+gyro_roll*sin(raw_gyro_Z*0.000001066); //tymi obliczeniami likwidujemy gimbal lock gyro_roll=gyro_roll-gyro_pitch*sin(raw_gyro_Z*0.000001066); } void printData() { // funkcja do wyswietlania wartosci na monitor Serial.print("Angle pitch: "); Serial.print(gyro_pitch); Serial.print(" "); Serial.print("Angle roll: "); Serial.print(gyro_roll); Serial.print(" "); Serial.print("Angle yaw: "); Serial.print(gyro_yaw); Serial.print(" "); Serial.println(); Serial.print("Angle pitch: "); Serial.print(gyro_pitch); Serial.print(" "); Serial.print("Angle roll: "); Serial.print(gyro_roll); Serial.print(" "); Serial.print("Angle yaw: "); Serial.print(gyro_yaw); Serial.print(" "); Serial.println(); Serial.print("Angle pitch: "); Serial.print(gyro_pitch); Serial.print(" "); Serial.print("Angle roll: "); Serial.print(gyro_roll); Serial.print(" "); Serial.print("Angle yaw: "); Serial.print(gyro_yaw); Serial.print(" "); Serial.println(); Serial.print("Angle pitch: "); Serial.print(gyro_pitch); Serial.print(" "); Serial.print("Angle roll: "); Serial.print(gyro_roll); Serial.print(" "); Serial.print("Angle yaw: "); Serial.print(gyro_yaw); Serial.print(" "); Serial.println(); }