SantyagoSantyago
YouTube RSS Google+ Facebook GitHub

Newsletter Arduino

Zapisz się do Newslettera, aby otrzymać informację o nowych wpsiach w dziale Arduino!

Arduino poradnik

Wstęp

Teoria

Biblioteki

Komponenty

Czujniki i sensory

Rozwiązania i algorytmy

Narzędzia

Mikrokontrolery i Arduino IDE

Arduino i klony

Poradniki wideo

Sprzęt dostarczają

Reklama na Blogu

Najnowsze poradniki

Ostatnie komentarze

Popularne wpisy

Facebook

Google+

Ostatnie fotografie

polskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorywieliczka-szyb-danilowicza

Odczyty Pitch & Roll oraz filtr Kalmana

Filtr Kalmana jest jednym z lepszych znanych mi filtrów stosowanych do filtracji danych sensorycznych (ale i nie tylko). Niestety standardowe filtry nie posiadają zbyt dużych możliwości, ponieważ z reguły nie są w stanie dowiedzieć się więcej o przepuszczanych przez nich danych. Filtr Kalmana działa tutaj znacznie lepiej. Filtrując dane mierzy również ich parametry statyczne (tzw. wariancję) oraz liczy błąd predykcji kolejnego pomiaru przyjmując współczynnik Kalmana, przez który przemnażane są dane na etapie korekcji.

Jeśli nasze dane będą obarczone dużym szumem, czyli ich wariancja będzie wysoka, to model będzie polegał na wartościach z predykcji. Natomiast jeśli dane będą posiadały mały szum, ale cały model będzie charakteryzował się dużą wariancją, to filtr będzie opierał się na danych pomiarowych.

Wartości Pitch i Roll możemy wyliczyć z danych pochodzących akcelerometru i relacji pomiędzy wartościami przyśpieszeń we wszystkich trzech osiach, jak i żyroskopu na podstawie zmiany prędkości tego kąta w czasie. Jak dobrze wiemy, żyroskopy charakteryzują się zjawiskiem dryftu, a akcelerometry sporym szumem. Jeśli jednak połączymy dane z obu czujników z filtrem Kalmana, uzyskamy w ten sposób znacznie lepsze wyniki.

Prostą implementację filtru Kalmana dla Arduino pobrać można z repozytorium Git.

3-osiowy żyroskop i akcelerometr MPU6050

Idealnym wręcz układem, który pozwoli nam na zastosowanie filtru Kalmana jest opisywany już przezemnie układ MPU6050 dostępny w modułach IMU GY-86 i GY-87. Ponieważ łączy w sobie oba czujniki, praktycznie nie będziemy potrzebowali dodatkowych elementów.

  1. #include <Wire.h>
  2. #include <MPU6050.h>
  3. #include <KalmanFilter.h>
  4.  
  5. MPU6050 mpu;
  6.  
  7. // Konfiguracja filtru Kalmana dla osi X i Y (kat, odchylka, pomiar)
  8. KalmanFilter kalmanX(0.001, 0.003, 0.03);
  9. KalmanFilter kalmanY(0.001, 0.003, 0.03);
  10.  
  11. // Obliczone wartosci Pitch i Roll tylko z akcelerometru
  12. float accPitch = 0;
  13. float accRoll = 0;
  14.  
  15. // Obliczone wartosci Pitch i Roll z uwzglednieniem filtru Kalmana i zyroskopu
  16. float kalPitch = 0;
  17. float kalRoll = 0;
  18.  
  19. void setup()
  20. {
  21.   Serial.begin(115200);
  22.  
  23.   // Inicjalizacja MPU6050
  24.   Serial.println("Inicjalizacja MPU6050");
  25.   while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  26.   {
  27.     Serial.println("Nie znaleziono MPU6050!");
  28.     delay(500);
  29.   }
  30.  
  31.   // Kalibracja zyroskopu
  32.   mpu.calibrateGyro();
  33. }
  34.  
  35. void loop()
  36. {
  37.   // Odczytanie wektorow z czujnikow
  38.   Vector acc = mpu.readNormalizeAccel();
  39.   Vector gyr = mpu.readNormalizeGyro();
  40.  
  41.   // Kalukacja Pitch &amp; Roll z akcelerometru
  42.   accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
  43.   accRoll  = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
  44.  
  45.   // Kalman - dane z akcelerometru i zyroskopu
  46.   kalPitch = kalmanY.update(accPitch, gyr.YAxis);
  47.   kalRoll = kalmanX.update(accRoll, gyr.XAxis);
  48.  
  49.   Serial.print("Pitch = ");
  50.   Serial.print(accPitch);
  51.   Serial.print(" Roll = ");
  52.   Serial.print(accRoll);
  53.  
  54.   Serial.print(" (K)Pitch = ");
  55.   Serial.print(kalPitch);
  56.   Serial.print(" (K)Roll = ");
  57.   Serial.print(kalRoll);
  58.  
  59.   Serial.println();
  60. }

3-osiowy żyroskop L3G4200D i akcelerometr ADXL345

