Mac
Marcin
Grzegorz
KrisS
Frigos
itepe
YCE 72
Luciano
github INA226 project help
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.
- #include <Wire.h>
- #include <MPU6050.h>
- #include <KalmanFilter.h>
- MPU6050 mpu;
- // Konfiguracja filtru Kalmana dla osi X i Y (kat, odchylka, pomiar)
- KalmanFilter kalmanX(0.001, 0.003, 0.03);
- KalmanFilter kalmanY(0.001, 0.003, 0.03);
- // Obliczone wartosci Pitch i Roll tylko z akcelerometru
- float accPitch = 0;
- float accRoll = 0;
- // Obliczone wartosci Pitch i Roll z uwzglednieniem filtru Kalmana i zyroskopu
- float kalPitch = 0;
- float kalRoll = 0;
- void setup()
- {
- Serial.begin(115200);
- // Inicjalizacja MPU6050
- Serial.println("Inicjalizacja MPU6050");
- while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
- {
- Serial.println("Nie znaleziono MPU6050!");
- delay(500);
- }
- // Kalibracja zyroskopu
- mpu.calibrateGyro();
- }
- void loop()
- {
- // Odczytanie wektorow z czujnikow
- Vector acc = mpu.readNormalizeAccel();
- Vector gyr = mpu.readNormalizeGyro();
- // Kalukacja Pitch & Roll z akcelerometru
- accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
- accRoll = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
- // Kalman - dane z akcelerometru i zyroskopu
- kalPitch = kalmanY.update(accPitch, gyr.YAxis);
- kalRoll = kalmanX.update(accRoll, gyr.XAxis);
- Serial.print("Pitch = ");
- Serial.print(accPitch);
- Serial.print(" Roll = ");
- Serial.print(accRoll);
- Serial.print(" (K)Pitch = ");
- Serial.print(kalPitch);
- Serial.print(" (K)Roll = ");
- Serial.print(kalRoll);
- Serial.println();
- }
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.
- #include <Wire.h>
- #include <ADXL345.h>
- #include <L3G4200D.h>
- #include <KalmanFilter.h>
- ADXL345 accelerometer;
- L3G4200D gyroscope;
- // Konfiguracja filtru Kalmana dla osi X i Y (kat, odchylka, pomiar)
- KalmanFilter kalmanX(0.001, 0.003, 0.03);
- KalmanFilter kalmanY(0.001, 0.003, 0.03);
- // Obliczone wartosci Pitch i Roll tylko z akcelerometru
- float accPitch = 0;
- float accRoll = 0;
- // Obliczone wartosci Pitch i Roll z uwzglednieniem filtru Kalmana i zyroskopu
- float kalPitch = 0;
- float kalRoll = 0;
- void setup()
- {
- Serial.begin(115200);
- // Inicjalizacja akcelerometru
- while(!accelerometer.begin())
- {
- Serial.println("Nie znaleziono ADXL345!");
- delay(500);
- }
- accelerometer.setRange(ADXL345_RANGE_2G);
- // Inicjalizacja L3G4200D
- Serial.println("Inicjalizacja L3G4200D");
- while(!gyroscope.begin(L3G4200D_SCALE_2000DPS, L3G4200D_DATARATE_400HZ_50))
- {
- Serial.println("Nie znaleziono L3G4200D!");
- delay(500);
- }
- gyroscope.calibrate(100);
- }
- void loop()
- {
- // Odczytanie wektorow z czujnikow
- Vector acc = accelerometer.readNormalize();
- Vector gyr = gyroscope.readNormalize();
- // Kalukacja Pitch &amp; Roll z akcelerometru
- accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
- accRoll = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
- // Kalman - dane z akcelerometru i zyroskopu
- kalPitch = kalmanY.update(accPitch, gyr.YAxis);
- kalRoll = kalmanX.update(accRoll, gyr.XAxis);
- Serial.print("Pitch = ");
- Serial.print(accPitch);
- Serial.print(" Roll = ");
- Serial.print(accRoll);
- Serial.print(" (K)Pitch = ");
- Serial.print(kalPitch);
- Serial.print(" (K)Roll = ");
- Serial.print(kalRoll);
- Serial.println();
- }
Zapraszam do poradnika Arduino, gdzie dowiesz się w jaki sposób działa filtr Kalmana.
Reklama
Komentarze
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.
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()
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.
Hi, Thank you for code, can anyone help me to get yaw value for MPU6050, using this code with Kalman filter?
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?
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
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
did u solve the error sir.
i have the same issue
Jak wyglądało by pobieranie wartości dla 3 osi? (roll pitch i yaw)?
