Фильтр Маджвика часть 2

4. Испытания

4.1 Оборудование


Фильтр был протестирован с использованием блока датчиков ориентации xsens MTX [18], содержащий 16-битные трехосевые: гироскоп, акселерометр и магнитометр. Устройство и соответствующее программное обеспечение используют режим операций, где сырые данные с датчиков поступают с частотой 512 Гц, а затем обрабатываются, чтобы обеспечить калиброванные измерения датчика. Калиброванные измерения датчика могут быть обработаны с помощью предлагаемого фильтра, чтобы обеспечить расчетную ориентацию датчика. Программное обеспечение также включает в себя вычисление дополнительной оценки ориентации Калмано-подобным фильтром. Оба фильтра — Калмано-подобный и предложенный, могут работать с использованием одних и тех же выходных данных датчиков. Точность каждого алгоритма оценивается по отношению к друг другу как точность независимых датчиков.



Система Vicon, состоящая из 8 MX3 + камера, подключенные к серверу MXultranet [46] и Nexus [47] программное обеспечение было использовано, чтобы обеспечить опорные измерения фактической ориентировки датчика ориентации. Система массива ИК (инфракрасный) чувствительных камер с включенными прожекторами IR. Камеры фиксируют на калиброванных положениях и ориентациях так, чтобы объект измерения был в поле зрения нескольких камер. Декартовы позиции ИК отражающих оптических маркеров, закрепленных на объект измерения могут быть вычислены в системе координат массива камер. Камеры были установлены на высоте примерно 2,5 м и равномерно распределены по всему периметру площадки 4 м на 4 м. Каждая камера была сориентирована на центр комнаты, приблизительно от 30◦ до 60◦ к горизонтали. Эксперименты проводились с объектом измерения в центре комнаты на высоте примерно 1 м. Для измерения ориентации датчика, он был прикреплен к измерительной платформе оптической ориентации, специально предназначенной для этого эксперимента. Система использовалась, чтобы регистрировать позиции оптических маркеров со скоростью 120 Гц.



4.2 Определение ориентации из оптических измерений


Платформа измерения ориентации состоит из 3,5 м, взаимно ортогональных стержней, жестко соединенных в центре.На каждом конце стержня закреплены оптические маркеры и в их пересечении закреплена платформа с датчиком ориентации. Платформа была изготовлена из алюминия, стержни из углеродного волокна и всё собрано с использованием клеев для того, чтобы у конструкции не было магнитных свойств, которые могут помешать магнетометру. Дополнительные оптические маркеры были помещены в произвольных положениях вдоль длины стержней нарушая осевую симметрию для того, чтобы помочь идентифицировать каждый стержень в данном измерении. На рис. 4 представлена фотография платформы измерения ориентации, где , ,
, , и — это измеренное положение каждого маркера рамки в пределах видимости камеры. Эти позиции могут быть использованы для определения 3-х взаимно ортогональных векторов , и в системе координат камеры, получая направления осей платформы X, Y, Z так. как это показано в уравнении (52), (53) и (54). Эти векторы определяют матрицу вращения, описывающую ориентацию измерительной платформы в системе координат камеры, как показано в уравнении (55).






Рис. 4. Фотография платформы измерения ориентации





Из-за ошибок измерений и допусков в конструкции рамки с маркерами, матрица вращения, определяемая уравнением (55), не может считаться ортогональной и поэтому не представляет собой чистое вращение. Бар-Ицхак представляет нам метод (48), где по оптимальному «лучшему соответствию» может быть извлечён кватернион[9] из не точной и не ортогональной матрицы поворота. Метод требует построения симметричной матрицы 4х4, К, определяемую уравнением (56), где соответствует m-ной строке n-ого столбца . Оптимальный кватернион[9] находится как нормализованный собственный вектор, соответствующий максимальному собственному значению матрицы К. Уравнение (57) определяет оптимальную альтернативу. Уравнение (57) определяет оптимальный альтернативный кватернион[9], как условно предполагается методом, где V1, V2, V3 и V4 определяют элементы нормализованного собственного вектора.





