IMU-сенсор на 10 степеней свободы и сравнение фильтров для расчета углов

Здравствуйте.

Если кому полезны будут мои нижеописанные изыскания, то буду рад.

Прикупил я тут в Амперке IMU-сенсор на 10 степеней свободы (Troyka-модуль) - гироскоп + акселерометр + компас + барометр. Решил сравнить фильтры для расчета углов с его помощью.

Работаю я на Маке с Mac OS X 10.9.5., Arduino IDE 1.6.7., Arduino Mega 2560, Matlab R2015b. Сенсор и Arduino Mega закрепил на жесткой площадке, которую легко вертеть по крену/тангажу на столе. Сенсор читаю амперковской библиотекой.

В сети есть много материалов по фильтрам для расчета углов. Мне оказались полезными вот какие: http://robottini.altervista.org/kalman-filter-vs-complementary-filter и статьи Kristian Lauszus (http://www.instructables.com/id/Guide-to-gyro-and-accelerometer-with-Arduino-inclu/ и http://forum.arduino.cc/index.php?topic=291507.0 ). И его же библиотека для Ардуино для Калмановского фильтра - https://github.com/TKJElectronics/KalmanFilter.

Далее рискую показаться избыточным для профессионалов, но для начинашек в этой области (коим я тоже являюсь) это может быть полезным.

Углы можно считать отдельно как по гироскопу, так и по акселерометру. Но у гироскопа присутствует неустранимое смещение (drift), а акселерометр шумит. Великая наука математика предлагает несколько вариантов решения этой проблемы. В применении к Ардуино (к ее процессорным возможностям и объемам памяти) их целых три: считать углы с использованием комплементарного фильтра (Complementary filter) (КФ), комплементарного фильтра 2-го порядка (КФ2) (Complementary filter 2nd order) и фильтра Калмана (Kalman filter). Эти фильтры используют показания и гироскопа и акселерометра и учитывают их drift и шум. Если принцип действия КФ понятен для простых инженерских мозгов (это я про себя), то за КФ2 и за Калманом стоит высокая математика и ее мало кто понимает. Калман считается самым точным, но он является самым ресурсоемким. Вот эти фильтры я и решил сравнить.

Расчет угла только по гироскопу (на примере расчета угла крена):
gyroAngleX(текущ) = gyroAngleX(предыд)+gX*dT
Расчет текущего угла идет в приращениях к предыдущему. В формуле: gX - текущее X-показание с гироскопа, dT - период опроса гироскопа (Sample Rate).

Расчет угла только по акселерометру (на примере расчета угла крена):
accelAngleX = ATAN2(aY,aZ)*RAD_TO_DEG
Расчет текущего угла идет не в приращениях, а в абсолюте. В формуле: ATAN2(x,y) - функция арктангенса x/y, учитывающая знаки x и y и выдающая угол соответствующего квадранта в радианах (если охота, см. в Вики). ATAN2() имеется в наборе мат-функций Arduino IDE. aY и aZ - соответственно Y и Z-показания акселерометра (я не ошибся: угол X расчитывается через пару aY-aZ, а угол Y - через пару aX-aZ). RAD_TO_DEG - коэффициент перевода радианов в градусы (в Arduino IDE эта константа так и называется).

Расчет угла по комплементарному фильтру (название "комплементарный", в смысле "дополнительный", отражает взаимный учет как показаний гироскопа, так и акселерометра) (на примере расчета угла крена):
cfAngleX(текущ) = K*(cfAngleX(предыд)+gX*dT)+(1-K)* accelAngleX
Расчет текущего угла идет через предыдущий. В формуле: gX - X-показание гироскопа, dT - sample rate (см. выше), accelAngleX - текущий угол, расчитанный только по акселю (см. выше), K - коэффициент комплементарного фильтра, определяемый динамикой системы. Kristian Lauszus утверждает, что для обычных, не сверхдинамичных систем значение K = 0,98 вполне себе подходящее. Из формулы видно, что K является "весовым" коэффициентом, определяющим вклад показаний акселя/гиро в результат.