Jeśli filtr chcemy zastosować dla dwóch osobnych czujników, na przykład dla L3G4200D oraz ADXL345 to nasz program będzie wyglądał następująco. Taka para czujników dostępna jest w module IMU GY-80.

  1. #include <Wire.h>
  2. #include <ADXL345.h>
  3. #include <L3G4200D.h>
  4. #include <KalmanFilter.h>
  5.  
  6. ADXL345 accelerometer;
  7. L3G4200D gyroscope;
  8.  
  9. // Konfiguracja filtru Kalmana dla osi X i Y (kat, odchylka, pomiar)
  10. KalmanFilter kalmanX(0.001, 0.003, 0.03);
  11. KalmanFilter kalmanY(0.001, 0.003, 0.03);
  12.  
  13. // Obliczone wartosci Pitch i Roll tylko z akcelerometru
  14. float accPitch = 0;
  15. float accRoll = 0;
  16.  
  17. // Obliczone wartosci Pitch i Roll z uwzglednieniem filtru Kalmana i zyroskopu
  18. float kalPitch = 0;
  19. float kalRoll = 0;
  20.  
  21. void setup()
  22. {
  23.   Serial.begin(115200);
  24.  
  25.   // Inicjalizacja akcelerometru
  26.   while(!accelerometer.begin())
  27.   {
  28.     Serial.println("Nie znaleziono ADXL345!");
  29.     delay(500);
  30.   }
  31.  
  32.   accelerometer.setRange(ADXL345_RANGE_2G);
  33.  
  34.   // Inicjalizacja L3G4200D
  35.   Serial.println("Inicjalizacja L3G4200D");
  36.   while(!gyroscope.begin(L3G4200D_SCALE_2000DPS, L3G4200D_DATARATE_400HZ_50))
  37.   {
  38.     Serial.println("Nie znaleziono L3G4200D!");
  39.     delay(500);
  40.   }
  41.  
  42.   gyroscope.calibrate(100);
  43. }
  44.  
  45. void loop()
  46. {
  47.   // Odczytanie wektorow z czujnikow
  48.   Vector acc = accelerometer.readNormalize();
  49.   Vector gyr = gyroscope.readNormalize();
  50.  
  51.   // Kalukacja Pitch &amp;amp; Roll z akcelerometru
  52.   accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
  53.   accRoll  = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
  54.  
  55.   // Kalman - dane z akcelerometru i zyroskopu
  56.   kalPitch = kalmanY.update(accPitch, gyr.YAxis);
  57.   kalRoll = kalmanX.update(accRoll, gyr.XAxis);
  58.  
  59.   Serial.print("Pitch = ");
  60.   Serial.print(accPitch);
  61.   Serial.print(" Roll = ");
  62.   Serial.print(accRoll);
  63.  
  64.   Serial.print(" (K)Pitch = ");
  65.   Serial.print(kalPitch);
  66.   Serial.print(" (K)Roll = ");
  67.   Serial.print(kalRoll);
  68.  
  69.   Serial.println();
  70. }

Zapraszam do poradnika Arduino, gdzie dowiesz się w jaki sposób działa filtr Kalmana.

Udpstępnij dalej!

http://www.jarzebski.pl/arduino/rozwiazania-i-algorytmy/odczyty-pitch-roll-oraz-filtr-kalmana.html

Reklama

Komentarze Komentarze
Avatar 1
kociolk Windows / Safari 537.36
08 grudzień 2014 - 20:07 Starogard Gdański

Czy program filtru, ten zamieszczony w repozytorium Git, dla L3G4200D i ADXL345 działa tylko w takiej postaci? Tzn czy po użyciu go w jakimś programie, który już wykonuje jakieś operacje z użyciem wyników z filtru czy nadal powinien działać tak samo. Pytam bo właśnie kiedy użyje wartości z wyjścia filtra do jakiś porównywań przy if`ach czy while`ach to filtr funkcjonuje poprawnie przez jakiś czas żeby nagle zacząć wyrzucać wartości liczone w tysiącach a następnie wartość 0 w obu osiach obrotu. Dodam że ten sam algorytm, biblioteka i parametry zastosowane tylko dla odczytywania i wyrzucania na port szeregowy wartości filtra działa bez zarzutów.

Avatar 2
Korneliusz Linux x86_64 / Mozilla Firefox 33.0
08 grudzień 2014 - 21:45 Bytom

To zależy co robi Twój program. Filtr Kalmana operuje na zmiany pomiarów odstępach czasu dt, pomiędzy wywołaniami update(). Które z reguły, a przynajmniej powinny być stałe. Jeśli będą zbyt wielkie, mogą zdarzyć się dziwne rzeczy o których wspominasz. Na myśl przychodzi mi jeszcze przepełnienie timera micros(), który jest przepełniony i resetowany po około 70minutach @ 16Mhz. Jeśli Twoje dt jest spore, możesz pokombinować z wartościami konfiguracji KalmanFilter()

Avatar 1
MP Windows 7 / Safari 537.36
22 kwiecień 2016 - 09:07 Brak informacji

Mam takie pytanko. Na jakiej podstawie dobierane były te nastawy początkowe (Kąt, odchyłka, pomiar)? Chciałbym z nimi troszkę poeksperymentować w moim projekcie. Jeśli byłaby taka możliwość prosiłbym o jakiś krótki komentarz.

Skomentuj wpis