[f.fwd1 0]

Добрый день.

Пытаюсь заставить работать алгоритм получения угла отклонения от направления на север (курс).
В качестве датчика использую MPU-9250 (акселерометр, гироскоп, компас).

Без проблем получаю показания всех 9-ти осей.
Затем подаю их на вход фильтра Маджвика.
На выходе получаю коэффициенты из которых получаю крен, тангаж и курс.

Он что-то показывает, но не то. Проблемы в общем то такие:
1. Курс сдвинут на -75 градусов относительно севера. Почему?
2. При вращении в горизонтальной плоскости - меняются показания крена и тангажа, хотя датчик стоит осью Z вертикально вниз. Почему?

 

[AlexandrY 2]

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

[f.fwd1 0]

45 минут назад, AlexandrY сказал:

Для получения углов надо провести соответствующее преобразование.

Можно поподробне, какие нужны преобразования?

При старте происходит калибровка акселерометра и гироскопа, компас калиброван тоже, но 1 раз (крутил во все стороны, потом искал смещение).
После калибровки все значения приведены в диапазон -1..1.
Их я и подаю на вход фильтра.  Только показания гироскопа подаются в виде gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f

 

Извиняюсь, ошибся

 

Показания магнитометра подаются на вход фильтра в диапазоне от -250.. 250.

 

[rkit 1]

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

[Aner 2]

Если ТС пишет о фильтре Маджвика. Тот кто в теме должно быть понятно ... . Исходники, описание тут -> https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

Если взять исходники и адаптировать под свой проект то особых проблем не должно быть.

 

[rkit 1]

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

[f.fwd1 0]

На самом деле задача такая: есть плата с MPU-9250 на борту. Нужно получать курс, крен, тангаж.

Нашел реализацию библиотеки на таком фильтре, да и многие ссылки касательно MPU-9250 ведут сюда:
https://github.com/kriswiner/MPU9250
Исходники у всех почти одинаковые, небольшие отличия, можно сказать косметические

Самое интересное что я делаю как в коде, однако не удается добиться нормальных показаний.

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

Еще выяснилось что гироскоп сильно шумит (вывел его показания на график).
Причем шумят все оси,
На графике приведены значения курса, тангажа, крена - диапазон 0 - 359
Гироскоп X Y Z. Ноль показаний на значении 100,  Разрешение датчика -250...250 °/s
Всплески - это удары по столу на котором горизонтально лежит плата.
Подскажите - это нормально такие колебания?

Извиняюсь за нубские вопросы, раньше не имел дела с этими датчиками.

[AlexandrY 2]

1 hour ago, f.fwd1 said:

Подскажите - это нормально такие колебания?

Да, колебания нормальные. 
Шумами как раз и отличаются дорогие датчики от дешевых.
Дешевым нужен компас чтобы не плыл курс. 
Но компас реагирует на любые сильные поля рядом (двигатели, соленоиды, динамики, магнитные защелки и т.д.) поэтому может только ухудшить рысканье.  
Ко всему этому у Маджвика есть два параметра которыми вы регулируете чувствительность к быстрым всплескам. 
Если вам нужен фильтр для быстрого (10 мс) и точного (0.1 градуc) управления ориентацией, то чистый Маджвик с дешевыми MEMS-ами не подходит. 

[f.fwd1 0]

Только что, AlexandrY сказал:

Да, колебания нормальные. 
Шумами как раз и отличаются дорогие датчики от дешевых.
Дешевым нужен компас чтобы не плыл курс. 
Но компас реагирует на любые сильные поля рядом (двигатели, соленоиды, динамики, магнитные защелки и т.д.) поэтому может только ухудшить рысканье.  
Ко всему этому у Маджвика есть два параметра которыми вы регулируете чувствительность к быстрым всплескам. 
Если вам нужен фильтр для быстрого (10 мс) и точного (0.1 градуc) управления ориентацией, то чистый Маджвик с дешевыми MEMS-ами не подходит. 

Точность нужна 1 - 2°, скорость можно 20 - 50 мс.

А вот еще такой вопрос. На борту есть компас.
Я его откалибровал. Нашем минимумы и максимумы, вычислил смещения (hard iron).
Нашел коэффициент для приведния каждой оси к одинаковым значеиям.
По формуле heading = atan2(y, x); получаю значения курса, перевожу в градусы.
С чем связано такое поведение что когда я выставляю его на север, восток и запад все более менее, а на юг значения уплывают более чем на 12°?
 

 

[AlexandrY 2]

40 minutes ago, f.fwd1 said:

С чем связано такое поведение что когда я выставляю его на север, восток и запад все более менее, а на юг значения уплывают более чем на 12°?

Связано с недостаточностью наблюдений. 
Начните регулярно и систематически воспроизводить измерения и увидите, что значения уплывают при любой ориентации.   

