Динамическая калибровка магнитометра

Я работаю над магнитометром AK8975, являющимся частью IMU. Что кажется мне очень сложным. Эта микросхема выдает трехмерный вектор в качестве выходного сигнала, описывающего магнитное поле Земли в любом месте на земле или вблизи нее.

Я попробовал два типа алгоритмов вычисления заголовка: один простой, arctan(-y/x)а другой математика отмены наклона (тангажа) и банка (крена), как указано ниже. И по наклонности, и по банкам дают неправильный вывод.

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

Я пробовал калибровку на ошибки мягкого и твердого железа. Я мог бы построить его в 3D и показать идеальную 3D-сферу. Все еще не работает на склонность или склонение.

Любой указатель будет полезен.

Код и его реализации, как показано ниже:

void Compass_Heading()
{
  double MAG_X;
  double MAG_Y;
  double cos_roll;
  double sin_roll;
  double cos_pitch;
  double sin_pitch;

  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch); 

  //// Tilt compensated Magnetic filed X:
  MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
  //// Tilt compensated Magnetic filed Y:
  MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
  //// Magnetic Heading


  MAG_Heading = atan2(-MAG_Y, MAG_X) ;

}

Там , где magnetom_x, #_yи #_zявляются компонентами 3D вектора , который на самом деле является значением RAW от магнитометра. крен и шаг от загадочного выхода фильтра Калмана от бортового акселерометра и гироскопа. Эти три датчика находятся в ATAVRSBIN1 . Крен и подача в порядке до этой стадии.

Теперь простой расчет заголовка в соответствии с journal_of_sensors_renaudin et al_2010c.pdf должен был быть MAG_Heading = atan2(-magnetom_y, magnetom_x) ;с компенсацией, как указано выше.

Общий код просто из открытых AHRS .

Данные в формате Roll, Pitch и Yaw. Я повернул устройство только рукой. Первые три были сконцентрированы только на Roll, Pitch и Yaw соответственно. Остальные два сначала поворачивают устройство примерно на 45 градусов вдоль X (прокручено), затем поворачивают вдоль локального Z магнитометра. Затем то же самое повторяют с вращением на 45 градусов вдоль Y (наклонно), затем поворачивают вдоль локального Z магнитометра.

Графики построены в диапазоне от -180 до 180 градусов.

Углы в градусах в файле Характеристики YAW на Roll.

Углы в градусах в файле Характеристики YAW на Pitch.

Углы в градусах в файле Характеристики YAW на самом Yaw.

Углы в градусах в файле Характеристики YAW на Yaw с наклоном 45 градусов.

Углы в градусах в файле Характеристики YAW на Yaw с углом 45 градусов.

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

Аналогично для последнего изображения устройство было наклонено на 45 градусов, а затем вдоль оси Z магнитометра.

Я надеюсь, что это поможет решить мою проблему.

Новые разработки заключаются в следующем:

Я работал над заголовком. Я получил следующий вывод. CSV

CSV

CSV

Мне нравятся твои графики. Они ясно показывают, что крен, тангаж и рыскание работают. Поздравляем! Это уже больший прогресс, чем большинство людей.

Я предполагаю, что код, который вы представили, вычисляет «неправильное» значение MAG_Heading, отличное от ожидаемого значения MAG_Heading.

Нам было бы намного легче помочь вам, если бы вы дали нам: (Это раздел «Опишите симптомы» «Как правильно задавать вопросы» )

• AK8975 магнитометр выходные значения M_X, M_Y и m_z в какой - то один момент времени.
• Значения высоты тона и крена в одно и то же время
• предположительно неправильное выходное значение MAG_Heading, рассчитанное по этим значениям
• то, что вы ожидали, будет правильным MAG_Heading

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

• Какой формат угла ожидают ваши функции sin () и cos () и atan2 ()? Вам нужно сделать какое-то преобразование между форматом, тональностью и креном, хранящимися в этом формате? Вам нужно конвертировать из этого формата в то, что вам нужно MAG_heading? (скобки, градусы или радианы? с плавающей или с фиксированной точкой?)
• Есть ли смещение в необработанных значениях m_x, m_y, m_z, которые необходимо вычесть?
• Выстроены ли все части так, как это предусмотрено кодом? В частности, совпадает ли ось тангажа и крена с осью магнитометра? (Должен ли m_x указывать вперед, вдоль оси крена? Должен ли m_y указывать вправо, вдоль оси тангажа?)
• Может быть, какое-то значение датчика или другое - возможно, m_z - необходимо отменить, прежде чем вводить в этот код?
• Может быть, этот код прерывается тем или иным прерыванием, которое портит его внутренние значения? Кажется, я вспоминаю другой проект, в котором после того, как кто-то вставил «деление» в подпрограмму прерывания, каждый расчет функции триггера в других местах программы часто давал неверный результат.
• Может быть, прерывания запускаются так часто, что этот код на самом деле никогда не заканчивается?

Кажется, есть другие люди, обсуждающие очень похожий код в другом месте: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU !!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; и т.п.

Приложение к сведению для LSM303 имеет полезное руководство для калибровки с компенсацией наклона компаса , который применим к вашей проблеме. Это довольно подробно, иначе я бы переписал здесь расчеты. Обратите внимание, что значения акселерометра необходимы для расчета полного шага, крена и рыскания, поскольку вращение вокруг оси линий магнитного поля не приводит к изменению значений магнитометра. Аналогично для гравитации с акселерометром.