Оценивание пространственной ориентации, или Как не бояться фильтров Махони и Маджвика

  • Tutorial

О чём речь


Появление на Хабре поста о фильтре Маджвика было по-своему символическим событием. Видимо, всеобщее увлечение дронами возродило интерес к задаче оценивания ориентации тела по инерциальным измерениям. При этом традиционные методы, основанные на фильтре Калмана, перестали удовлетворять публику — то ли из-за высоких требований к вычислительным ресурсам, неприемлемых для дронов, то ли из-за сложной и неинтуитивной настройки параметров.

Пост сопровождался весьма компактной и эффективной реализацией фильтра на C. Однако судя по комментариям, физический смысл этого кода, а равно и всей статьи, для кого-то остался туманным. Что ж, признаем честно: фильтр Маджвика — самый замысловатый из группы фильтров, основанных в общем-то на очень простых и элегантных принципах. Эти принципы я и рассмотрю в своём посте. Кода здесь не будет. Мой пост — не рассказ о какой-то конкретной реализации алгоритма оценивания ориентации, а скорее приглашение к изобретению собственных вариаций на заданную тему, которых может быть очень много.

image

Представление ориентации


Вспомним основы. Чтобы оценить ориентацию тела в пространстве, нужно для начала выбрать какие-то параметры, которые в совокупности однозначно определяют эту ориентацию, т.е. по сути ориентацию связанной системы координат $xyz$ относительно условно неподвижной системы — например, географической системы NED (North, East, Down). Затем нужно составить кинематические уравнения, т.е. выразить скорость изменения этих параметров через угловую скорость от гироскопов. Наконец, нужно ввести в расчёт векторные измерения от акселерометров, магнитометров и т.д. Вот самые употребительные способы представления ориентации:

Углы Эйлера — крен (roll, $\phi$), тангаж (pitch, $\theta$), курс (heading, $\psi$). Это самый наглядный и самый лаконичный набор параметров ориентации: количество параметров в точности равно количеству вращательных степеней свободы. Для этих углов можно записать кинематические уравнения Эйлера. Их очень любят в теоретической механике, но в задачах навигации они малопригодны. Во-первых, знание углов не позволяет напрямую преобразовать компоненты какого-либо вектора из связанной в географическую систему координат или наоборот. Во-вторых, при тангаже ±90 градусов кинематические уравнения вырождаются, крен и курс становятся неопределёнными.

Матрица поворота — матрица $\mathbf{C}$ размера 3×3, на которую нужно умножить любой вектор в связанной системе координат, чтобы получить тот же вектор в географической системе: $\mathbf{r}_{NED}=\mathbf{C}\mathbf{r}_{xyz}$. Матрица всегда ортогональна, т.е. $\mathbf{C}=\mathbf{C}^{T}$. Кинематическое уравнение для неё имеет вид $\dot{\mathbf{C}}=\mathbf{C}\mathbf{\Omega}$.
Здесь $\mathbf{\Omega}$ — матрица из компонент угловой скорости, измеренных гироскопами в связанной системе координат:

$\mathbf{\Omega}= \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \\ \end{bmatrix}$


Матрица поворота чуть менее наглядна, чем углы Эйлера, зато в отличие от них позволяет непосредственно преобразовывать векторы и ни при каком угловом положении не лишается смысла. С вычислительной точки зрения её главный недостаток — избыточность: ради трёх степеней свободы вводятся сразу девять параметров, и все их нужно обновлять согласно кинематическому уравнению. Задачу можно слегка упростить, воспользовавшись ортогональностью матрицы.

Кватернион поворота — радикальное, но очень неинтуитивное средство против избыточности и вырождения. Это четырёхкомпонентный объект $\mathbf{q}=q_{0}+q_{1}\mathbf{i}+q_{2}\mathbf{j}+q_{3}\mathbf{k}$ — не число, не вектор и не матрица. На кватернион можно смотреть с двух ракурсов. Во-первых, как на формальную сумму скаляра $q_{0}$ и вектора $q_{1}\mathbf{i}+q_{2}\mathbf{j}+q_{3}\mathbf{k}$, где $\mathbf{i}, \mathbf{j}, \mathbf{k}$ — единичные векторы осей (что, конечно, звучит абсурдно). Во-вторых, как на обобщение комплексных чисел, где теперь используется не одна, а три разных мнимых единицы $\mathbf{i}, \mathbf{j}, \mathbf{k}$ (что звучит не менее абсурдно). Как кватернион связан с поворотом? Через теорему Эйлера: тело всегда можно перевести из одной заданной ориентации в другую одним конечным поворотом на некоторый угол $\alpha$ вокруг некоторой оси с направляющим вектором $\mathbf{u}$. Эти угол и ось можно объединить в кватернион: $\mathbf{q}=\mathrm{cos}(\alpha/2)+\mathbf{u}\mathrm{sin}(\alpha/2)$. Как и матрицу, кватернион можно использовать для непосредственного преобразования любого вектора из одной системы координат в другую: $\mathbf{r}_{NED}=\mathbf{q}\mathbf{r}_{xyz}{\mathbf{q}}^{-1}$. Как видно, кватернионное представление ориентации тоже страдает от избыточности, но намного меньше, чем матричное: лишний параметр всего один. Обстоятельный обзор кватернионов уже был на Хабре. Там шла речь о геометрии и 3D-графике. Нас же интересует ещё и кинематика, поскольку скорость изменения кватерниона нужно связать с измеряемой угловой скоростью. Соответствующее кинематическое уравнение имеет вид $\dot{\mathbf{q}}=1/2\mathbf{q}\mathbf{\omega}$, где вектор $\mathbf{\omega}$ тоже считается кватернионом с нулевой скалярной частью.

Схемы фильтров