Расчет угла по комплементарному фильтру 2-го порядка. Формулу приводить не буду. Она в некотором смысле похожа на предыдущую, но сложнее. Пытливый мозг поймет эту формулу из нижеприведенного ардуиновского скетча. И КФ, и КФ2 являются приближениями (порядками) численного интегрирования. Наверняка можно придумать и КФ3, и КФ4 и т.д. У КФ2 тоже есть свой коэффициент, который также принимается равным 0,98.

Расчет угла по фильтру Калмана. Тут уж формулу не приведу точно. Как было сказано в одной статье "Фильтр Калмана использует свойства кватернионов и Z-преобразований линейной алгебры". Ни убавить, ни прибавить. Полазил по кодам библиотеки фильтра Калмана. Там сплошь вычисления трехмерных матриц. Желания разбираться с этим не возникло. В другой статье сказали так: "Фильтр Калмана - это магия. Он просто работает." В библиотеке Калмановского фильтра разглядел и свои "калмановские" коэффициенты, но тот же Kristian Lauszus настоятельно рекомендует их не трогать. Не буду! Важно сказать лишь одно: фильтр Калмана тоже вычисляет текущий угол на основании предыдущего.

Общее: и КФ, и КФ2, и Калман получают на входе одни и те же три параметра: accelAngleX, gX, dT.

Перед тем, как начать считать углы, есть еще одна небольшая задача: определить начальный угол положения. Моя установка стоит на столе, но стол ведь тоже неидеально горизонтальный. Или по каким-то причинам надо стартовать не из горизонтального положения. Поскольку единственным не-рекуррентным способом (не через предыдущий) расчета угла является расчет чисто по акселю, то буду использовать его (для других методов не определен предущий угол). Но аксель шумит. Делаю так: с интервалом в 100 мсек делаю 10 замеров угла по акселю и результат усредняю. Есть другие предложения как определять нач. угол?

Итак, вот что делает моя ардуиновская программа:
- считает только углы крена (X). Для того, чтобы сравнить работу фильтров, этого вполне достаточно;
- вычисляет начальный угол положения;
- делает N циклов в каждом из которых считывает шесть параметров с акселя/гиро;
- по полученным данным просчитывает пять вариантов углов: чисто по акселю, чисто по гиру, по КФ, по КФ2, по Калману, определяет времена расчета углов каждым из пяти способов;
- по окончании циклов - выводит всю необходимую информацию в монитор Arduino IDE (или в Matlab для построения графиков) Если кому интересно, то код матлабовского скрипта для построения графиков могу выложить.

Еще одно замечание: при расчете угла чисто по акселю, по КФ, по КФ2 и по Калману я каждый раз считаю accelAngleX через atan2(). Казалось бы, можно было бы посчитать accelAngleX один раз, а в других расчетах просто ссылаться. Но для точной оценки времени расчетов каждым из методов требуется такая избыточность.

Общая длительность цикла составляет порядка 0,1 сек (частота опроса сенсора, соответственно, получается 10 Гц). В описании этого сенсора сказано: "Частота обновления сигнала Гироскопа: 100…800 Гц, Акселерометра: 0.5…1000 Гц". Так что я вроде вписался в эти параметры. Так вот, при цикле в 0,1 сек удается сделать 100 замеров (если пробовать больше, то не хватает оперативной памяти Ардуины и компилятор ругается) за 10 секунд верчения сенсора.

Ардуиновский код для приема и обсчета углов прилагается.

Продолжение в следующем посте...

 

По результатам работы:

Время цикла (он же sample rate) dT = 105670 мксек или ≈106 мсек из которых 100 мсек - это тупой delay(100). То есть шесть параметров с акселя/гиро считываются за 6 мсек.

Времена вычисления одного угла разными способами (это все времена на 16 МГц Меге):
- чисто по гиру: Tgyr=19 мксек (одно float умножение и одно float сложение);
- чисто по акселю: Tacc=196 мксек (atan2() и одно float умножение);
- по КФ: Tcf=243 мксек (atan2(), четыре float умножения и три float сложения);
- по КФ2: Tcf2=290 мксек (atan2() и небольшая куча float умножений/сложений);
- по Калману: Tkalm=500 мксек (atan2() и внутренняя калмановская кухня).

