SantyagoSantyago
Avatar

Witaj!
Blog archiwalny. Już niebawem nowy serwis!

YouTube RSS Facebook GitHub

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

Reklama na Blogu

Najnowsze poradniki

Ostatnie komentarze

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.

Reklama

Komentarze Komentarze
Avatar 1
kociolk Windows / Safari 537.36
08 December 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 December 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 April 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.

Avatar 1
Ibrahim Windows / Safari 537.36
05 April 2017 - 14:10 Brak informacji

Hi, Thank you for code, can anyone help me to get yaw value for MPU6050, using this code with Kalman filter?

Avatar 1
Helix59 Windows 7 / Mozilla Firefox 53.0
06 May 2017 - 19:12 Brak informacji

Dziękuje za artykuł! Mi należy wykorzystać GY - 80 z esp8266. Lecz zaproponowana biblioteka filtru Kalman w esp8266 pracuje nie poprawnie. Że należy zrobić, żeby wykorzystać waszą bibliotekę filtru Kalman na esp8266?

Спасибо за статью! Мне нужно использовать GY-80 с esp8266. Но предложенная библиотека фильтра Kalman в esp8266 работает не корректно. Что нужно сделать, чтобы использовать вашу библиотеку фильтра Kalman на esp8266?

Avatar 1
skeezoo Windows XP / Safari 537.36
01 June 2017 - 16:42 Brak informacji

first great job jarzebski.
Hi Helix59, I was going thru the same project that you are trying to do, combining ESP8266 with GY80, but as you mentioned it didn\'t worked and angles would overflow.
after some diagnostics I found out the problem in the L3G library . when trying to combine the LSB and MSB in raw read function , the combination gives a 32bit int and not a 16bit before it is converted to float. so a -1 would give a 65535.0 instead .
try to modify and if it didnt work and you need a modified version of L3G library let me know

Avatar 1
Greg Windows 7 / Mozilla Firefox 55.0
04 December 2017 - 11:19 Brak informacji

Witam,
Chciałbym wykorzystać podany kod z filtrem Kalmana do MPU6050, ale nie mogę skompilowac tego kodu.
Od razu wyskakuje błąd w linii kodu "while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))" gdzie kompilator nie rozpoznaje komendy begin.
Niby wszystkie biblioteki są wgrane, ale nie mogę przejść tego etapu.

Może ktoś miął podobny problem i wie co może być przyczyną?

Pozdrawiam
Grzegorz

Avatar 1
varun Windows 7 / Safari 537.36
04 August 2018 - 08:27 Brak informacji

did u solve the error sir.
i have the same issue

Avatar 1
Lord225 Windows / Safari 537.36
26 March 2018 - 16:59 Brak informacji

Jak wyglądało by pobieranie wartości dla 3 osi? (roll pitch i yaw)?