Самый наивный подход к вычислению ориентации — вооружиться кинематическим уравнением и обновлять в соответствии с ним любой понравившийся нам набор параметров. Например, если мы выбрали матрицу поворота, то можем написать цикл с чем-нибудь в духе C += С * Omega * dt. Результат разочарует. Гироскопы, особенно MEMS, имеют большие и нестабильные смещения нуля — в результате даже в полном покое вычисляемая ориентация будет иметь неограниченно накапливающуюся ошибку (дрейф). Все ухищрения, придуманные Махони, Маджвиком и многими другими, не исключая и меня, были направлены на компенсацию этого дрейфа за счёт вовлечения измерений от акселерометров, магнитометров, приёмников GNSS, лагов и т.д. Так родилось целое семейство фильтров ориентации, опирающихся на простой базовый принцип.
Базовый принцип. Для компенсации дрейфа ориентации нужно прибавить к измеренной гироскопами угловой скорости дополнительную управляющую угловую скорость, построенную на основе векторных измерений других датчиков. Вектор управляющей угловой скорости должен стремиться совместить направления измеренных векторов с их известными истинными направлениями.
Здесь заключён совершенно иной подход, чем в построении корректирующего слагаемого фильтра Калмана. Главное отличие именно в том, что управляющая угловая скорость — не слагаемое, а множитель при оцениваемой величине (матрице или кватернионе). Отсюда вытекают важные преимущества:

  • Оценивающий фильтр можно строить для самой ориентации, а не для малых отклонений ориентации от той, которую дают гироскопы. При этом оцениваемые величины будут автоматически удовлетворять всем требованиям, которые налагает задача: матрица будет ортогональной, кватернион — нормированным.
  • Физический смысл управляющей угловой скорости намного яснее, чем корректирующего слагаемого в фильтре Калмана. Все манипуляции делаются с векторами и матрицами в обычном трёхмерном физическом пространстве, а не в абстрактном многомерном пространстве состояний. Это заметно упрощает доработку и настройку фильтра, а в качестве бонуса позволяет избавиться от матриц большой размерности и тяжеловесных матричных библиотек.

Теперь посмотрим, как эта идея реализуется в конкретных вариантах фильтров.

