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

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

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

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

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

Спасибо за помощь!

РЕШЕНИЕ:

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

результат № 1

результат № 2

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

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