Но какой фильтр я ни выберу (КФ, КФ2, Калман), мне все равно придется считать atan2(). Поэтому "чистая калмановская кухня" весит 500 - 243 ≈ 260 мксек. При расчете двух углов (крен-тангаж) времена надо умножать на 2. То есть при цикле опроса акселя/гира порядка 100 мсек (а чаще и не надо из-за параметров сенсора и инерции серв (если строить систему стабилизации положения на сервах, что я и собираюсь делать) - поправьте меня, если ошибаюсь) одна мсек уйдет на расчет двух углов по Калману, т.е. 1% времени цикла. Не так страшен Калман, как его малюют!

А теперь про точности. Все очень хорошо видно на прилагаемом графике, построенном в Matlab'e на основе расчитанных углов. За 10 секунд я повернул свою площадку с сенсором из исходного положения в ≈+25 градусов по крену и вернул назад в исходное.

По точности, как и обещали ученые, победил Калман. Щас я объясню что к чему. Угол начального положения, расчитанный усреднением десяти углов по акселю, составляет ≈1,5 градуса (эх, неровный у меня стол!). Конечный угол ( = начальному), расчитанный по Калману, равен 1,48 градуса. С учетом округления можно считать их совпавшими. Конкуренты были не столь точны.

Тонкая черная линия gyroA (углы чисто по гироскопу) за все время измерений отклонялась в плюс и в итоге отклонилась на 2,8 град от Калмана (вот он, дрифт акселерометра!). Тонкая синяя линия accelA (углы чисто по акселерометру) все время колбасилась вокруг "линии партии" (это шум акселерометра). Зеленая линия углов, расчитанных по комплементарному фильтру, отклонилась в итоге от Калмана на 1,2 градуса. А линия углов, расчитанных по комплементарному фильтру 2-го порядка (голубая), хоть в итоге и была весьма близка к Калману, но в процессе измерений отклонялась от него существенно.

Таких замеров и расчетов я сделал много и всегда результат был один - по точности побеждал Калман. А поскольку и время на его расчет меня совсем не пугает, то в итоге я выбираю Калмана для расчета углов для будущих систем.

Извините, если был многословен.

 

А вот и ардуиновский код:

Код (C++):

// GYROACC - W2. Расчет углов по показаниям акселерометра/гироскопа
#include <Wire.h>        // библиотека для работы I²C
#include <troyka-imu.h>    // библиотека для работы с модулями IMU
#include "Kalman.h"        // библиотека фильтра Калмана

#define Buzz 7        // пищалка

// если MATLAB=0 - то компилируется версия для вывода инфы в Serial Monitor Arduino IDE,
// если MATLAB!=0 - то компилируется версия для передачи инфы в Matlab

#define MATLAB 0

#define  numReadings 100        // число замеров
#define delayReadings 100    // задержка после каждого цикла чтения

// Считаем только углы крена (X) - для оценки.
float aX[ numReadings +1], aY[ numReadings +1], aZ[ numReadings +1];    // знач-я с акселя
float gX[ numReadings +1], gY[ numReadings +1], gZ[ numReadings +1];     // знач-я с гиро
// массивы расчитываемых углов
float gyroAngleX[ numReadings +1];        // по гиро
float accelAngleX[ numReadings +1];        // по акселю
float cfAngleX[ numReadings +1];            // по компл-му фильтру (КФ)
float cf2AngleX[ numReadings +1];        // по КФ 2-го порядка
float kalmanAngleX[ numReadings +1];        // по Калману

#define CFK 0.98            // коэффициент КФ

unsigned long Tcycle;        // время цикла чтения (мксек)
float dT=0;        // период обращ-я на чтение одного пар-ра акселя/гиро (sample rate), сек

float CF2K= 0.98;            // коэффициент для КФ2
int N;     float WW = 0;        // вспомогательно
float XY1=0, ZZ=0;            // вспомогательно для КФ2

unsigned long Tgyr, Tacc, Tcf, Tcf2, Tkalm;    // время расчетов углов
Gyroscope gyro; Accelerometer accel; Kalman kalmanX;  