4.3 Калибровка совмещения систем отчёта


Для того, чтобы сравнить оптические измерения ориентации платформы в кадре камеры, , и предполагаемую фильтром ориентацию относительно Земли, , требуется знать выравнивание системы отчёта Земли относительно системы отчёта камеры, , и выравнивание измерительной платформы, относительно системы отчёта датчика . После того, как эти величины найдены, оптическое измерение ориентации датчика в системе отчёта Земли, , определяется по формуле (58). Хотя использование оптического оборудования описано в приложениях к документам [26, 24, 41], есть предложение немного обсудить калибровку этих двух величин.




Оси X и Z системы отчёта Земли определяются её магнитным полем и гравитацией. Измерение этих полей в системе отчёта камеры могут быть использованы для выравнивания . Направление силы тяжести определяют с помощью нити с грузиком (маятника), длиной 1 метр, прикреплённой к платформе.Оптический маркер помещается на грузике и достигается окончание раскачивания, т. е. грузик приводится в неподвижное состояние.



Дополнительный оптический маркер требуются при произвольном фиксированном месте относительно статического маятника (грузика), чтобы нарушить осевую симметрию «созвездия» оптических маркеров. Рис. 5 показывает прокомментированную фотографию маятника, где и определяют положение маркеров в системе отчёта камеры. Среднее положение каждого маркера в течение определенного периода времени, определяет направление маятника в системе отчёта камеры, . Это непосредственно определяет ось Z системы отчёта Земли в системе отчёта камеры, , как показано в уравнении (59).







Рис. 5: Фотография маятника и оптических маркеров, используемых для измерения направления силы тяжести в системе отчёта камеры



Направление магнитного поля Земли было измерено с помощью магнитного компаса, изготовленного из метрового стержня из углеродного волокна с неодимовыми магнитами на концах, прикрепленных к каждому концу. Направление северного и южного полюса магнитов совпадает. Компас подвешен на хлопчатобумажной нити и приведен в состояние покоя. Оптические маркеры размещены на концах стержня, а также несимметрично в других местах вдоль стержня, чтобы нарушить осевую симметрию «созвездия» (вероятно чтобы идентифицировать каждый конец). На рис. 6 показана прокомментированная фотография компаса, где и определяю положние оптических маркеров в системе отчёта камеры. Среднее положение каждого маркера в течении некоторого периода времени определяет положение компаса в системе отчёта камеры, , как показано в выражении (60).





Рисунок 6: Фотография магнитного компаса и оптических маркеров, используемых для измерения направления магнитного поля Земли в системе отчёта камеры





Из-за ошибок измерения и дисбаланса подвешенного магнитного компаса, нельзя считать ортогональным по направлению к силе тяжести, определённого как и поэтому не может быть использовано для прямого определения оси X Земли. Неортогональность может быть вычислена как проекция на ось . Эта составляющая может быть удалена из для определения направления оси X Земли в системе отчёта камеры , как показано в выражении (61). После нормализации мы получаем ось Х Земли, , как показано в выражении (62).





Ось Y системы отчёта Земли в системе отчёта камеры может быть вычислено как перпендикуляр к и , как определено в уравнении (63) и где знаки выбираются так, чтобы направление осей соответствовало соглашению (имеется ввиду правая и левая система координат). Выравнивание земной системы отчёта может быть определено как матрица поворота , построенная из , и. Кватернионное представление может быть получено по методу Бара-Ицхака [48].





Для того, чтобы найти выравнивание , предполагается, что статическая погрешность фильтра основанного на методе Калмана в среднем равна нулю. Средняя величина на выходе была вычислена при измерении платформы, находящейся в неподвижном состоянии в течении примерно 10 секунд. Это было использовано с выравниванием и оптическим измерением для определения выравнивания платформы в системе отчёта датчика , как определено в выражении (65).





