Создание собственной системы стабилизации
Блок схема - сильнА, ничего не скажешь… хотелось бы конечно версию для наипростейшего случая (тот же аксель и его шум по одной оси) о потом уже наворачивать “кубики”, если дойдет смысл… Или всё это необходимо даже по минимуму ? , тогда я умываю руки…
Олег, ты не понял, как одномерности так и многомерности в нём нет как таковой - есть нужные выводимые данные такие как углы или скорости, они рассчитываются прямо в алгоритме при этом в полном алго. есть ещё вектор управления (грубо вводится ещё и то куда ты решил повернуть) - первое действие - это приращение аргументов т.е. зная прошлое ускорение и время цикла представляем себе новую скорость и пройденный путь (рассчётное) потом получаем данные и сравниваем/смешиваем их с расчётными - смотрим кто где обшибнулся мы или датчики - из этого выводим новые коэф. влияния датчиков на алгоритм для следующей интерации, смешанные данные отдаём на пид, а так же представляем их старыми данными для нового рассчёта, естественно и новые коэф. влияния впендрюливаем и так по кругу…
Олег, ты не понял
Понимание пока только начинает приходить, постепенно… Такие вещи надо именно осознать на “атомарном” уровне, иначе никакого творчества не получится, а только копирование…
Хочу попробовать пока накропать в своей прошивке фильтрацию сырых данных акселя по выше-предоставленному примеру, там никакого управления, я так понял, нет , матриц тоже,… если ещё заменить иезуитские термины типа “апостериорный” и “априорный” (…бесит отрава…) на “предварительный” и “последующий”, то уже в голове начинает вырисовываться смысловая картина.
Короче надежда применить у в собственном коде магический фильтр забрезжила…, а уж насколько он магический покажут результаты…
неее ))) всё хуже тут идёт оперирование данными с различных устройста: для простоты (отступлю - я как-то заводил тему о данных из прошлого) 1 и основоположное имеем “старые” (по моей терминологии т.е. сняли мы угловые скорости с дус) данные с ДУС плюс старый угол (он нам вернулся из конца алгоритма в начало уже профильтрованный) - из них мы предполагаем(рассчитываем) новый угол и сверяем/смешиваем с новыми данными с акселя с определённой пропорцией (ни чего не напоминает? - да блин это же обычный комплиментарник) - всё мы полученные данные отправляем в пид и в зад, и вот тут начинается самое интересное (сама магия Калмана) - мы на основе стат. и фундаментальных данных расчитываем коэффициенты влияния датчиков на алгоритм для следующей интерации…
всё хуже тут идёт оперирование данными с различных устройста
Сергей, вариантов и схем применения сего фильтра наверно очень много, не спорю (в этом и интерес, наверно.)
Я ж пока вот тупо взял сырые данные с осей акселерометра при максимальной чувствительности (2G), при этом имеем в состоянии покоя сенсора шкалу 1024 ед./G и шум=~25-30 ед. Затем прогнал их через алгоритм, указанный в статье, и … о чудо ! шум буквально ветром сдуло… до сих пор не верю, но показания стоят как влитые, как будто это и не аксель вовсе.
Никакое усреднение не даёт такой результат, однозначно.
Как следствие, в алгоритме калибровке акселя оффсеты по осям будут теперь практически идеальные, и далее имеем возможность более точного вычисления линейных ускорений…
Короче, незнаю кому как, а для меня это вполне результат… Не уверен, но наверно и с Дусом можно провернуть тоже, не говоря уж про магнитометр.
А например после, и это можно накатить:
мы на основе стат. и фундаментальных данных расчитываем коэффициенты влияния датчиков на алгоритм для следующей интерации…
никто не запрещает…
Никакое усреднение не даёт такой результат, однозначно.
А как насчет задержек в этом случае? Усреднить в покое тоже можно так, что шум пропадет, только задержка полезного сигнала будет неприемлемой.
Интересно было бы сравнить с БИХ фильтром, там можно и несколько предыдущих значений учитывать при расчете нового. Хотя можно ли их вообще напрямую сравнивать?
А как насчет задержек в этом случае?
Задержки и точность, смотрю, как и везде, взаимосвязаны и управляются установкой “дисперсии”,
но как ни крути - результат на порядок лучше чем при усреднении.
Раньше пробовал аж в три прохода: сначала из массива данных вырезал все максимумы и минимумы (пики), а потом усреднял оставшееся, и всеравно не то…
Чтоб точнее оценить, надо мне телеметрию передавать пошустрей, а она у меня всего герц этак 10… (по радиоканалу)
Задержки и точность, смотрю, как и везде, взаимосвязаны и управляются установкой “дисперсии”,
но как ни крути - результат на порядок лучше чем при усреднении.
Раньше пробовал аж в три прохода: сначала из массива данных вырезал все максимумы и минимумы (пики), а потом усреднял оставшееся, и всеравно не то…
Чтоб точнее оценить, надо мне телеметрию передавать пошустрей, а она у меня всего герц этак 10… (по радиоканалу)
Рад что вам пригодилось. 😁
Боюсь, только, данная реализация фильтра Калмана, по сути работает без внутренней модели (Xn = Xn-1). А так как Q и R - конст, то P устанавливается на постоянном значении и дальше уже К - постоянно и по сути данный фильтр превращается в апериодический фильтр или по другому комплементарный фильтр.
Проще тогда воспользоваться уравнением:
формула расчета апериодической функции:
Y(n) = Y(n-1) + (X(n)-Y(n-1))*(t/Tф);
или в форме комплементарного фильтра:
Y(n) = Y(n-1)*(1-t/Тф)+(X(n))*(t/Tф); - , где К = t/Tф;
Y(n) - текущее выходное значение фильтра
Y(n-1) предыдущее
X(n) - вход фильтра
t - время цикла обработки
Тф - время фильтра характеристика силы фильтрации и создаваемого запаздывания время запаздывания будет равно 1/3 Тф
Если побробовать фильтр Калмана из примера и фильтры с этими формулами результат будет примерно одинаков. При одинаковой инертности и времени запаздывания примерно одинаковое качество фильтрации.
Причем апериодический фильтр можно даже немного улучшить используя звено третьего порядка, то есть, самый простой вариант просто применить друг за другом три аппериодиских фильтра выход первого на вход второго и т.д. при этом взять Тф/3 для каждого тогда качество фильтрации будет немного лучше при такой же инертности.
Но когда в фильтре Калмана реализована модель процесса то тут все может быть по другому.
Но когда в фильтре Калмана реализована модель процесса то тут все может быть по другому.
А какую модель можно применить например в случае фильтрации данных с акселерометра? В каком случае в принципе имеет смысл использовать фильтр Калмана?
(ни чего не напоминает? - да блин это же обычный комплиментарник)
и по сути данный фильтр превращается в апериодический фильтр или по другому комплементарный фильтр.
Мнения экспертов сошлись…, но я осмелюсь поспорить:
у комплементарного фильтра мы точно знаем - какое значение долна принять переменная через энное количество шагов, т.е. “целеуказание” уже известно и остается только к нему стремиться через коэфициенты… (актуально для “притягивания за уши” одного неточного датчика к более точному)
Здесь же (у калмана) наблюдается чуток другая природа, а именно: К-фильтра как бы “скользит” по зашумлённому сигналу в поисках “истины”, которая заранее не известна по определению…
по сути работает без внутренней модели
боюсь что модель присутствует неявно, а именно заложена в характере самого шума сигнала (а шум шуму - рознь), соответственно то, что на акселе это сработало, само по себе удача, и не факт что будет эффективно на другом железе…
Здесь же (у калмана) наблюдается чуток другая природа, а именно: К-фильтра как бы “скользит” по зашумлённому сигналу в поисках “истины”, которая заранее не известна по определению…
В данном примере дисперсии Q и R константы по этому реальный шум не имеет значение и P устремляется к заранее известному числу и К соответсвенно тоже.
Вот если сделать например R зависимой от реального шума, то тогда вероятно вы увидите определенные улучшения.
Например самый простой вариант:
e[k] = abs(y[k] - xest1[k]);
или
e[k] = abs(y[k] - xest1[k])+Rmin;
R[k] = e[k]*Cr;
где Cr - некий подстроечный коэффициент который при e[k] = ШумМакс даст R[k] примерно равным R константу
Идея в том что когда сигнал не меняется то P и К уменьшаются до некой минимальной величины, если же сигнал например акселерометра начинает меняться то P растет и К растет так чтобы больше учитывать показания прибора а не модели. Тем самым мы получим меньшее западздывание при изменениях сигнала. Фильтр будет подстраиваться под дисперсию ошибки.
И тут все будет зависить от того как считать дисперсию.
Можно поступить еще веселее. Попытаться учесть распределение шума по Гаусу. Самый простой вариант если величина шума меньше некой константы то 0 если больше то по формуле что выше.
Идея в том что
боюсь что модель присутствует неявно, а именно заложена в характере самого шума сигнала (а шум шуму - рознь), соответственно то, что на акселе это сработало, само по себе удача, и не факт что будет эффективно на другом железе…
Модель именно процесса здесь все же отсутсвует
X[k] = X[k-1] это постоянный сигнал
вот если ввести сюда некую формулу процесса
X[k] = X[k-1] + Vconst, то другое дело
Но это когда мы действительно знаем что процесс меняется именно так
А если нет. Например для акселерометра модели нет.
А вот для скорости рассчитываемой из показаний акселерометра есть, но уже нет измерителя скорости. 😁
А вот для скорости рассчитываемой из показаний акселерометра есть, но уже нет измерителя скорости.
тут измерителями скорости должны выступать те же датчики воздушной скорости и/или GPS, в данном случае не стоит считать что-то одно - это система и довольно большая из 14 - 16 состояний/показаний и решать её нужно в целом, а не по отдельности…
сейчас в том же ардукоптере, применено довольно интересное решение - например компас может вообще выводиться из алгоритма если его показания сильно разнятся с показаниями ДУС, при этом если датчик возвращается в “норму” - он возвращается в алгоритм…
но уже нет измерителя скорости.
Измеритель поставим, не проблема… ))
и P устремляется к заранее известному числу
А вот это не могу понять (пока)…:
“кому известному ?” самому фильтру ? - это ещё куда не шло, но тогда скорей - “рассчитанному”
Иначе говоря, если заранее знать “истинное значение” среди шума, тогда нафига вообще что то фильтровать…
(я именно о случае одного источника шума рассуждаю, а не о разных)
в данном случае не стоит считать что-то одно - это система и довольно большая
налицо два разных подхода к решению задачи: один для “одиночного” сигнала и второй для смешивания разных…, тут то наверно и непонимание…
(я именно о случае одного источника шума рассуждаю, а не о разных)
шумов много и разных, можно отфильтровать собственный шум датчика, а дальше - вибрации вызванные моторами, пропами, какими-то резонансными составляющими рамы, самой платы контроллера и их гармоник - куча неизвестных причём меняющихся во времени без всякой закономерности, и то что работает на столе или в относительно спокойном состоянии сводя коэффициенты фильтра в одну точку, на практике из белого шума сделает однобокий цветной и не дай бог ещё и в резонанс с чем-то, простой пример (не относящийся к данному примеру) был у меня контроллер на 330-м лсм-е и что, пока взлетает или носится как угорелый - всё нормально, как только в режиме висения примерно на 50% газа - ну ни в какую, а как оказалось при оборотах 4500 - 5000 - жудкий резонанс самого акселя с вибрациями от вмг, так вот сменить вмг на более оборотистые и в ховере эта штука бы уже полетела но зато на малых оборотах были бы проблемы - вот ещё одно условие (неизвестное) как с ним бороться - только собиранием имперических данных и по ним выстраивать алгоритм именно к конкретному датчику! почему я упёрся в мпу - я его знаю я могу с ним “сработаться”…
налицо два разных подхода к решению задачи: один для “одиночного” сигнала и второй для смешивания разных…, тут то наверно и непонимание…
Отсюда и был мой вопрос, который чаще всего пытаются обходить либо не затрагивать вообще в инструкциях по фильтру Калмана:
В каком случае в принципе имеет смысл использовать фильтр Калмана?
сейчас в том же ардукоптере, применено довольно интересное решение - например компас может вообще выводиться из алгоритма если его показания сильно разнятся с показаниями ДУС, при этом если датчик возвращается в “норму” - он возвращается в алгоритм…
Эти “трюки” работают только в определенных условиях. Пример: влетели в зону магнтной аномалии, компасс начал врать, его из коррекции ДУС исключили. Потом гирик подрейфовал немного в нужную сторону и сравнялся с компассом в своих показаниях. И вот автопилот уверен, что все отлично, а на самом деле оба датчика врут.
И вот тут принципиальный вопрос который мне непонятен: как в фильтре Калмана определить, насколько датчик врет в текущий момент? Те же вибрации - они могут как сбивать аксель, так и абсолютно не искажать картину, если он их успевает усреднять, когда нет потери данных из-за низкой частоты выборки и нет сатурации. Имхо, кто врет сильнее - аксель или гироскоп, сказать очень сложно, если вообще возможно.
Измеритель поставим, не проблема… ))
А вот это не могу понять (пока)…:
“кому известному ?” самому фильтру ? - это ещё куда не шло, но тогда скорей - “рассчитанному”
Иначе говоря, если заранее знать “истинное значение” среди шума, тогда нафига вообще что то фильтровать…
(я именно о случае одного источника шума рассуждаю, а не о разных)
Все верно говорите, имеется ввиду “рассчитываемому”
Если посмотрите график то увидите что при Q и R конст. то P и К остановятся на некотором значении и больше вообще не будут меняться. Если же вы это установившееся значение сразу вобьете в P(0) и К(0) то вообще ничего меняться не будет 😃
А вот если R - это реальное текущее значение дисперсии то тут ужу Р и К будут меняться и подстраиваться под условия, а это уже интереснее.
жудкий резонанс самого акселя с вибрациями
В сотый раз убеждаюсь - все проблемы ИНС от акселя…
на какие только извраты не приходится идти чтоб только скомпенсировать его корявые свойства… на оптику надо переходить… (так, к слову)
НУ наконец-то, есть что почитать на форуме 😃 Спасибо, коллеги!
Статьи весьма годные, особо та, что на фин.странице. В коем веке увидел описание сложных сущностей простым человеческим языком.
Насколько я понял, матрицы H и F константны, т.е. по сути они есть системы лин.ур-ний с фиксированными коэффициентами? Вся магия КФ заключена в динамическом управлении коэффициентами коррекции экстраполяции вектора состояния системы x через матрицу F (для простоты B и u упоминать не буду) с учетом привязки к измеренному параметру системы (если H=1, либо связному с ней через через матрицу H).
Я правильно понял? Если да, то вся “особость” системы закладывается разрабом в H и F, а КФ рулит G на основе P. Т.е. “отключает” те датчики, у которых высокая дисперсия в P, ожидание и измерение сильно разорваны.
Посему странно использовать КФ для фильтрации одномерного сигнала, ибо нет никаких “последствий” фильтрации, не с чем больше сравнить результат прогноза и нет закономерности эволюции состояния F, можно только измерить дисперсию сигнала R и сравнить с заданной дисперсией Q и получить коэффициент. Всё…
Эти “трюки” работают только в определенных условиях. Пример: влетели в зону магнтной аномалии, компасс начал врать, его из коррекции ДУС исключили. Потом гирик подрейфовал немного в нужную сторону и сравнялся с компассом в своих показаниях. И вот автопилот уверен, что все отлично, а на самом деле оба датчика врут.
Это не совсем так, если в модели F учитываются все параметры системы. С другой стороны если имеем систему с конечным набором датчиков, котрые врут “в одну сторону” и соответствуют модели F, то значит такая система ТОЛЬКО так и видит мир. По сути недоработка на этапе разработки, либо такого рода проблемы не принимались во внимание.
В сотый раз убеждаюсь - все проблемы ИНС от акселя…
Олег, к чему такие крайности? Очевидно, что все жизненные проблемы от самой жизни ))
Хотя вот есть пример безаксельного коптера
запустишь по горизонту — будет помнить этот горизонт и приводится к нему из любого положения. но да, он читерит маленько если использовать мышиный сенсор (с разрешением 8*8, вроде) и “поправляет” этот горизонт при опущенных стиках и висении. и это без акселей, гпс, барометров, компасов и пр.
к чему такие крайности?
До сих пор вопрос устранения линейных ускорений из “гравитационных” остается открытым… (для меня во всяком случае)).
GPS наверно поможет, но как то некрасиво это всё…, а главное ненадёжно…
Мысль пока только такая - тупо отключать этот аксель при кренах аппарата свыше определенных углов и лететь да ДУСе, но тогда придется серьёзно поработать над “уплыванием” из за вибраций в течении максимального периода времени…
Короче, буду лепить “карусельку с вибратором” (нет, не с тем что…😃) для экспериментов и пробовать/смотреть что из этой затеи получится.
Поднимем тему!
Завёлся спор о рейсерских прошивках - частотах датчиков и всякой лабуде, так вот я провёл пару экспериментов и, короче надо развеять пару мифов!
32кГц - хрень и чушь
- все датчики которые пользуются и не пользуются максимум механически способны выдать аналоговый сигнал на 29кГц!
- MPU6000 - максимум что может отдать из регистров - частота 1кГц (проверено на столе)
- MPU6500 и иже с ними 9150 9250 9255 - максимум работы с регистрами 8кГц ДУС 4кГц - аксель (проверено и облётано)
- ITG-3701 и ICM-20601 - тоже самое только ДУС 4000 гр/сек аксель - внимание! якобы 32g, но тут же написано - 10G шок и похоже звездец как и в выше описаных - как это понимать…
тестил betaflight на ф4-м если верить данным гуя загрузка проца (без фпу) 1 процент на 8 килогерцах петли, короче ничё он там не делает, смысл картинкимне не понятен, но отладку на F7-м я прикупил - посмотрим)))
с отключенными фильтрами MPU отдает на 2кГц
- все датчики которые пользуются и не пользуются максимум механически способны выдать аналоговый сигнал на 29кГц!
А вот это про что речь? Я отстал от жизни?