Фильтр Махони. Вся зубодробительная математика оригинальной статьи Махони написана ради обоснования несложных уравнений (32). Перепишем их в наших обозначениях. Если отвлечься от оценивания смещений нуля гироскопов, то останутся два ключевых уравнения — собственно кинематическое уравнение для матрицы поворота (с управляющей угловой скоростью в виде матрицы $\mathbf{\Omega}'$) и закон формирования этой самой скорости в виде вектора $\mathbf{\omega}'$. Предположим для простоты, что ни ускорений, ни магнитных наводок нет, и благодаря этому нам доступны измерения ускорения свободного падения $-\hat{\mathbf{g}}_{xyz}$ от акселерометров и напряжённости магнитного поля Земли $\hat{\mathbf{m}}_{xyz}$ от магнитометров. Оба вектора измеряются датчиками в связанной системе координат, а в географической системе их положение заведомо известно: $-\mathbf{g}_{NED}$ направлен вверх, $\mathbf{m}_{NED}$ — на магнитный север. Тогда уравнения фильтра Махони будут выглядеть так:

$\dot{\mathbf{C}}=\mathbf{C}(\mathbf{\Omega}+\mathbf{\Omega}') \\ \mathbf{\omega'}=k_{g}\hat{\mathbf{g}}_{xyz}\times \mathbf{C}^{T}\mathbf{g}_{NED}+k_{m}\hat{\mathbf{m}}_{xyz}\times \mathbf{C}^{T}\mathbf{m}_{NED}$

Посмотрим внимательно на второе уравнение. Первое слагаемое в правой части — это векторное произведение. Первый множитель в нём — измеренное ускорение свободного падения, второй — истинное. Поскольку множители обязаны быть в одной системе координат, то второй множитель преобразуется к связанной системе умножением на $\mathbf{C}^{T}$. Угловая скорость, построенная как векторное произведение, перпендикулярна плоскости векторов-множителей. Она позволяет поворачивать расчётное положение связанной системы координат, пока векторы-множители не совпадут по направлению — тогда векторное произведение обнулится и поворот прекратится. Коэффициент $k_{g}$ задаёт жёсткость такой обратной связи. Второе слагаемое выполняет аналогичную операцию с магнитным вектором. По сути фильтр Махони воплощает хорошо известный тезис: знание двух неколлинеарных векторов в двух разных системах координат позволяет однозначно восстановить взаимную ориентацию этих систем. Если векторов больше двух, то это даст полезную избыточность измерений. Если вектор всего один, то одну вращательную степень свободы (движение вокруг этого вектора) зафиксировать не удастся. Например, если дан только вектор $-\mathbf{g}$, то можно скорректировать дрейф крена и тангажа, но не курса.

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

Виртуальная гироплатформа. В фильтре Махони мы прилагали управляющую угловую скорость $\mathbf{\omega'}$ к связанной системе координат. Но можно приложить её и к расчётному положению географической системы координат. Кинематическое уравнение тогда примет вид

$\dot{\mathbf{C}}=\mathbf{C}\mathbf{\Omega}-\mathbf{\Omega}'\mathbf{C}$

Оказывается, такой подход открывает путь к очень плодотворным физическим аналогиям. Достаточно вспомнить то, с чего начиналась гироскопическая техника, — курсовертикали и инерциальные навигационные системы на основе гиростабилизированной платформы в кардановом подвесе.

image
www.theairlinepilots.com

Задачей платформы там была материализация географической системы координат. Ориентация носителя измерялась относительно этой платформы датчиками углов на рамах подвеса. Если гироскопы имели дрейф, то вслед за ними дрейфовала и платформа, и в показаниях датчиков углов накапливались ошибки. Чтобы эти ошибки устранить, вводилась обратная связь от акселерометров, установленных на платформе. Например, отклонение платформы от горизонта вокруг северной оси воспринималось акселерометром восточной оси. Этот сигнал позволял задать управляющую угловую скорость $\mathbf{\omega'}$, возвращающую платформу в горизонт.

Теми же самыми наглядными понятиями мы можем пользоваться и в своей задаче. Выписанное кинематическое уравнение нужно тогда читать так: скорость изменения ориентации представляет собой разность двух вращательных движений — абсолютного движения носителя (первое слагаемое) и абсолютного движения виртуальной гироплатформы (второе слагаемое). Аналогию можно распространить и на закон формирования управляющей угловой скорости. Вектор $-\hat{\mathbf{g}}_{NED}=-\mathbf{C}\hat{\mathbf{g}}_{xyz}$ олицетворяет показания акселерометров, якобы стоящих на гироплатформе. Тогда из физических соображений можно написать:

$\omega'_{N}=-gk_{g}\hat{g}_{E},\; \omega'_{E}=gk_{g}\hat{g}_{N}$

К точно такому же результату можно было бы прийти и формальным путём, сделав векторное перемножение в духе фильтра Махони, но теперь уже не в связанной, а в географической системе координат. Только нужно ли это?

Первый намёк на полезную аналогию платформенной и бесплатформенной инерциальной навигации появляется, видимо, в древнем патенте «Боинга». Затем эта идея активно разрабатывалась Салычевым, а в последнее время — и мной тоже. Очевидные преимущества такого подхода:

  • Управляющую угловую скорость можно формировать на основе понятных физических принципов.
  • Естественным образом оказываются разделены горизонтальные и курсовой каналы, очень различные по своим свойствам и способам коррекции. В фильтре Махони они смешаны.
  • Удобно компенсировать влияние ускорений за счёт привлечения данных GNSS, которые выдаются именно в географических, а не связанных осях.
  • Легко обобщить алгоритм на случай высокоточной инерциальной навигации, где приходится учитывать форму и вращение Земли. Как это сделать в схеме Махони, я не представляю.

Фильтр Маджвика. Маджвик избрал трудный путь. Если Махони, судя по всему, интуитивно пришёл к своему решению, а потом обосновал его математически, то Маджвик с самого начала проявил себя формалистом. Он взялся решать задачу оптимизации. Рассудил он так. Зададим ориентацию кватернионом поворота. В идеальном случае расчётное направление какого-нибудь измеряемого вектора (пусть у нас это будет $-\mathbf{g}$) совпадает с истинным. Тогда будет ${\mathbf{q}}^{-1}\mathbf{g}_{NED}\mathbf{q}=\hat{\mathbf{g}}_{xyz}$. В реальности это не всегда достижимо (особенно если векторов больше чем два), но можно попробовать минимизировать отклонение $\mathbf{F}={\mathbf{q}^{-1}\mathbf{g}_{NED}\mathbf{q}}-\hat{\mathbf{g}}_{xyz}$ от точного равенства. Для этого введём критерий минимизации

$E=\frac{1}{2}|\mathbf{F}|^{2}\rightarrow \mathrm{min}$

Минимизация требует градиентного спуска — движения маленькими шагами в сторону, противоположную градиенту $\nabla E$, т.е. противоположную наискорейшему возрастанию функции $E$. Кстати, Маджвик допускает ошибку: во всех своих работах он вообще не вводит $E$ и настойчиво пишет $\nabla \mathbf{F}$ вместо $\nabla E$, хотя фактически вычисляет именно $\nabla E$.

Градиентный спуск в итоге приводит к следующему условию: для компенсации дрейфа ориентации нужно добавить к скорости изменения кватерниона из кинематического уравнения новое отрицательное слагаемое, пропорциональное $\nabla E$:

$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\mathbf{\omega}-\beta \frac{\nabla E}{|\nabla E|}$

Здесь Маджвик немного отступает от нашего «базового принципа»: он добавляет корректирующий член не к угловой скорости, а к скорости изменения кватерниона, а это не совсем одно и то же. В итоге может оказаться, что обновлённый кватернион перестанет быть единичным и, соответственно, утратит способность представлять ориентацию. Поэтому для фильтра Маджвика искусственная нормировка кватерниона — жизненно важная операция, в то время как для других фильтров — желательная, не необязательная.

Влияние ускорений


До сих пор предполагалось, что истинных ускорений нет и акселерометры измеряют только ускорение свободного падения $-\mathbf{g}$. Это позволяло получить эталон вертикали и с его помощью скомпенсировать дрейф крена и тангажа. Однако в общем случае акселерометры, независимо от своего принципа действия, измеряют кажущееся ускорение — векторную разность истинного ускорения и ускорения свободного падения $\mathbf{f}=\mathbf{a}-\mathbf{g}$. Направление кажущегося ускорения не совпадает с вертикалью, и в оценках крена и тангажа появляются ошибки, вызванные ускорениями.

Это легко проиллюстрировать с помощью аналогии виртуальной гироплатформы. Её система коррекции устроена так, что платформа останавливается в том угловом положении, в котором обнуляются сигналы акселерометров, якобы установленных на ней, т.е. когда измеряемый вектор $\mathbf{f}$ становится перпендикулярен осям чувствительности акселерометров. Если ускорений нет, это положение совпадает с горизонтом. Когда возникают горизонтальные ускорения, гироплатформа отклоняется. Можно сказать, что гироплатформа похожа на сильно задемпфированный маятник или отвес.

image

В комментариях к посту о фильтре Маджвика промелькнул вопрос о том, можно ли надеяться на то, что этот фильтр менее восприимчив к ускорениям, чем, например, фильтр Махони. Увы, все описанные здесь фильтры эксплуатируют одни и те же физические принципы и поэтому страдают от одних и тех же проблем. Обмануть физику математикой нельзя. Что же тогда делать?

Самый простой и грубый способ придумали ещё в середине прошлого века для авиационных гировертикалей: уменьшать или вовсе обнулять управляющую угловую скорость при наличии ускорений или угловой скорости курса (которая свидетельствует о входе в вираж). Тот же метод можно перенести и в нынешние бесплатформенные системы. Об ускорениях при этом нужно судить по значениям $\hat{f}_{N}, \hat{f}_{E}$, а не $\hat{f}_{x}, \hat{f}_{y}$, которые в вираже сами по себе нулевые. Однако в величине $\hat{\mathbf{f}}_{NED}$ не всегда можно отличить истинные ускорения от проекций ускорения свободного падения, обусловленных тем самым наклоном гироплатформы, который требуется устранить. Поэтому метод работает ненадёжно — зато не требует никаких дополнительных датчиков.

Более точный способ основан на использовании внешних измерений скорости от приёмника GNSS. Если известна скорость $\mathbf{v}$, то её можно численно продифференцировать и получить истинное ускорение $\dot{\mathbf{v}}$. Тогда разность $\hat{\mathbf{f}}_{NED}-\dot{\mathbf{v}}$ будет в точности равна $-\hat{\mathbf{g}}_{NED}$ независимо от движения носителя. Ей можно пользоваться как эталоном вертикали. Например, можно задать управляющие угловые скорости гироплатформы в виде

$\omega'_{N}=gk_{g}(\hat{f}_{E}-\dot{v}_{E}),\; \omega'_{E}=-gk_{g}(\hat{f}_{N}-\dot{v}_{N})$


Смещения нуля датчиков


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

Гироскопы. Разберёмся со смещениями нуля гироскопов $\delta{\mathbf{\omega}}$. Расчётное положение связанной системы координат уходит от своего истинного положения с угловой скоростью, определяемой двумя противодействующими факторами — смещениями нуля гироскопов и управляющей угловой скоростью: $\delta{\mathbf{\omega}}-\mathbf{\omega'}$. Если системе коррекции (например, в фильтре Махони) удалось остановить уход, то в установившемся режиме окажется $\delta{\mathbf{\omega}}=\mathbf{\omega'}$. Иными словами, в управляющей угловой скорости $\mathbf{\omega'}$ заключена информация о неизвестном действующем возмущении $\delta{\mathbf{\omega}}$. Поэтому можно применить компенсационное оценивание: мы не знаем величины возмущения непосредственно, однако знаем, какое корректирующее воздействие нужно, чтобы его уравновесить. На этом основано оценивание смещений нуля гироскопов. Например, у Махони оценка обновляется по закону

$\dot{\hat{\delta\mathbf{\omega}}}=k_{b}\mathbf{\omega'}$

Однако результат у него получается странный: оценки достигают 0,04 рад/с. Такой нестабильности смещений нуля не бывает даже у самых скверных гироскопов. Подозреваю, проблема связана с тем, что Махони не использует GNSS или другие внешние датчики — и в полной мере страдает от влияния ускорений. Только по вертикальной оси, где ускорения не вредят, оценка выглядит более или менее здравой:

image
Mahony et al., 2008

Акселерометры. Оценить смещения нуля акселерометров $\delta{\mathbf{f}}$ намного сложнее. Информацию о них приходится извлекать из той же управляющей угловой скорости $\mathbf{\omega'}$. Однако в прямолинейном движении эффект смещений нуля акселерометров неотличим от наклона носителя или перекоса установки блока датчиков на нём. Никакой добавки к $\mathbf{\omega'}$ акселерометры не создают. Добавка появляется только при развороте, что и позволяет разделить и независимо оценить погрешности гироскопов и акселерометров. Пример того, как это можно сделать, есть в моей статье. Вот картинки оттуда:

image

Вместо заключения: а что же с фильтром Калмана?


У меня нет сомнения, что описанные здесь фильтры почти всегда будут иметь преимущество перед традиционным фильтром Калмана в отношении быстродействия, компактности кода и удобства настройки — для этого они и создавались. Что касается точности оценивания, то здесь всё не столь однозначно. Мне встречались неудачно спроектированные фильтры Калмана, которые и по точности заметно проигрывали фильтру с виртуальной гироплатформой. Маджвик также доказывал выгоды своего фильтра относительно каких-то калмановских оценок. Однако для одной и той же задачи оценивания ориентации можно соорудить не менее десятка разных схем фильтра Калмана, и у каждой будет бесчисленное количество вариантов настройки. У меня нет никаких поводов думать, что фильтр Махони или Маджвика окажется точнее лучшего из возможных фильтров Калмана. И конечно, за калмановским подходом всегда останется преимущество универсальности: он не налагает никаких жёстких ограничений на конкретные динамические свойства оцениваемой системы.
AdBlock has stolen the banner, but banners are not teeth — they will be back

More
Ads

Comments 21

    0
    Я хотел бы задать немного вопросов по теме:

    Насколько я понял из статьи, принципиальная разница в фильтрах Махони и в Мэджвика в том, что в одном случае корректируется угловая скорость, а во втором кватернион? Если еще значительные различия в принципе работы?

    У кого из этих двух лучше с быстродействием?

    Как так получилось, что ардуинщики большинстве своём знают только о фильтре Мэджвика?
      0
      1. Для программиста отличия два: а) у Махони — матрица, у Маджвика — кватернион; б) у Махони корректируется угловая скорость, у Маджвика — скорость изменения кватерниона. Для математика важно ещё и то отличие, что у Махони решение строится геометрически, у Маджвика — аналитически.

      2. Не сравнивал, но предполагаю, что у Маджвика может выйти чуть быстрее за счёт меньшей избыточности (4 параметра ориентации вместо 9). Но Маджвик очевидно проиграет на этапе построения своих градиентов, якобианов и т.п., особенно если векторов больше одного и нужно ещё оценить уход гироскопов.

      3. Кроме смутных технических соображений из п. 2, могу предположить, что Маджвик больше занят саморекламой. Он коммерсант, а Махони — университетский учёный и уже давно сменил тему исследований. А ведь кроме Махони и Маджвика, было ещё много народу, кто занимался подобными вещами: и Боннабель, и тот же Салычев. О них вообще редко вспоминают.
        0
        Понято… Чтож, когда в следующий раз дойдут руки до навигации, попробую фильтр Махони…
        Оффтоп: С Салычевым я даже шапошно знаком :)… И книга у него весьма подробная. Фактически настольная. А про Боннабеля впервые слышу. Надо будет ознакомится.
          0
          У Боннабеля была очень амбициозная цель: построить общую «геометрическую» теорию таких фильтров, пригодных не только для навигации, но и вообще для всех задач, где используется, например, фильтр Калмана. Красиво размахнулся, но, увы, слабовато ударил.

          А у Салычева я и учился, и работал, и диссер защищал.
            0
            Может статься, мы встречались :)
      +1
      то ли из-за высоких требований к вычислительным ресурсам, неприемлемых для дронов
      Господа, я просто оставлю это здесь:
      The Apollo computer used 2k of magnetic core RAM and 36k wire rope [...]. The CPU was built from ICs [...]. Clock speed was under 100 kHz [...]. The fact that the MIT engineers were able to pack such good software (one of the very first applications of the Kalman filter) into such a tiny computer is truly remarkable.
      — Interview with Jack Crenshaw, by Matthew Reed, TRS-80.org (2009) [1]
        +1
        Да, это был шедевр. Не знаю, удалось ли кому-то такое повторить. Кстати, там интегрировалось кинематическое уравнение для матрицы поворота, а малые ошибки дооценивались фильтром Калмана. Ассемблерные исходники выкладывались в интернете.
        0
        Интересно, какой фильтр реализован в DPS InvenSens MPU. (https://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/) Сталкивались?
          0
          Не сталкивался. Но судя по документации — никакого. Упомянутый там Digital Motion Processor, вероятно, предназначен только для предобработки сырых измерений. Нигде не упомянуты выходные данные об ориентации, нет и сведений об их точности. У меня создалось впечатление, что реализация фильтра возлагается на пользователя.
            0
            Спасибо за ответ.
            Я с ним работал. Глубоко не копал. Мне нужна была стабилизация. Пробовал Магдвика на Ардуино. И их собственный DMP. Чисто на практике. Их соственный DMP оказался удобней. Не грузит Ардуино, а для меня было критичным передача данных с фиксированным (не переменным) лагом. Быстро поправляется, если обездвижить. Фильтр Магдвига, я так и не понял как настраивать). Он иногда давал необъяснимые большие всплески. И судя по вашей статье, возможно (может ошибаюсь), это «фича» алгоритма, в отсутствие внешних датчиков. Invensense вроде пишет, что их собственный алгоритм учитывает все датчики, и акселерометр и гироскопы (на одной вафле), а фильтр магдвига (судя по коду который я использовал) этого не делает.
              0
              Для меня так и осталось непонятным, оценивает ли он углы. По вашим словам выходит, что всё-таки оценивает. Однако в документации я ни слова о них по-прежнему не вижу.

              Фильтр Маджвика, разумеется, может использовать и акселерометры, и магнитометры наряду с гироскопами. Тем не менее, каждая область применения (самолёт, дрон, автомобиль, пешеход), по-хорошему, требует своих настроек фильтра — хоть Калмана, хоть Махони, хоть Маджвика. Поэтому мораль своей статьи я вижу именно в том, чтобы сподвигнуть публику самостоятельно изобретать, дорабатывать и настраивать фильтры ориентации, а не полагаться на готовые решения.
                0
                Благородная цель. Однако публика низка и ленна). Но ваша статья безусловно облегчила понимание. Спасибо.
          0
          А вот еще вопрос в догонку.
          Поведение системы ориентации на земле, когда есть реакция опоры и в воздухе — две разные вещи.

          Как отлаживать фильтры для дронов, чтобы не получить много удивления в момент отрыва аппарата от земли?
            0
            Сложный вопрос. Боюсь, удивляться придётся. Именно для дронов я фильтры не делал, но делал для самолётов и вертолётов. Там самая большая проблема была не с наличием или отсутствием реакции опоры, а с ускорениями и вибрациями. Более или менее адекватную имитацию этих условий (в смысле погрешностей датчиков) давал автомобиль — этим я пользовался в своём диссере. Во всяком случае, автомобиль намного больше похож на самолёт, чем стол :)
              0
              подвешивать и обдувать напольным вентилятором
                0
                Но точно не для дрона самолётного типа — там направленное движение и виражи играют слишком большую роль :)
                0
                Можно попробовать вначале вешать балластом на уже гарантированно летающего дрона не включая в управляющую схему.
                0
                Приведу здесь некоторую информацию, касательно фильтра Маджвика. Может быть, кому-нибудь она будет полезна. Это, кстати, по странному стечению обстоятельств говорит в пользу тезиса автора данного топика, о том, что не всегда стоит полагаться на готовые решения.

                Я не очень сильно разбираюсь в математической подоплёке реализации фильтра, однако есть обсуждение фильтра Маджвика, где упоминается пара проблем в авторской реализации. Одна связана с быстрым обратным корнем. И вторая, как утверждается (ближе к концу обсуждения), в ошибочной реализации расчёта градиентов в версии с магнитометром для языка Си.

                Выше упоминалось, о не слишком удачном опыте использования данного фильтра на arduino. Если посмотреть на одну из реализаций библиотеки для arduino, то она повторяет оригинальную авторскую версию на Си. Не знаю, может ли это быть одной из причиной. Мои эксперименты, пока, не дали однозначного ответа, насколько данные замечания критичны для работы фильтра.
                  0
                  Спасибо. Вопрос об ошибке в градиенте при использовании магнитометра уже поднимали в комментариях к тому посту, на который я ссылаюсь в начале. Для меня это ещё один аргумент в пользу фильтров с более «геометрическим» и наглядным корректирующим членом, чем у Маджвика.
                  0
                  Если кто-то интересуется работами Маджвика: у него есть три известных публикации по одноименному фильтру (2010, 2011 и 2014 гг.).
                  Наиболее известна публикация 2010 г. – она представляет собой отчет по НИОКР в Бристольском Университете (связанной с компьютеризированной хирургией, если правильно помню). В ней описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Приведены формулы для расчета коэффициентов фильтрации шума и систематической ошибки (дрейфа) сигналов гироскопа.
                  В 2011 г. Маджвик сотоварищи опубликовал вторую работу (тезисы выступления на конференции IEEE on Rehabilitation Robotics), в которой описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Кроме того, в ней приведена формула для расчета коэффициента фильтрации шума сигналов гироскопа и экспериментальные данные о влиянии величины этого коэффициента на параметры фильтра.
                  В 2014 г. он защитил в Бристольском Университете докторскую диссертацию на основе описанных выше разработок и затем потерял интерес к этой теме. Понятно, что диссертация – наиболее капитальная из его публикаций. Кто хочет разобраться – лучше читать именно ее.
                  1. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io and University of Bristol (UK) vol. 25, 113–118, 30.04.2010
                  2. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm. 2011 IEEE International Conference on Rehabilitation Robotics, Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 – July 1, 2011
                  3. Sebastian O.H. Madgwick. AHRS algorithms and calibration solutions to facilitate new applications using low-cost MEMS. PhD Theses, Department of Mechanical Engineering, University of Bristol, Bristol, UK, 2014
                    0
                    Если посмотреть реализацию фильтра Махони в INAV можно заметить то о чём вы рассказывали GPS или датчик воздушной скорости используется для компенсации дрейфа акселерометров в момент больших перегрузок или показания акселерометра вообще не используются:

                    Реализация фильтра Махони в INAV


                    /**
                     * In Cleanflight accelerometer is aligned in the following way:
                     *      X-axis = Forward
                     *      Y-axis = Left
                     *      Z-axis = Up
                     * Our INAV uses different convention
                     *      X-axis = North/Forward
                     *      Y-axis = East/Right
                     *      Z-axis = Up
                     */
                    
                    // the limit (in degrees/second) beyond which we stop integrating
                    // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
                    // which results in false gyro drift. See
                    // http://gentlenav.googlecode.com/files/fastRotations.pdf
                    #define SPIN_RATE_LIMIT     20
                    #define MAX_ACC_SQ_NEARNESS 25      // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
                    
                    t_fp_vector imuAccelInBodyFrame;
                    t_fp_vector imuMeasuredGravityBF;
                    t_fp_vector imuMeasuredRotationBF;
                    float smallAngleCosZ = 0;
                    
                    float magneticDeclination = 0.0f;       // calculated at startup from config
                    static bool isAccelUpdatedAtLeastOnce = false;
                    
                    STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
                    STATIC_UNIT_TESTED float rMat[3][3];
                    
                    attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
                    
                    static imuRuntimeConfig_t *imuRuntimeConfig;
                    static pidProfile_t *pidProfile;
                    
                    static float gyroScale;
                    
                    static bool gpsHeadingInitialized = false;
                    
                    STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
                    {
                        float q1q1 = q1 * q1;
                        float q2q2 = q2 * q2;
                        float q3q3 = q3 * q3;
                        
                        float q0q1 = q0 * q1;
                        float q0q2 = q0 * q2;
                        float q0q3 = q0 * q3;
                        float q1q2 = q1 * q2;
                        float q1q3 = q1 * q3;
                        float q2q3 = q2 * q3;
                    
                        rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
                        rMat[0][1] = 2.0f * (q1q2 + -q0q3);
                        rMat[0][2] = 2.0f * (q1q3 - -q0q2);
                    
                        rMat[1][0] = 2.0f * (q1q2 - -q0q3);
                        rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
                        rMat[1][2] = 2.0f * (q2q3 + -q0q1);
                    
                        rMat[2][0] = 2.0f * (q1q3 + -q0q2);
                        rMat[2][1] = 2.0f * (q2q3 - -q0q1);
                        rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
                    }
                    
                    void imuConfigure(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile)
                    {
                        imuRuntimeConfig = initialImuRuntimeConfig;
                        pidProfile = initialPidProfile;
                    }
                    
                    void imuInit(void)
                    {
                        int axis;
                    
                        smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
                        gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
                    
                        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                            imuAccelInBodyFrame.A[axis] = 0;
                        }
                    
                        imuComputeRotationMatrix();
                    }
                    
                    void imuTransformVectorBodyToEarth(t_fp_vector * v)
                    {
                        float x,y,z;
                    
                        /* From body frame to earth frame */
                        x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
                        y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
                        z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
                    
                        v->V.X = x;
                        v->V.Y = -y;
                        v->V.Z = z;
                    }
                    
                    void imuTransformVectorEarthToBody(t_fp_vector * v)
                    {
                        float x,y,z;
                    
                        v->V.Y = -v->V.Y;
                        
                        /* From earth frame to body frame */
                        x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z;
                        y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y + rMat[2][1] * v->V.Z;
                        z = rMat[0][2] * v->V.X + rMat[1][2] * v->V.Y + rMat[2][2] * v->V.Z;
                    
                        v->V.X = x;
                        v->V.Y = y;
                        v->V.Z = z;
                    }
                    
                    static float invSqrt(float x)
                    {
                        return 1.0f / sqrtf(x);
                    }
                    
                    STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
                    {
                        if (initialRoll > 1800) initialRoll -= 3600;
                        if (initialPitch > 1800) initialPitch -= 3600;
                        if (initialYaw > 1800) initialYaw -= 3600;
                    
                        float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
                        float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
                    
                        float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
                        float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
                    
                        float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
                        float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
                    
                        q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
                        q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
                        q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
                        q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
                    
                        imuComputeRotationMatrix();
                    }
                    
                    static bool imuUseFastGains(void)
                    {
                        return !ARMING_FLAG(ARMED) && millis() < 20000;
                    }
                    
                    static float imuGetPGainScaleFactor(void)
                    {
                        if (imuUseFastGains()) {
                            return 10.0f;
                        }
                        else {
                            return 1.0f;
                        }
                    }
                    
                    static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                                    int accWeight, float ax, float ay, float az,
                                                    bool useMag, float mx, float my, float mz,
                                                    bool useCOG, float courseOverGround)
                    {
                        static float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    // integral error terms scaled by Ki
                        static float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    // integral error terms scaled by Ki
                        float recipNorm;
                        float ex, ey, ez;
                        float qa, qb, qc;
                    
                        /* Calculate general spin rate (rad/s) */
                        float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
                    
                        /* Step 1: Yaw correction */
                        // Use measured magnetic field vector
                        if (useMag || useCOG) {
                            float kpMag = imuRuntimeConfig->dcm_kp_mag * imuGetPGainScaleFactor();
                    
                            recipNorm = mx * mx + my * my + mz * mz;
                            if (useMag && recipNorm > 0.01f) {
                                // Normalise magnetometer measurement
                                recipNorm = invSqrt(recipNorm);
                                mx *= recipNorm;
                                my *= recipNorm;
                                mz *= recipNorm;
                    
                                // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
                                // This way magnetic field will only affect heading and wont mess roll/pitch angles
                    
                                // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
                                // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
                                float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
                                float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
                                float bx = sqrtf(hx * hx + hy * hy);
                    
                                // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
                                float ez_ef = -(hy * bx);
                    
                                // Rotate mag error vector back to BF and accumulate
                                ex = rMat[2][0] * ez_ef;
                                ey = rMat[2][1] * ez_ef;
                                ez = rMat[2][2] * ez_ef;
                            }
                            else if (useCOG) {
                                // Use raw heading error (from GPS or whatever else)
                                while (courseOverGround >  M_PIf) courseOverGround -= (2.0f * M_PIf);
                                while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
                    
                                // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
                                // (Rxx; Ryx) - measured (estimated) heading vector (EF)
                                // (cos(COG), sin(COG)) - reference heading vector (EF)
                                // error is cross product between reference heading and estimated heading (calculated in EF)
                                float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0];
                    
                                ex = rMat[2][0] * ez_ef;
                                ey = rMat[2][1] * ez_ef;
                                ez = rMat[2][2] * ez_ef;
                            }
                            else {
                                ex = 0;
                                ey = 0;
                                ez = 0;
                            }
                    
                            // Compute and apply integral feedback if enabled
                            if(imuRuntimeConfig->dcm_ki_mag > 0.0f) {
                                // Stop integrating if spinning beyond the certain limit
                                if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
                                    integralMagX += imuRuntimeConfig->dcm_ki_mag * ex * dt;    // integral error scaled by Ki
                                    integralMagY += imuRuntimeConfig->dcm_ki_mag * ey * dt;
                                    integralMagZ += imuRuntimeConfig->dcm_ki_mag * ez * dt;
                    
                                    gx += integralMagX;
                                    gy += integralMagY;
                                    gz += integralMagZ;
                                }
                            }
                    
                            // Calculate kP gain and apply proportional feedback
                            gx += kpMag * ex;
                            gy += kpMag * ey;
                            gz += kpMag * ez;
                        }
                    
                    
                        /* Step 2: Roll and pitch correction -  use measured acceleration vector */
                        if (accWeight > 0) {
                            float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor();
                    
                            // Just scale by 1G length - That's our vector adjustment. Rather than 
                            // using one-over-exact length (which needs a costly square root), we already 
                            // know the vector is enough "roughly unit length" and since it is only weighted
                            // in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
                            recipNorm = 1.0f / GRAVITY_CMSS;
                    
                            ax *= recipNorm;
                            ay *= recipNorm;
                            az *= recipNorm;
                    
                            float fAccWeightScaler = accWeight / (float)MAX_ACC_SQ_NEARNESS;
                    
                            // Error is sum of cross product between estimated direction and measured direction of gravity
                            ex = (ay * rMat[2][2] - az * rMat[2][1]) * fAccWeightScaler;
                            ey = (az * rMat[2][0] - ax * rMat[2][2]) * fAccWeightScaler;
                            ez = (ax * rMat[2][1] - ay * rMat[2][0]) * fAccWeightScaler;
                    
                            // Compute and apply integral feedback if enabled
                            if(imuRuntimeConfig->dcm_ki_acc > 0.0f) {
                                // Stop integrating if spinning beyond the certain limit
                                if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
                                    integralAccX += imuRuntimeConfig->dcm_ki_acc * ex * dt;    // integral error scaled by Ki
                                    integralAccY += imuRuntimeConfig->dcm_ki_acc * ey * dt;
                                    integralAccZ += imuRuntimeConfig->dcm_ki_acc * ez * dt;
                    
                                    gx += integralAccX;
                                    gy += integralAccY;
                                    gz += integralAccZ;
                                }
                            }
                    
                            // Calculate kP gain and apply proportional feedback
                            gx += kpAcc * ex;
                            gy += kpAcc * ey;
                            gz += kpAcc * ez;
                        }
                    
                        // Integrate rate of change of quaternion
                        gx *= (0.5f * dt);
                        gy *= (0.5f * dt);
                        gz *= (0.5f * dt);
                    
                        qa = q0;
                        qb = q1;
                        qc = q2;
                        q0 += (-qb * gx - qc * gy - q3 * gz);
                        q1 += (qa * gx + qc * gz - q3 * gy);
                        q2 += (qa * gy - qb * gz + q3 * gx);
                        q3 += (qa * gz + qb * gy - qc * gx);
                    
                        // Normalise quaternion
                        recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
                        q0 *= recipNorm;
                        q1 *= recipNorm;
                        q2 *= recipNorm;
                        q3 *= recipNorm;
                    
                        // Pre-compute rotation matrix from quaternion
                        imuComputeRotationMatrix();
                    }
                    
                    STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
                    {
                        /* Compute pitch/roll angles */
                        attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
                        attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
                        attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination;
                    
                        if (attitude.values.yaw < 0)
                            attitude.values.yaw += 3600;
                    
                        /* Update small angle state */
                        if (rMat[2][2] > smallAngleCosZ) {
                            ENABLE_STATE(SMALL_ANGLE);
                        } else {
                            DISABLE_STATE(SMALL_ANGLE);
                        }
                    }
                    
                    // Idea by MasterZap
                    static int imuCalculateAccelerometerConfidence(void)
                    {
                        int32_t axis;
                        int32_t accMagnitude = 0;
                    
                        for (axis = 0; axis < 3; axis++) {
                            accMagnitude += (int32_t)accADC[axis] * accADC[axis];
                        }
                    
                        // Magnitude^2 in percent of G^2
                        accMagnitude = accMagnitude * 100 / ((int32_t)acc.acc_1G * acc.acc_1G);
                    
                        int32_t nearness = ABS(100 - accMagnitude);
                    
                        return (nearness > MAX_ACC_SQ_NEARNESS) ? 0 : MAX_ACC_SQ_NEARNESS - nearness;
                    }
                    
                    static bool isMagnetometerHealthy(void)
                    {
                        return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
                    }
                    
                    static void imuCalculateEstimatedAttitude(float dT)
                    {
                        float courseOverGround = 0;
                    
                        int accWeight = 0;
                        bool useMag = false;
                        bool useCOG = false;
                    
                        accWeight = imuCalculateAccelerometerConfidence();
                    
                        if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
                            useMag = true;
                        }
                    #if defined(GPS)
                        else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
                            // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
                            if (gpsHeadingInitialized) {
                                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
                                useCOG = true;
                            }
                            else {
                                // Re-initialize quaternion from known Roll, Pitch and GPS heading
                                imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
                                gpsHeadingInitialized = true;
                            }
                        }
                    #endif
                    
                        imuMahonyAHRSupdate(dT,     imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
                                            accWeight, imuMeasuredGravityBF.A[X], imuMeasuredGravityBF.A[Y], imuMeasuredGravityBF.A[Z],
                                            useMag, magADC[X], magADC[Y], magADC[Z],
                                            useCOG, courseOverGround);
                    
                        imuUpdateEulerAngles();
                    }
                    
                    /* Calculate rotation rate in rad/s in body frame */
                    static void imuUpdateMeasuredRotationRate(void)
                    {
                        int axis;
                    
                        for (axis = 0; axis < 3; axis++) {
                            imuMeasuredRotationBF.A[axis] = gyroADC[axis] * gyroScale;
                        }
                    }
                    
                    /* Calculate measured acceleration in body frame cm/s/s */
                    static void imuUpdateMeasuredAcceleration(void)
                    {
                        int axis;
                    
                        /* Convert acceleration to cm/s/s */
                        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                            imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
                            imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
                        }
                    
                    #ifdef GPS
                        /** Centrifugal force compensation on a fixed-wing aircraft
                          * a_c_body = omega x vel_tangential_body
                          * assumption: tangential velocity only along body x-axis
                          * assumption: GPS velocity equal to body x-axis velocity
                          */
                        if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
                            imuMeasuredGravityBF.A[Y] -= gpsSol.groundSpeed * imuMeasuredRotationBF.A[Z];
                            imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
                        }
                    #endif
                    }
                    
                    #ifdef HIL
                    void imuHILUpdate(void)
                    {
                        /* Set attitude */
                        attitude.values.roll = hilToFC.rollAngle;
                        attitude.values.pitch = hilToFC.pitchAngle;
                        attitude.values.yaw = hilToFC.yawAngle;
                    
                        /* Compute rotation quaternion for future use */
                        imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
                    
                        /* Fake accADC readings */
                        accADC[X] = hilToFC.bodyAccel[X] * (acc.acc_1G / GRAVITY_CMSS);
                        accADC[Y] = hilToFC.bodyAccel[Y] * (acc.acc_1G / GRAVITY_CMSS);
                        accADC[Z] = hilToFC.bodyAccel[Z] * (acc.acc_1G / GRAVITY_CMSS);
                    }
                    #endif
                    
                    void imuUpdateAccelerometer(void)
                    {
                    #ifdef HIL
                        if (sensors(SENSOR_ACC) && !hilActive) {
                            updateAccelerationReadings();
                            isAccelUpdatedAtLeastOnce = true;
                        }
                    #else
                        if (sensors(SENSOR_ACC)) {
                            updateAccelerationReadings();
                            isAccelUpdatedAtLeastOnce = true;
                        }
                    #endif
                    }
                    
                    void imuUpdateGyroAndAttitude(void)
                    {
                        /* Calculate dT */
                        static uint32_t previousIMUUpdateTime;
                        uint32_t currentTime = micros();
                        float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
                        previousIMUUpdateTime = currentTime;
                    
                        /* Update gyroscope */
                        gyroUpdate();
                    
                        if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
                    #ifdef HIL
                            if (!hilActive) {
                                imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
                                imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
                                imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
                            }
                            else {
                                imuHILUpdate();
                                imuUpdateMeasuredAcceleration();
                            }
                    #else
                                imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
                                imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
                                imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
                    #endif
                        } else {
                            accADC[X] = 0;
                            accADC[Y] = 0;
                            accADC[Z] = 0;
                        }
                    }
                    
                    bool isImuReady(void)
                    {
                        return sensors(SENSOR_ACC) && isGyroCalibrationComplete();
                    }
                    
                    bool isImuHeadingValid(void)
                    {
                        return (sensors(SENSOR_MAG) && persistentFlag(FLAG_MAG_CALIBRATION_DONE)) || (STATE(FIXED_WING) && gpsHeadingInitialized);
                    }
                    
                    float calculateCosTiltAngle(void)
                    {
                        return rMat[2][2];
                    }
                    
                    float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
                    {
                        if (throttleTiltCompensationStrength) {
                            float tiltCompFactor = 1.0f / constrainf(rMat[2][2], 0.6f, 1.0f);  // max tilt about 50 deg
                            return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
                        }
                        else {
                            return 1.0f;
                        }
                    }
                    
                    




                    Вопрос по фильтру Калмона может ли данный фильтр без дополнительных данных GPS или воздушной скорости самостоятельно решать когда использовать данные ускорения для востановления дрейфа гиросков а когда нет только при помощи IMU тройки? В своих тестах на коленке я замечал, что резкое линейное ускорение (от броска БЛА) в фильтре Маждвика приводит к уплыванию показаний тонгажа и крена, на BNO 055 с фильтром Калмона такой проблемы нет но зная, что в INAV для компенсации используются дополнительные датчики непонятно как это может быть реализовано без них.

                    Only users with full accounts can post comments. Log in, please.