4.4. Процедура проведения экпериментов


Оптические данные измерения и сырые данные датчиков ориентации регистрировались одновременно. Затем сырые данные датчиков ориентации были обработаны соответствующей программой, чтобы откалибровать данные датчика и выход фильтра на основе Калмана. Эти данные затем синхронизируются с данными оптических измерений, при этом данные оптических измерений интерполируются из-за более высокой частоты замеров датчиков ориентации. Затем калиброванные данные датчиков обрабатываются с помощью обоих вариантов предлагаемого метода: с магнитометром и без. Оптические данные извлекаются методами описанными в 4.2 и 4.3.



Коэффициент усиления β предлагаемого фильтра был установлен в 0,033 для реализации без магнитометра и 0,041 для реализации с магнитометром. Обобщения из раздела 5.3 показывают, что найденные коэффициенты обеспечивают максимальную точность. Тем не менее, в течении первых 10 секунд использовался коэффициент 2,5, чтобы обеспечить сходимость алгоритма при начальных условиях.



Коэффициент усиления ζ, применимый только к варианту с гироскопом, был установлен в 0, так как калибровка датчиков ориентации не предполагает дрейф смещения гироскопа.



Данные были получены для последовательности поворотов произведённых вручную. Измерительная платформа приводится в состояние покоя на 20-30 секунд, чтобы дать время алгоритму стабилизироваться. Затем платформу поворачивают на 90◦ вокруг своей оси Х, а затем на 180◦ в противоположном направлении, а затем делается поворот на 90◦ для приведения платформы в исходное положение. Платформа между каждым вращением удерживается в неподвижном состоянии от 3 до 5 секунд. Эта последовательность затем повторяется для осей Y и Z. Пик угловой скорости измеряемый в течении каждого поворота составлял от 110◦/с до 190◦/с. Эксперимент повторялся 8 раз, чтобы получить представление о точности системы.



5. Результаты


Среди общепринятых методов количественной оценки характеристик фильтра ориентации [24, 26, 18, 19, 20, 21] мы видим среднеквадратичные ошибки в подвижном (динамические) и неподвижном (статические) состоянии в отрыве от углов Эйлера, описывающих курс, крен, тангаж. Тангаж φ, крен θ и курс ψ соответствуют вращению вокруг осей X, Y, Z соответственно в системе отчёта датчика. Вышеупомянутые углы отвязаны от углов Эйлера и могут быть более просто интерпретироваться и визуализироваться. Недостатком углов Эйлера является то, что в них не описывается связь между параметрами, а также они подвержены крупным неустойчивым ошибкам, когда значения некоторых углов достигают полюсов.



Углы Эйлера были вычислены непосредственно из получаемого кватерниона с использованием уравнений (7), (8) и (9).
В общей сложности были вычислены 4 набора параметров Эйлера, они соответствуют калиброванным оптическим измерениям ориентации, оценки ориентации фильтром на основе метода Калмана и предлагаемый фильтр оценки ориентации для реализаций с датчиком и без. Ошибки оцениваемых параметров Эйлера: φ, θ and ψ, были вычислены как разность между параметрами Эйлера калиброванных оптических измерений и соответствующими параметрами на выходе из фильтра.



5.1. Средние результаты


На рис. 7, 8 и 9 показаны средние результаты для 8 экспериментов как для фильтра на основе метода Калмана и предлагаемая реализация фильтра с магнитометром. В каждом рисунке 3 графика. Верхний график представляет оптически измеренный угол. Далее оценка угла фильтром на основе метода Калмана, и наконец оценка угла представляемым фильтром. Два графика внизу представляют расчётную ошибку в каждом из предлагаемых углов.




Рис 7. Средние результаты измерений и оценок угла φ (вверху) и ошибки оценки (внизу)




Рис 8. Средние результаты измерений и оценок угла θ (вверху) и ошибки оценки (внизу)




