Как использовать фильтр Калмана для гироскопа на arduino?

Question

[Niptlox Company]

Я нашел что значения можно обрабатывать с помощью фильтра Кальмана, но не разу не видел чтобы их фильтровали сразу все вместе, а только отдельно x, y, z. Возможно ли их обработать вместе?

Comments

[freeExec]

А какая разница, если есть какая-то зависимость между X и Y то их надо решать не в рамках фильтра К.

Answer 1

[Николай Китаев]

Меня учили, что фильтр Калмана - это адаптивный фильтр. Адаптация заключается в том, что надо иметь мат.модель процесса, который вы сглаживаете. То есть полученное замеренное значение сравнивается с прогнозом и на основании разницы к нему применяется поправка. Разумеется, что уравнения движения могут содержать и X, и Y, и Z и остальные буквы латинского и русского алфавита.

Например, уравнение движения самолета состоит минимум из 6 диф.ур. А если есть автоматические устройства, которые живут совместной жизнью и самолёт летит в реальной атмосфере, то количество уравнений доходит до 40.

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

Один знакомый математик сказал, что ф.К. это не формула, а метод, реализаций которых можно наизобретать бесчисленное кол-во.

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

П.С. Есть готовый авиагоризонт WT901B со встроенным ф.К. В последних реализациях его сделали с настраиваемым частотным фильтром, 6 значений частот фильтрации. Мы этот авиагоризонт используем уже очень давно, с 2017 г. Продается на Али, 3000 руб примерно. Выдает весь спектр сигналов по 3 осям - перегрузки - угловые скорости - магнитное поле - пространственные углы. Плюс давление, абсолютная барометрическая высота и температура на плате (=воды в районе Сочи). Рекомендую взять еще и отладочную плату на USB-C, удобно. Общаемся с этим модулем с Mega 2560 R3 по I2C, так удобнее, чем по UART.

П.С.С.: https://habr.com/ru/post/255661/ В приложении к статье готовый код ф. К. и огромный список литературы на эту тему.

Можно применить несколько Ардуин обмениваться между ними данными по I2C. У меня сейчас в работе конструкция, в которой основной контроллер Мега 2560, и две вспомогательных Ардуино Pro Micro. Одна общается с GPS другая измеряет длительность сигнала. Так оказалось проще, чем нагрузить всю работу на одну Мегу.

Answer 2

[Borys Latysh]

"Фильтр Кальмана" - АЛГОРИТМ позволяющий сгладить колебания значений в зависимости от предидущих значений какой то функции.
У нас 3 независимые функции. X,Y,Z
Мы должны обрабатывать каждую - НЕЗАВИСИМО.

P.S. Метод отличный главное чтобы Ардуина успевала обработать 3 потока данных. Я бы подумал про ESP32.

Comments

[Николай Китаев]

Почему должны, почему независимо? Автор как раз спрашивает - а можно вместе?
Говорю же - от задачи надо идти. Можно и вместе.

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

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

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

Например, искусственный горизонт. Там тоже три оси X, Y и Z, но назвать их несвязанными никак нельзя. Как минимум это углы Эйлера одного и того же объекта, между которыми один и тот же угол постоянно - 90°. Если, например это акселерометры, и движение очень медленное (вычисляем по производным тех же акселерометров и угловым скоростям), то связь между всеми тремя осями акселерометров - простая тригонометрия. Фильтровать отдельно показания осей X, Y и Z такого объекта - грубейшая ошибка.

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

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

С фильтром такого не происходит .

[Borys Latysh]

Николай Китаев, К чему такой запал.
У контроллера 1 ядро, потому при всем уважении "одновременно" НЕ получится.
На уровне железа все будет обсчитываться по очереди. Алгоритм можно обыграть но по факту все равно все будет на 1 ядре и следовательно 3 задачи посчитает (в лучшем случае) за х3 времени.

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

Вообще ориентация в пространстве тема специфическая. И решение таких задач на контроллерах ввиду малых вычислительных возможностей требует внимательного знания математики.

[Николай Китаев]

Borys Latysh, оно конечно одно ядро против двух и 16 Мгц против 70 всегда проиграет.

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

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