void setup()
{     Serial.begin(115200); gyro.begin(); accel.begin(); N= numReadings; }
void loop()
{    delay(1000);
    // определяем угол начального положения, усредняя 10 расчетных углов по акселю
    for (int i = 1; i<11; i++)
    {    // читаем начальные значения
        aX[0] = accel.readX_G(); aY[0] = accel.readY_G(); aZ[0] = accel.readZ_G();
        accelAngleX[i] = (atan2(aY[0],aZ[0])); delay(100);
    }
    accelAngleX[0] = 0; for (int i = 1; i<11; i++) { accelAngleX[0] += accelAngleX[i]; }
    accelAngleX[0] = accelAngleX[0]*RAD_TO_DEG/10;        // усредняем
    // прописываем нач. угол в начальные расчетные углы
    gyroAngleX[0]= accelAngleX[0]; cfAngleX[0]= accelAngleX[0]; cf2AngleX[0]= accelAngleX[0];
    kalmanAngleX[0]= accelAngleX[0]; kalmanX.setAngle(accelAngleX[0]);
    XY1=0;    // для КФ2

// читаем 6* N значений от акселя/гиро
#if MATLAB == 0
    Serial.println("It starts in 3 beeps .......");
#endif
        for (int i=1; i<4; i++) { tone(Buzz,1500,100); delay(1000); }     // гудим 3 раза
#if MATLAB == 0
        Serial.println("...Started...");
#endif
        Tcycle = micros();
     for (int i=1; i<  N+1; i++)
    {    gX[i] = gyro.readX_DegPerSec(); gY[i] = gyro.readY_DegPerSec();
        gZ[i] = gyro.readZ_DegPerSec();
        aX[i] = accel.readX_G(); aY[i] = accel.readY_G(); aZ[i] = accel.readZ_G();
        delay(delayReadings); }
    Tcycle = micros()-Tcycle; Tcycle /=  N; dT= float(Tcycle)/1000000;
// считаем углы чисто по гиру
    Tgyr = micros(); for (int i=1; i<  N+1; i++) { gyroAngleX[i]=gyroAngleX[i-1]+gX[i]*dT; }
    Tgyr = micros()-Tgyr; Tgyr /=  N;        // Tgyr - вр выч-я угла по гиру (мксек)
// считаем углы чисто по акселю
    Tacc = micros(); for (int i=1; i<  N+1; i++) { accelAngleX[i]=(atan2(aY[i],aZ[i]))*RAD_TO_DEG; }
    Tacc = micros()-Tacc; Tacc /=  N;        // Tacc - вр выч-я угла по акселю (мксек)
// считаем углы по комплементарному фильтру (КФ)
    Tcf = micros();
    for (int i=1; i<  N+1; i++)
    {    WW = (atan2(aY[i],aZ[i]))*RAD_TO_DEG;
        cfAngleX[i]= CFK*(cfAngleX[i-1]+gX[i]*dT)+(1-CFK)*WW; }
    Tcf = micros()-Tcf; Tcf /=  N;         // Tcf - вр выч-я угла по КФ (мксек)
// считаем углы по комплементарному фильтру 2-го порядка (КФ2)
    Tcf2 = micros();
     for (int i=1; i<  N+1; i++)
    {    // считаем X - крен
        // считаем accelAngleX[i] здесь для замера Tcf2
        WW = (atan2(aY[i],aZ[i]))*RAD_TO_DEG;
        ZZ = (WW-cf2AngleX[i-1])*CF2K*CF2K;
        XY1 = XY1 + ZZ*dT;
        ZZ = XY1 + gX[i] + (WW-cf2AngleX[i-1])*2*CF2K;
        cf2AngleX[i]=cf2AngleX[i-1]+ZZ*dT;
    }
    Tcf2 = micros()-Tcf2; Tcf2 /=  N; // Tcf2 - выч. угла по КФ2 (мксек)
// считаем углы по Калману
    Tkalm = micros();
    for (int i=1; i<  N+1; i++)
    {    WW = (atan2(aY[i],aZ[i]))*RAD_TO_DEG;
        kalmanAngleX[i]= kalmanX.getAngle(WW, gX[i], dT); }
    Tkalm = micros()-Tkalm; Tkalm /=  N;     // Tkalm - выч. угла по Калману (мксек)

#if MATLAB == 0
// распечатываем T и массивы
Serial.println("=========================================");
Serial.print(" numReadings = "); Serial.println( N);
Serial.print("Tcycle, mcsec = "); Serial.print(Tcycle);
Serial.print("; dT, sec = "); Serial.println(dT,8);
Serial.println("=== Calculation time for X angle, mcsec: ===");
Serial.print("Tgyr = "); Serial.print(Tgyr); Serial.print("\t");
Serial.print("Tacc = "); Serial.print(Tacc); Serial.print("\t");
Serial.print("Tcf = "); Serial.print(Tcf); Serial.print("\t");
Serial.print("Tcf2 = "); Serial.print(Tcf2); Serial.print("\t");
Serial.print("Tkalm = "); Serial.print(Tkalm); Serial.println("");
Serial.println("---------------------- Raw Values - First 10 values ---------------------------");
Serial.print("aX: \t"); for (int i=1; i<  11; i++) {Serial.print(aX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("aY: \t"); for (int i=1; i<  11; i++) {Serial.print(aY[i]); Serial.print(", \t");} Serial.println("");
Serial.print("aZ: \t"); for (int i=1; i<  11; i++) {Serial.print(aZ[i]); Serial.print(", \t");} Serial.println("");
Serial.print("gX: \t"); for (int i=1; i<  11; i++) {Serial.print(gX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("gY: \t"); for (int i=1; i<  11; i++) {Serial.print(gY[i]); Serial.print(", \t");} Serial.println("");
Serial.print("gZ: \t"); for (int i=1; i<  11; i++) {Serial.print(gZ[i]); Serial.print(", \t");} Serial.println("");
Serial.println("---------------------- Calculated X-Angles - First 10 values ---------------------------");
Serial.print("gyroAngleX: \t"); for (int i=0; i< 10; i++) {Serial.print(gyroAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("accelAngleX: \t"); for (int i=0; i< 10; i++) {Serial.print(accelAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("cfAngleX: \t"); for (int i=0; i< 10; i++) {Serial.print(cfAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("cf2AngleX: \t"); for (int i=0; i< 10; i++) {Serial.print(cf2AngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("kalmanAngleX: \t"); for (int i=0; i< 10; i++) {Serial.print(kalmanAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.println("----------------------Calculated X-Angles - Last 5 values ---------------------------");
Serial.print("gyroAngleX: \t"); for (int i=N-4; i<  N+1; i++) {Serial.print(gyroAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("accelAngleX: \t"); for (int i=N-4; i<  N+1; i++) {Serial.print(accelAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("cfAngleX: \t"); for (int i=N-4; i<  N+1; i++) {Serial.print(cfAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("cf2AngleX: \t"); for (int i=N-4; i<  N+1; i++) {Serial.print(cf2AngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.print("kalmanAngleX: \t"); for (int i=N-4; i<  N+1; i++) {Serial.print(kalmanAngleX[i]); Serial.print(", \t");} Serial.println("");
Serial.println(".............................................................");
#else
    tone(Buzz,1500,100); delay(1000);    // звоним
    for (int i=0; i<  N+1; i++)     // шлем в Matlab - N + 1 цикл
    {    Serial.println(gyroAngleX[i]); Serial.println(accelAngleX[i]);
        Serial.println(cfAngleX[i]); Serial.println(cf2AngleX[i]); Serial.println(kalmanAngleX[i]); }
#endif

while (2>0) {}        // бесконечный цикл
}
 

 

Помоги с фильтром Калмана. Мне надо получить углы по трем осям. Дело в том, что со стандартным кодом от Амперки, Ардуино будто бы доводит данные. И я хочу перевести на фильтр калмана

 

Братец, так вроде код выше всё и объясняет. Что еще непонятно? Ты сформулируй.

 

Крестьянин лектору об устройстве автомобиля: "Всё понятно, вот только куда лошадь запрягать?"
Кто бы ПОПУЛЯРНО объяснил работу фильтра Калмана на уровне его библиотеки!..

 

Привет, у меня тут тоже дилемма возникла. Тоже этим датчиком меряю, использую комплементарный фильтр с 0,02 коэффициентом, период цикла - 3мс (т к использую в коде с другими модулями). Проблема в том что в просчёта дикие запаздывания вплоть до 2-3с между реальным углом и устаканиваем отфильтрованного угла. То есть я отклоняю датчик, а итоговый угол как бы догоняет реальный.