Рис 9. Средние результаты измерений и оценок угла ψ (вверху) и ошибки оценки (внизу)



5.2. Статические и динамические характеристики


Среднеквадратичные отклонения углов , и рассчитывались из предположения, что угловая скорость в неподвижном состоянии была < 5 ◦/сек, а в подвижном ≥ 5 ◦/сек.



Этот порог был выбран так, чтобы быть достаточно высоким по отношению к уровню шума входных данных. Каждое значение среднеквадратичного отклонения вычислялось в отрезок времени, в котором производились вращения соответствующего угла Эйлера, как показано на рис. 7, 8 и 9. Так было сделано, чтобы предотвратить ошибки из-за первоначального сближения или особенностей в представлении углов Эйлера, то есть когда θ = ± 90 (речь о складывании рамок). Результаты отражены в таблице 1. Каждое значение представляет собой среднее всех 8 экспериментов. Значения среднеквадратичного отклонения не вычислялись для реализации без магнитометра, так как данная реализация не предназначена для точного определения курса и не может его компенсировать накопление ошибок в этом параметре.




Таблица 1. Среднеквадратичные отклонения для фильтров: на основе метода Калмана и предложенного метода в реализациях с магнитометром и без



Результаты показывают, что предлагаемый фильтр достигает более высокой точности, чем фильтр на основе метода Калмана. Производители датчиков ориентации указывают средние отклонения фильтров на основе метода Калмана в пределах < 0.5◦ для углов φ и θ и < 1◦ для ψ [18]. Эти значения не соответствуют приведённым в таблице 1. Другие исследования [49] показали, что точность может быть значительно меньше, чем приведённые изготовителями и что указываемый ими уровень точности достигается только во время калибровки. Нмзкие уровни точности в измерении курса ψ вызваны низким уровнем точности датчика в измерении магнитного поля Земли. Наклон магнитного поля Земли во время тестирования был между 65◦ и 70◦ по отношению к горизонту [39]. Как следствие компонент вектора магнитного потока для определения курса относительно невелик. Больший компонент вектора, наряду с измерением гравитации, служит дополнительным критерием для определения крена θ и тангажа φ. Следовательно среднеквадратичное отклонение по тангажу и крену будет меньше, чем по курсу . Магнитометр, как указано в [18], имеет пропускную способность 10 Гц, которая по сравнению с пропускной способностью акселерометра и гироскопа, которая составляет от 30 до 40 Гц, также предполагает повышенную ошибку в определении курса в неподвижном состоянии.



5.3. Влияние коэффициента усиления фильтра на точность


Результаты исследования влияния коэффициента усиления β на точность фильтра отображены в виде графика на рис. 10. Экспериментальные данные были обработаны ограничиваясь значениями β в пределах между 0 и 0,5. Для каждой реализации фильтра существует своё оптимальное значение β, которое достаточно высокое для того, чтобы компенсировать накопление ошибок и достаточно низкое, чтобы ненужный шум не попадал в большие шаги градиентного спуска.




Рис. 10. Влияние коэффициента усиления β фильтра на точность



5.4. Влияние частоты замеров на точность


На рис. 11 представлены результаты исследования влияния частоты замеров на точность фильтра. Эксперименты проводились, используя оптимальное значение коэффициента усиления β для обеих реализаций (с магнитометром и без). Изменение частоты замеров было сымитировано путём пропуска части замеров и составило от 1 Гц до 512 Гц. Из рис. 11 можно видеть, что предлагаемый фильтр достигает такого же уровня точности при частоте замеров 50 Гц, как при частоте 512 Гц. Обе реализации фильтра достигают снижение статической ошибки (в неподвижном состоянии) < 2◦ и динамической ошибки (в подвижном состоянии платформы) < 7◦ уже при частоте замеров 10 Гц. Этот уровень точности достаточен для использования в приложениях по захвату движений человека.




