Я смотрю на общее описание фильтра Калмана, на примеры реализации и я не понимаю, почему при экстраполяции /коррекции ковавариации ошибки (P) никак не учитывается текущее состояние системы, входы/выходы фильтра.
Мы экстраполируем вектор состояния, матрицу ковариации ошибки. Потом рассчитываем коэффициент усиления Кальмана, используя его, вносим коррекцию в вектор состояния и матрицу ковариации ошибки.
Но, по тому, что я вижу, предсказание ошибки никак не засвистит от входа/выхода фильтра.
В предсказании/корректировке учувствуют начальное значение ковариации ошибки, матрица перехода (F), ковариация шума процесса (Q), ковариация шума измерения (R), и т. д. Но ведь это все константы. Тоесть предсказание ошибки зависти только от этих констант и начальных условий?
Ну так вроде там 2mg на LSB, так что вроде 0.002*4000=8g
Нет, по даташиту на MPU-6050, чувствительность в данном режиме 4096 LSB/g. 1g = 4096 LSB.
Просто в LSM330DL заводская калибровка по всем осям была боле точной, а в MPU-6050 заводская калибровка по Z сильно ушла в минус. Хотя, это не критично, я все равно программно калибрую датчик, на заводскую калибровку можно забить.
За часа три гироскоп уплывает в зависимости от оси от 3-7 градусов.
На всякий случай уточню – мы говорим о интегрировании данных с гироскопа, без внесения коррекции? Для меня ваш результат выглядит недосягаемым, у меня ошибка в 3 градуса накапливается за ~8 минут (и это в идеальных условиях).
У меня, правда, больше диапазон (2000 deg/sec), но я пробовал уменьшать диапазон, результаты были лучше, но не настолько хороши как у Вас.
Завтра, для гарантии, повторно протестирую на 500 deg/sec.
Спасибо, значит я на верном пути. Но фильтр Калмана так и остается для меня магией. Я понимаю только отдельные моменты, общего понимания принципа работы данного фильтра у меня нет.
А какая ошибка угла у Вас получается в абсолютном значении? Например, плата лежит горизонтально, поворачиваем ее на 45 градусов а потом возвращаем в исходное положение. На сколько при этом уплывает значение угла (хочу сравнить со своими результатами)? И какие у вас настройки гироскопа (диапазон, частота опроса, используете ли внутренний фильтр)?
Грубо говоря, я предполагаю что коптрер имеет собственно линейное ускорение если
— Модуль вектора g отличается от 9.8
— Резко увеличивается дифференциал ошибки
— я отклоняю ручку крена/тангажа на пульте
— и т. д.
Ок, я могу записать это все как
Я попытаюсь сначала аналитически выделить те «переменные» внутреннего состояния, от которых потенциально зависит Aerror. Потом представить их в виде формулы (например, как сумма с некоторыми весовыми коэффициентами). А вот как подобрать потом значения этих весовых коэффициентов – вопрос. Либо мат. моделирование, либо тупо подбор по результатам экспериментов на коптере (но это плохой вариант, как я уже писал, тяжело субъективно оценить незначительные изменения в поведении коптера).
В скоординированном вираже акселерометр всегда показывает примерно то же, что и равномерном прямолинейном полете без крена и тангажа
ОК, но мы и вводим Aerror для того чтобы на таких виражах не вносилась ошибка. Мы можем «распознать», что мы находимся в вираже и вернуть Aerror близкое к 1, тогда коррекция (вернее, в данном случае, ошибка) в этот момент вноситься не будет. Как распознать «вираж» — у нас для этого есть множество внутренних параметров (текущее значение углов крена и тангажа, дифференциал ошибки, сигналы с пульта и т. д.). Мы можем учесть эти данные при расчете Aerror. Если совсем в лоб, то, например, возвращать Aerror = 1 если текущее значение крена и тангажа больше 10 градусов (это просто пример, на самом деле я планирую сделать функцию расчета Aerror намного более сложной и учитывать много параметров).
Уточню, я говорил о применении регулятора для внесения коррекции. Если говорить о ПИД для расчета управляющего воздействия на двигатели – то это классический ПИД, там нет акселерометров, вероятностей, и т. д. Есть толь отклонение от заданного угла (вычисленного с помощью БИНС), которое нужно скомпенсировать.
Возможно я ошибаюсь, но в таком случае при определенном сочетании параметров условия устойчивости могут нарушаться.
Да, просто так баловаться с коэффициентами опасно, но можно принудительно ограничить диапазон изменения коэффициента и т. д.
Попробую изложить свою идею подробнее. Давайте забудем о шумах акселерометра, собственных ускорениях, вибрации и т. д. Предположим (для начала) что акселерометр показывает «истинный вектор g», и углы, полученные с акселерометра соответствуют реальному положенную ЛА. И рассмотрим одномерный случай.
Предположим, что после интегрирования показаний гироскопа (в очередной итерации цикла), мы получили угол Ag (угол с гироскопа). Одновременно, мы имеем тот-же угол с акселерометра (Аа). В данном случае, как мы пока договорились, мы считаем, что угол с акселерометра Аа истинный, а в Ag присутствует ошибка (вызванная интегрированием).
Ок, вводи коррекцию через Пропорциональный регулятор
Рассогласование/ошибка (E) равна
E = Аа – Ag;
Соответственно, введение коррекции будет выглядеть как
Ag += Kp * E;
Kp – коэффициент нашего регулятора, он связывает значение ошибки и значение вводимой корректировки.
А вот, теперь, вспомним, что на акселерометр действует собственное ускорение, вибрация и т. д. Для нас это паразитические воздействия, они могут вместо внесения корректировки внести ошибку. Но мы эти паразитические воздействия (как-то, косвенно) можем оценить и представить как Aerror. Aerror — это наша оценка вероятности ошибки в показаниях акселерометра в данный момент, например, по шкале [0… 1].
Логично, что чем больше Aerror, тем с большей вероятностью мы вместо коррекции угла внесем ошибку. Но мы можем учесть это в формуле регулятора.
Ag += (Kp * (1 — Aerror ) ) * E;
Теперь коэффициент нашего пропорционального регулятора лежит в диапазоне [Kp… 0] и зависит от вероятности ошибки в показаниях акселерометра. Чем больше вероятность ошибки — тем меньше будет величина вводимой коррекции.
Мне кажется, что функционирование самих ПИДов, на которые потом поступают обсчитанные значения с датчиков, будет осуществлять гибкое регулирование.
Не совсем понял, что Вы в данном случае имеете ввиду (о каком ПИД идет речь): ПИД регулятор для расчета управляющего воздействия на двигатели или ПИД регулятор для внесения коррекции в расчетный угол?
Получается система с положительной обратной связью, а это (насколько я помню из теории регулирования) чревато автоколебаниями.
Возможно, я непонятно высказался. Я не планирую вводить положительную обратную связь.
Если проводить аналогию с пропорциональным регулятором, то я предлагаю классический регулятор с отрицательной обратной связью, единственное, коэффициент регулятора я предлагаю сделать динамическим.
В динамике же, при возврате в исходное положение, возникает существенное отклонение от исходного положения
Вы говорите о сырых данных с гироскопа (угловой скорости) или о интеграле (значении угла)?
Нет, от этой идеи я не отказался. Я отказался от жесткого условия (при таких-то условиях вносим поправку, при других не вносим вообще).
Я решил сделать коэффициент комплементарного фильтра не константой, а функцией от некоторого состояния системы (текущее значение модуля g, дифференциал ошибки и т. д.) Получается некое подобие адаптивного фильтра, чем более «идеальные» условия, тем «активнее» вноситься поправка, и наоборот.
Кстати, коллеги, рекомендую посмотреть следующую ветку форума на rcdesign. Автор предоставляет мало информации (и порой, его посты похожи на тролинг) но результаты впечатляют.
Если принять это предположение как верное, то остаётся скомпенсировать паразитные вибрационные составляющие ускорений по трём осям и получится весьма близкая к реальности модель.
Вы правы, но есть нюансы.
В частном случае (на стенде, где одни двигатель вращается на постоянной скорости) я запросто могу искусственно ввести коррекцию, и скомпенсировать постоянную составляющую вибрации.
Но, как я уже писал,
Частота вибрации зависит от оборотов. Плюс на коптере много моторов, частоты их вибраций могут быть близки и при наложении возникнут «биения». Плюс резонанс конструкции (рамы).
Непонимаю только зачем было говорить о увеличении длины если это геометрический (или статистический) эффект
Да, честно говоря, особой смысловой нагрузки этот график не несет.
Я его привел, потому, как сам изначально не учел, что модуль вектора увеличится от вибрации.
У меня была идея – чтобы уменьшить влияние собственного ускорения, вносить коррекцию только тогда, когда модуль вектора с акселерометра приблизительно равен 9.8 м/с^2. Если модуль сильно отличается от этого значения – значит, явно присутствует линейное ускорение, и внесение коррекции в таком случае, только увеличит ошибку.
В коде написал проверку – вносим поправку в расчетный угол только тогда, когда модуль вектора с акселерометра лежит в диапазоне {9.7… 9.9}. Протестировал на столе, потряс плату – вроде все ок. Поставил на каптер – а коррекция вноситься как-то странно, погрешность быстро накапливается. Долго тупил и думал, пока не вывел аналогичный график и не понял причину происходящего.
Уже думал надо будет выбросить те LSM330DLC, что успел купить :)
Мы с Вами где-то потеряли нить разговора и, видимо, я непонятно описал, что изображено на графиках.
Вернемся к изначальному комментарию с тремя графиками.
Первый график – моментальное значение модуля (длинны) вектора с акселерометра. При появлении вибрации длинна увеличивается, что логично.
Второй график – три оси (красный – X, зеленый – Y, синий – Z). После включения двигателя все жутко шумит.
Третий график – те-же три оси, с шумом, пропущенные через ФНЧ. Здесь, по сравнению с предыдущим графиком я увеличил масштаб (я указал это в комментарии), чтобы было лучше видно постоянную составляющую по X и Y.
Проекция Z на третьем графике равна 1g.
Я так понимаю, Вас сбила с толку разница в масштабе между вторым и третьим графиком?
Если уреднять проекции, то g останется g при симметричном шуме.
Я смотрю на общее описание фильтра Калмана, на примеры реализации и я не понимаю, почему при экстраполяции /коррекции ковавариации ошибки (P) никак не учитывается текущее состояние системы, входы/выходы фильтра.
Мы экстраполируем вектор состояния, матрицу ковариации ошибки. Потом рассчитываем коэффициент усиления Кальмана, используя его, вносим коррекцию в вектор состояния и матрицу ковариации ошибки.
Но, по тому, что я вижу, предсказание ошибки никак не засвистит от входа/выхода фильтра.
В предсказании/корректировке учувствуют начальное значение ковариации ошибки, матрица перехода (F), ковариация шума процесса (Q), ковариация шума измерения (R), и т. д. Но ведь это все константы. Тоесть предсказание ошибки зависти только от этих констант и начальных условий?
Нет, по даташиту на MPU-6050, чувствительность в данном режиме 4096 LSB/g. 1g = 4096 LSB.
Просто в LSM330DL заводская калибровка по всем осям была боле точной, а в MPU-6050 заводская калибровка по Z сильно ушла в минус. Хотя, это не критично, я все равно программно калибрую датчик, на заводскую калибровку можно забить.
Ну, с произвольным вращением понятно, там проявляется погрешность измерения, погрешность интегрирования и т. д.
В вот Ваши результаты «статического» теста впечатляют.
На всякий случай уточню – мы говорим о интегрировании данных с гироскопа, без внесения коррекции? Для меня ваш результат выглядит недосягаемым, у меня ошибка в 3 градуса накапливается за ~8 минут (и это в идеальных условиях).
У меня, правда, больше диапазон (2000 deg/sec), но я пробовал уменьшать диапазон, результаты были лучше, но не настолько хороши как у Вас.
Завтра, для гарантии, повторно протестирую на 500 deg/sec.
— Модуль вектора g отличается от 9.8
— Резко увеличивается дифференциал ошибки
— я отклоняю ручку крена/тангажа на пульте
— и т. д.
Ок, я могу записать это все как
Это просто пример, функция не обязательно должна быть линейной и т.д.
ОК, но мы и вводим Aerror для того чтобы на таких виражах не вносилась ошибка. Мы можем «распознать», что мы находимся в вираже и вернуть Aerror близкое к 1, тогда коррекция (вернее, в данном случае, ошибка) в этот момент вноситься не будет. Как распознать «вираж» — у нас для этого есть множество внутренних параметров (текущее значение углов крена и тангажа, дифференциал ошибки, сигналы с пульта и т. д.). Мы можем учесть эти данные при расчете Aerror. Если совсем в лоб, то, например, возвращать Aerror = 1 если текущее значение крена и тангажа больше 10 градусов (это просто пример, на самом деле я планирую сделать функцию расчета Aerror намного более сложной и учитывать много параметров).
И это всего лишь идея, рабочая гипотеза. Критика и комментарии приветствуется.
Да, просто так баловаться с коэффициентами опасно, но можно принудительно ограничить диапазон изменения коэффициента и т. д.
Попробую изложить свою идею подробнее. Давайте забудем о шумах акселерометра, собственных ускорениях, вибрации и т. д. Предположим (для начала) что акселерометр показывает «истинный вектор g», и углы, полученные с акселерометра соответствуют реальному положенную ЛА. И рассмотрим одномерный случай.
Предположим, что после интегрирования показаний гироскопа (в очередной итерации цикла), мы получили угол Ag (угол с гироскопа). Одновременно, мы имеем тот-же угол с акселерометра (Аа). В данном случае, как мы пока договорились, мы считаем, что угол с акселерометра Аа истинный, а в Ag присутствует ошибка (вызванная интегрированием).
Ок, вводи коррекцию через Пропорциональный регулятор
Рассогласование/ошибка (E) равна
E = Аа – Ag;
Соответственно, введение коррекции будет выглядеть как
Ag += Kp * E;
Kp – коэффициент нашего регулятора, он связывает значение ошибки и значение вводимой корректировки.
А вот, теперь, вспомним, что на акселерометр действует собственное ускорение, вибрация и т. д. Для нас это паразитические воздействия, они могут вместо внесения корректировки внести ошибку. Но мы эти паразитические воздействия (как-то, косвенно) можем оценить и представить как Aerror. Aerror — это наша оценка вероятности ошибки в показаниях акселерометра в данный момент, например, по шкале [0… 1].
Логично, что чем больше Aerror, тем с большей вероятностью мы вместо коррекции угла внесем ошибку. Но мы можем учесть это в формуле регулятора.
Ag += (Kp * (1 — Aerror ) ) * E;
Теперь коэффициент нашего пропорционального регулятора лежит в диапазоне [Kp… 0] и зависит от вероятности ошибки в показаниях акселерометра. Чем больше вероятность ошибки — тем меньше будет величина вводимой коррекции.
Не совсем понял, что Вы в данном случае имеете ввиду (о каком ПИД идет речь): ПИД регулятор для расчета управляющего воздействия на двигатели или ПИД регулятор для внесения коррекции в расчетный угол?
Возможно, я непонятно высказался. Я не планирую вводить положительную обратную связь.
Если проводить аналогию с пропорциональным регулятором, то я предлагаю классический регулятор с отрицательной обратной связью, единственное, коэффициент регулятора я предлагаю сделать динамическим.
Вы говорите о сырых данных с гироскопа (угловой скорости) или о интеграле (значении угла)?
Я решил сделать коэффициент комплементарного фильтра не константой, а функцией от некоторого состояния системы (текущее значение модуля g, дифференциал ошибки и т. д.) Получается некое подобие адаптивного фильтра, чем более «идеальные» условия, тем «активнее» вноситься поправка, и наоборот.
Сейчас занимаюсь поиском этой самой функции.
При диапазоне измерений +/- 8g
Ну, своей (программной) калибровкой я, конечно, это все исправляю. Просто странно что «заводская» калибровка так сильно отличается по разным осям.
Вы правы, но есть нюансы.
В частном случае (на стенде, где одни двигатель вращается на постоянной скорости) я запросто могу искусственно ввести коррекцию, и скомпенсировать постоянную составляющую вибрации.
Но, как я уже писал,
на реальном коптере все сложнее.
Да, честно говоря, особой смысловой нагрузки этот график не несет.
Я его привел, потому, как сам изначально не учел, что модуль вектора увеличится от вибрации.
У меня была идея – чтобы уменьшить влияние собственного ускорения, вносить коррекцию только тогда, когда модуль вектора с акселерометра приблизительно равен 9.8 м/с^2. Если модуль сильно отличается от этого значения – значит, явно присутствует линейное ускорение, и внесение коррекции в таком случае, только увеличит ошибку.
В коде написал проверку – вносим поправку в расчетный угол только тогда, когда модуль вектора с акселерометра лежит в диапазоне {9.7… 9.9}. Протестировал на столе, потряс плату – вроде все ок. Поставил на каптер – а коррекция вноситься как-то странно, погрешность быстро накапливается. Долго тупил и думал, пока не вывел аналогичный график и не понял причину происходящего.
Нет, выбрасывать не стоит. :)
Я и не усредняю.
Мы с Вами где-то потеряли нить разговора и, видимо, я непонятно описал, что изображено на графиках.
Вернемся к изначальному комментарию с тремя графиками.
Первый график – моментальное значение модуля (длинны) вектора с акселерометра. При появлении вибрации длинна увеличивается, что логично.
Второй график – три оси (красный – X, зеленый – Y, синий – Z). После включения двигателя все жутко шумит.
Третий график – те-же три оси, с шумом, пропущенные через ФНЧ. Здесь, по сравнению с предыдущим графиком я увеличил масштаб (я указал это в комментарии), чтобы было лучше видно постоянную составляющую по X и Y.
Проекция Z на третьем графике равна 1g.
Я так понимаю, Вас сбила с толку разница в масштабе между вторым и третьим графиком?
Так оно и есть (3-тий график)