Объединить данные гироскопа и акселерометра

Я строю балансировочный робот, используя систему Lego Mindstorm NXT. Я использую два датчика от HiTechnic, первый из которых - акселерометр, а второй - гироскоп. Я успешно отфильтровал шум от обоих датчиков и полученных углов для обоих в диапазоне от -90 до 90 градусов, при этом 0 градусов идеально сбалансированы.

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

enter image description here

Наиболее часто используемый подход, который я видел, чтобы объединить эти датчики с твердостью, - это использовать фильтр Калмана. Тем не менее, я не эксперт в области исчисления, и я действительно не понимаю математических символов, но я понимаю математику в исходном коде.

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

Благодарим за помощь!

РЕЗУЛЬТАТЫ РЕШЕНИЯ:

Хорошо, kersny решил мою проблему, представив мне дополнительные фильтры. Это график, иллюстрирующий мои результаты:

Результат № 1

enter image description here

Результат № 2

enter image description here

Как вы можете видеть, фильтр корректирует гироскопический дрейф и объединяет оба сигнала в один плавный сигнал.

Изменить: Так как я все равно исправлял сломанные изображения, я подумал, что было бы интересно показать установку, которую я использовал для генерации этих данных:

enter image description here

Ответы

Ответ 1

Фильтры Kalman отличные и все, но я считаю, что дополнительный фильтр намного проще реализовать с аналогичными результатами. Лучшие статьи, которые я нашел для кодирования дополнительного фильтра, эта вики (наряду с в этой статье о преобразовании датчиков в технические единицы) и PDF в zip файле на на этой странице (в разделе "Техническая документация" я верьте, что имя файла в zip - filter.pdf);

PS. Если вы застряли на фильтрах Калмана, здесь, это код C-синтаксиса для Arduino, который его реализует.