Рис. 11. Влияние частоты замеров на точность фильтра для реализаций с магнитометром и без



5.5. Дрейф нуля гироскопа


Калиброванные данные гироскопа, используемые в предлагаемом фильтре, не содержат каких-либо ошибок смещения. Чтобы исследовать способность предлагаемого фильтра в компенсации дрейфа смещения, ошибки были искусственно введены в данные всех 8 экспериментов. Постоянный дрейф 0.2◦/сек/сек был добавлен к замерам гироскопа по оси X и постоянная ошибка смещения −0.2 ◦/сек/сек к замерам гироскопа по оси Y . Коэффициент усиления ζ выставлялся в 0 в течении первых 10 секунд во время каждого эксперимента, пока фильтр сходился с начальными условиями. После этого значение устанавливалось в 0,015 как соответствующее максимальной скорости смещения гироскопа 1◦/сек/сек. Рис. 12 показывает средние результаты 8 экспериментов, показывая смещения нуля гироскопа по оценкам фильтра от действительного положения по осям гироскопа X и Y. Точность фильтра в этих условиях описаны в таблице 1.




Рис. 12 Следящий фильтр дрейфа нуля гироскопа



6. Обсуждение


В начале работы над фильтром предполагалось, что акселерометр и магнитометр будут измерять только силу тяжести и магнитное поле Земли. На практике, из-за ускорений движения это приводит к ошибочному наблюдаемому направлению силы тяжести (особенно если перегрузка создаваемая движением соизмерима или больше величины силы тяжести), что даёт потенциально неправильную оценку высоты, а искажения магнитного поля дают неправильную оценку курса. Во многих реализациях фильтров авторы делают предположение, что ускорение движения и магнитные искажения присутствуют только короткий период времени. Поэтому величина коэффициента усиления фильтра β может быть выбрана достаточно низкой, чтобы отклонение вызванное ошибочными представлениями о гравитационных и магнитных искажениях наблюдаемых на местах, снизилось до приемлемого уровня за период. Минимально допустимое значение β ограничивается погрешностью измерений гироскопа.



Во многих приложениях может быть полезным использовать динамический прирост значений β и ζ. Это приведёт к снижению влияния акселерометра или магнитометра на оценку текущего положения во время проблемных периодов, например, при обнаружении большой перегрузки. Использование больших значений усиления фильтра во время инициализации может также улучшить сходимость фильтра в начальных условиях. Например, было установлено, что увеличение β и ζ на 10 позволяет за 5 секунд с момента включения фильтра компенсировать ошибку смещения гироскопа 1000 град / с в каждой оси.



Структура фильтра для установки решетки датчиков ИНС с магнитометром аналогична предложенной Бахманом и другими [34]. Обе реализации фильтра оценивают погрешность измерения гироскопа как градиент поверхности ошибок, созданной измерениями акселерометра и магнитометра. Фильтр Бахмана вычисляет это, используя подход Гаусса-Ньютона, который требует численного дифференцирования и инверсии матрицы. Фильтр, предлагаемый в настоящем докладе, использует аналитический вывод Якоби и работает на нормализованном градиенте поверхности ошибок. В результате, фильтр, предложенный в этой статье, предусматривает существенное сокращение вычислительной нагрузки и позволяет фильтру оптимально усиливать источник, основываясь на наблюдаемых характеристиках системы.



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



7. Выводы


По сути повтор текста из введения.



Приложение А. Реализация фильтра на С без магнитометра


Следующий исходный код является реализацией фильтра ориентации без магнитометра. С.-код был оптимизирован чтобы минимизировать необходимое количество арифметических операций за счет оперативной памяти. На каждое обновление фильтра требуется 109 скалярных арифметических операций (18 операций сложения, 20 вычитания, 57 умножений, 11 делений и 3 получения квадратного корня).Реализация требует 40 байт оперативной памяти для глобальных переменных и 100 байт оперативной памяти для локальных переменных во время каждого вызова функции обновления фильтра.



