Калибровка и фильтрация данных магнитометра.

Всем привет! Недавно я решил опробовать какой-нибудь фильтр, чтобы улучшить точность датчиков (акселерометр, гироскоп, магнитометр). Выбор пал на Madgwik's AHRS, который вроде как неплохо сглаживает показания + сразу задает кватернионы. Однако при тестировании результаты оказались совсем печальными.. несмотря на то, что плата с датчиками (GY-87) была закреплена на бруске, датчики показывали, что брусок вращается (!). Чуть ниже я графики прикреплю. Я думаю, что скорее всего проблема в магнитометре, потому что если учитывать только не отфильтрованные данные акселерометра, то тангаж и крен расчитывются с погрешностью в 0.01 радиан (!). Так вот, мог бы кто-нибудь помочь с его "настройкой" или хотя бы просто какие-нибудь статьи полезные на эту тему скинуть?
p.s. Саму реализацию AHRS под arduino брал тут -> https://github.com/mikeshub/Pololu_Open_IMU

 

Рыскание^

Крен^

Тангаж^

 

Про фильтры Кальмана читали?

 

Да, но фильтр Кальмана немного другое, меня интересует именно AHRS

 

Ох, неужели никто не занимался калибровкой магнитометра? Или все сейчас на пляже загорают?