[rkit 1]

6 hours ago, f.fwd1 said:

Еще выяснилось что гироскоп сильно шумит (вывел его показания на график).

Разумеется, для того и нужен это фильтр. Выложи лучше свой код.

 

5 hours ago, AlexandrY said:

Шумами как раз и отличаются дорогие датчики от дешевых.
Дешевым нужен компас чтобы не плыл курс. 

Дорогие imu тоже шумят и тоже требуют компенсации. Нешумящих датчиков не бывает в природе. Не то что IMU, а вообще никаких.

[f.fwd1 0]

   Код в общем то достаточно простой

void CompassProcess(void)
{
   // Измерения по каждой из 9 осей делаются в таймере, каждые 10мс
   // затем складываются в кольцевой буфер 
   // в бесконечном цикле, при появлении данных они копируются в 
   // эту структуру AGM_Read_copy
  
    // aRes = 2.0/32768.0 = Диапазон измерений / разрешение
    // accelBias = среднее арифметическое от 100 измерений
    ax = (float)AGM_Read_copy.acc[0] * aRes - accelBias[0];
    ay = (float)AGM_Read_copy.acc[1] * aRes - accelBias[1];   
    az = (float)AGM_Read_copy.acc[2] * aRes - accelBias[2];
    
    // gRes = 250.0/32768.0 = Диапазон измерений / разрешение
    // gyroBias = среднее арифметическое от 100 измерений
    gx = (float)AGM_Read_copy.gyro[0] * gRes - gyroBias[0];
    gy = (float)AGM_Read_copy.gyro[1] * gRes - gyroBias[1];  
    gz = (float)AGM_Read_copy.gyro[2] * gRes - gyroBias[2];   

    // mRes = 10.0*4912.0/32760.0; = 
    // magCalibration - прочитанное из самого датчика заводские значения калибровки
  	// magbias - среднее арифметическое от 100 измерений
	mx = (float)AGM_Read_copy.mag[0] * mRes * magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
    my = (float)AGM_Read_copy.mag[1] * mRes * magCalibration[1] - magbias[1];  
    mz = (float)AGM_Read_copy.mag[2] * mRes * magCalibration[2] - magbias[2];
  
    // 
  	diff = AGM_Read_copy.timestamp;
    // коэффициент для фильтра
    // DeltaTDivider - подбирается, пока не знаю сколько нужно, пробовал разные значения
  	deltat = (float)((diff)/DeltaTDivider);
  
  	// есть еще коэффициент Beta - его тоже пробовал менять от 0,01 до 200
  
    // без магнитометра
    MadgwickAHRSupdateIMU(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, ax, ay, az);
    // с магнитометром
    //MadgwickQuaternionUpdate(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, ax, ay, az, my, mx, -mz);

  	yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
    roll  *= 180.0f / PI;
}

[f.fwd1 0]

Добрый вечер.

Вроде все настроил. курс и наклоны показывает правильно.
Но есть одно но.

Если компас указывает на север (или юг), то при изменении тангажа курс не меняется, что правильно.
И в тоже время если сделать крен, то показания компаса уходят сильно - при 10 градусах крена - 10град курса (примерно)

Если компас указывает на восток (или запад), то при изменении тангажа курс меняется (те же 10 на 10) а крен уже не меняет курс.

В чем может быть дело?
По логике вроде связано с мгнитометром, потому что если бы был акселеометр - то он бы врал по какой то оси, но другую показывал бы плавильно. И ошибка была бы то по крену то по тангажу.



 

[AlexandrY 2]

11 hours ago, f.fwd1 said:

В чем может быть дело?
По логике вроде связано с мгнитометром, потому что если бы был акселеометр - то он бы врал по какой то оси, но другую показывал бы плавильно. И ошибка была бы то по крену то по тангажу.

Возможно вы не правильно все же понимает смысл углов.
Есть множество систем поворотов - https://en.wikipedia.org/wiki/Euler_angles.
И вы до сих пор не обозначили в какой системе работаете.  

[f.fwd1 0]

10 минут назад, AlexandrY сказал:

Возможно вы не правильно все же понимает смысл углов.
Есть множество систем поворотов - https://en.wikipedia.org/wiki/Euler_angles.
И вы до сих пор не обозначили в какой системе работаете.  

Скорей всего так и есть.

Взял реализацию фильтра, она в свободном доступе. На вход ему требуются значения 9 осей.
Передаю все 9 осей как в листинге выше.
На выходе стабильный курс, измеряет наклоны по обеим осям. Все устраивает, кроме одного.
Нет держит курс при наклонах с восток-запад.
Не могу понять в чем дело.

AlexandrY, в Ваших сообщениях были упоминания о MPU9250. У Вас удалось добиться правильных показаний курса с компенсацией крена и тангажа?