//Math library required for 'sqrt'

#include <math.h>

//System constants

#define deltat 0.001f // sampling period in seconds (shown as 1 ms)

#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)

#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta // Global system variables

float a_x, a_y, a_z; // accelerometer measurements floatw_x, w_y, w_z; // gyroscope measurements in rad/s

float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f; // estimated orientation quaternion elements with initial conditions

void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z)

{

// Local system variablesfloat norm; // vector norm

float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements

float f_1, f_2, f_3; // objective function elements

float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements

float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error

//Axulirary variables to avoid reapeated calcualtionsfloat halfSEq_1 = 0.5f * SEq_1;

float halfSEq_2 = 0.5f * SEq_2;float halfSEq_3 = 0.5f * SEq_3;float halfSEq_4 = 0.5f * SEq_4;float twoSEq_1 = 2.0f * SEq_1;float twoSEq_2 = 2.0f * SEq_2;float twoSEq_3 = 2.0f * SEq_3;

//Normalise the accelerometer measurement

norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z); a_x /= norm;

a_y /= norm; a_z /= norm;

// Compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 ­ twoSEq_1 * SEq_3 ­ a_x; f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 ­ a_y;

f_3 = 1.0f ­ twoSEq_2 * SEq_2 ­ twoSEq_3 * SEq_3 ­ a_z; J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication J_12or23 = 2.0f * SEq_4;

J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication

J_14or21 = twoSEq_2;

J_32 = 2.0f * J_14or21; // negated in matrix multiplication

J_33 = 2.0f * J_11or24; // negated in matrix multiplication

//

SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 ­ J_32 * f_3; SEqHatDot_3 = J_12or23 * f_2 ­ J_33 * f_3 ­ J_13or22 * f_1; SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;

//Normalise the gradient

norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);

SEqHatDot_1 /= norm;

SEqHatDot_2 /= norm;

SEqHatDot_3 /= norm;

SEqHatDot_4 /= norm;

// Compute the quaternion derrivative measured by gyroscopes SEqDot_omega_1 = ­halfSEq_2 * w_x ­ halfSEq_3 * w_y ­ halfSEq_4 * w_z; SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z ­ halfSEq_4 * w_y; SEqDot_omega_3 = halfSEq_1 * w_y ­ halfSEq_2 * w_z + halfSEq_4 * w_x; SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y ­ halfSEq_3 * w_x;

//

//Normalise quaternion

norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); SEq_1 /= norm;

SEq_2 /= norm;

SEq_3 /= norm;

SEq_4 /= norm;

}




Приложение B . Реализация фильтра на С с магнитометром

Следующий исходный код является реализацией фильтра ориентации с магнитометром и системой компенсации дрейфа гироскопа. С.­код был оптимизирован чтобы минимизировать необходимое количество арифметических операций за счет оперативной памяти. На каждое обновление фильтра требуется 277 скалярных арифметических операций (51 операция сложения, 57 вычитания, 155 умножений, 14 делений и 5 получений квадратного корня).Реализация требует 72 байта оперативной памяти для глобальных переменных и 260 байт оперативной памяти для локальных переменных во время каждого вызова функции обновления фильтра.



//Math library required for 'sqrt'

#include <math.h>

//System constants

#define deltat 0.001f // sampling period in seconds (shown as 1 ms)

#define gyroMeasError 3.14159265358979 * (5.0f / 180.0f) // gyroscope measurement er ror in rad/s (shown as 5 deg/s)

#define gyroMeasDrift 3.14159265358979 * (0.2f / 180.0f) // gyroscope measurement er ror in rad/s/s (shown as 0.2f deg/s/s)

#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta

#define zeta sqrt(3.0f / 4.0f) * gyroMeasDrift // compute zeta // Global system variables

float a_x, a_y, a_z; // accelerometer measurements floatw_x, w_y, w_z; // gyroscope measurements in rad/s float m_x, m_y, m_z; // magnetometer measurements

float SEq_1 = 1, SEq_2 = 0, SEq_3 = 0, SEq_4 = 0; // estimated orientation quaternio n elements with initial conditions

float b_x = 1, b_z = 0; // reference direction of flux in earth framefloat w_bx = 0, w_by = 0, w_bz = 0; // estimate gyroscope biases error // Function to compute one filter iteration

void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, floata_z, float m_x, float m_y, float m_z)

{

// local system variablesfloat norm; // vector norm

float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quatern ion rate from gyroscopes elements

float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements

float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements

J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; // floatSEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction

of the gyroscope error

float w_err_x, w_err_y, w_err_z; // estimated direction of the gyroscope error (angular)

float h_x, h_y, h_z; // computed flux in the earth frame

//axulirary variables to avoid reapeated calcualtionsfloat halfSEq_1 = 0.5f * SEq_1;

float halfSEq_2 = 0.5f * SEq_2;float halfSEq_3 = 0.5f * SEq_3;float halfSEq_4 = 0.5f * SEq_4;float twoSEq_1 = 2.0f * SEq_1;float twoSEq_2 = 2.0f * SEq_2;float twoSEq_3 = 2.0f * SEq_3;float twoSEq_4 = 2.0f * SEq_4;float twob_x = 2.0f * b_x;float twob_z = 2.0f * b_z;

float twob_xSEq_1 = 2.0f * b_x * SEq_1;float twob_xSEq_2 = 2.0f * b_x * SEq_2;float twob_xSEq_3 = 2.0f * b_x * SEq_3;float twob_xSEq_4 = 2.0f * b_x * SEq_4;float twob_zSEq_1 = 2.0f * b_z * SEq_1;float twob_zSEq_2 = 2.0f * b_z * SEq_2;float twob_zSEq_3 = 2.0f * b_z * SEq_3;float twob_zSEq_4 = 2.0f * b_z * SEq_4;float SEq_1SEq_2;

float SEq_1SEq_3 = SEq_1 * SEq_3;float SEq_1SEq_4;

float SEq_2SEq_3;

float SEq_2SEq_4 = SEq_2 * SEq_4;float SEq_3SEq_4;

float twom_x = 2.0f * m_x;float twom_y = 2.0f * m_y;float twom_z = 2.0f * m_z;

//normalise the accelerometer measurement

norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z); a_x /= norm;

a_y /= norm; a_z /= norm;

// normalise the magnetometer measurement

norm = sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm;

m_y /= norm; m_z /= norm;

// compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 ­ twoSEq_1 * SEq_3 ­ a_x; f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 ­ a_y;

f_3 = 1.0f ­ twoSEq_2 * SEq_2 ­ twoSEq_3 * SEq_3 ­ a_z;



f_4 = twob_x * (0.5f ­ SEq_3 * SEq_3 ­ SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 ­ S Eq_1SEq_3) ­ m_x;

f_5 = twob_x * (SEq_2 * SEq_3 ­ SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) ­ m_y;

f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f ­ SEq_2 * SEq_2 ­ SE q_3 * SEq_3) ­ m_z;

J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication

J_12or23 = 2.0f * SEq_4;

J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication

J_14or21 = twoSEq_2;

J_32 = 2.0f * J_14or21; // negated in matrix multiplication

J_33 = 2.0f * J_11or24; // negated in matrix multiplication

J_41 = twob_zSEq_3; // negated in matrix multiplication

J_42 = twob_zSEq_4;

J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication J_44 = 2.0f * twob_xSEq_4 ­ twob_zSEq_2; // negated in matrix multiplication J_51 = twob_xSEq_4 ­ twob_zSEq_2; // negated in matrix multiplication

J_52 = twob_xSEq_3 + twob_zSEq_1;

J_53 = twob_xSEq_2 + twob_zSEq_4;

J_54 = twob_xSEq_1 ­ twob_zSEq_3; // negated in matrix multiplication

J_61 = twob_xSEq_3;

J_62 = twob_xSEq_4 ­ 2.0f * twob_zSEq_2;

J_63 = twob_xSEq_1 ­ 2.0f * twob_zSEq_3;

J_64 = twob_xSEq_2;

// compute the gradient (matrix multiplication)

SEqHatDot_1 = J_14or21 * f_2 ­ J_11or24 * f_1 ­ J_41 * f_4 ­ J_51 * f_5 + J_61 * f_6;

SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 ­ J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;

SEqHatDot_3 = J_12or23 * f_2 ­ J_33 * f_3 ­ J_13or22 * f_1 ­ J_43 * f_4 + J_53 * f_5 + J_63 * f_6;

SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 ­ J_44 * f_4 ­ J_54 * f_5 + J_64 * f_6;

// normalise the gradient to estimate direction of the gyroscope error

norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);

SEqHatDot_1 = SEqHatDot_1 / norm;

SEqHatDot_2 = SEqHatDot_2 / norm;

SEqHatDot_3 = SEqHatDot_3 / norm;

SEqHatDot_4 = SEqHatDot_4 / norm;



// compute angular estimated direction of the gyroscope error

w_err_x = twoSEq_1 * SEqHatDot_2 ­ twoSEq_2 * SEqHatDot_1 ­ twoSEq_3 * SEqHatDo t_4 + twoSEq_4 * SEqHatDot_3;

w_err_y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 ­ twoSEq_3 * SEqHatDo t_1 ­ twoSEq_4 * SEqHatDot_2;

w_err_z = twoSEq_1 * SEqHatDot_4 ­ twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDo t_2 ­ twoSEq_4 * SEqHatDot_1;

//compute and remove the gyroscope baisesw_bx += w_err_x * deltat * zeta;

w_by += w_err_y * deltat * zeta; w_bz += w_err_z * deltat * zeta; w_x ­= w_bx;

w_y ­= w_by; w_z ­= w_bz;

//compute the quaternion rate measured by gyroscopes

SEqDot_omega_1 = ­halfSEq_2 * w_x ­ halfSEq_3 * w_y ­ halfSEq_4 * w_z;

SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z ­ halfSEq_4 * w_y;

SEqDot_omega_3 = halfSEq_1 * w_y ­ halfSEq_2 * w_z + halfSEq_4 * w_x;

SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y ­ halfSEq_3 * w_x;

//

//normalise quaternion

norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); SEq_1 /= norm;

SEq_2 /= norm;

SEq_3 /= norm;

SEq_4 /= norm;

// compute flux in the earth frame

SEq_1SEq_2 = SEq_1 * SEq_2; // recompute axulirary variables

SEq_1SEq_3 = SEq_1 * SEq_3;

SEq_1SEq_4 = SEq_1 * SEq_4;

SEq_3SEq_4 = SEq_3 * SEq_4;

SEq_2SEq_3 = SEq_2 * SEq_3;

SEq_2SEq_4 = SEq_2 * SEq_4;

h_x = twom_x * (0.5f ­ SEq_3 * SEq_3 ­ SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 ­ S Eq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3);

h_y = twom_x * (SEq_2SEq_3 + SEq_1SEq_4) + twom_y * (0.5f ­ SEq_2 * SEq_2

­ SE q_4 * SEq_4) + twom_z * (SEq_3SEq_4 ­ SEq_1SEq_2);

h_z = twom_x * (SEq_2SEq_4 ­ SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0.5f ­ SEq_2 * SEq_2 ­ SEq_3 * SEq_3);

// normalise the flux vector to have only components in the x and z

b_x = sqrt((h_x * h_x) + (h_y * h_y));

b_z = h_z;

}






Часть 1 Фильтр Маджвика

Написать:
11:57
1752
Нет комментариев. Ваш будет первым!