Делаю OSD, автопилот и инерциалку...
Сутью вашего алгоритма не поделитесь? Уверен, что многим интересно.
В принципе все просто! 😃
Мы знаем формулу расчета для матрицы поворота: D(a[n],b[n],c[n]). abc - это углы поворота которые мы получаем с гироскопа.
C[n] - это значение компаса в момент времени n, C’[n] - оценка компаса в момент времени n будет D(a,b,c)*C[n-1];
Но мы знаем, что a,b,c содержат ошибки e1,e2,e3.
В итоге, мы можем записать следующее уравнение:
C[n] = D(a[n]-e1,b[n]-e2,c[n]-e3)*C[n-1];
С[n],С[n-1],…С[n-бесконечность] - известно
abc[n],abc[n-1],…,abc[n-бесконечность] - известно
e1,e2,e3 - константы и их надо найти. 😃
Дальше просто составляем систему уравнений и решаем ее. 😃
В случае если C[n]=C[n-1] решения не нет, так как определитель равен 0 (это как раз тот случай, когда оси вертятся как зонтик вокруг вектора). Так-же не имеем решения когда самолет находится на месте и вектор C[n] - константа, и система имеет бесконечное множество решений. 😃
Но как только самолет начинает двигаться, т.е. С[n] != С[n-1] != С[n-2] , сразу получаем одно единственное решение в виде E[n]. 😃
Коррекцию проводим на разных интервалах времени с различной фильтрацией и т.д… 😃
В итоге имеем, что меняя положение относительно неподвижного вектора, можно определить постоянную ошибку поворота по всем осям. 😃
Т.е. если гироскоп откорректирован и самолет летит ровно, то мы не можем вычислить коррекцию, но она нам и не нужно. А вот как только самолет начинает немного изменять свое положение (даже от порыва ветра), так сразу мы можем эту коррекцию вычислить. 😃
По идее, нам достаточно даже просто компаса с ГПС, чтобы вычислить все три ошибки. 😉
Восторженные оценки алгиритма приветствуются! 😃
Интересное решение. Но все эти размышления верны, если мы считаем, что e1,e2,e3 остаются постоянными для моментов времён n, n-1, n-2. Верно? Только в этом случае мы можем составить три уравнения с 3-мя неизвестными (в нашем случае e1,e2,e3 - неизвестные).
e1,e2,e3 остаются постоянными для моментов времён n, n-1, n-2. Верно?
Верно!
Если Е и меняется, то медленнее чем скорость с которой мы измеряем значение компаса и гироскопов.
К тому-же нам все равно, меняется е или нет. Нам надо найти среднюю ошибку е за три иттерации. Т.е. скорость изменения ошибки должна быть в три раза меньше чем скорость изменения вектора. 😃
И вообще, нам все равно что там будет. Главное угадать знак ошибки. Или даже скажем так: главное чтобы знак ошибки выдавался чаще правильно чем не правильно. В этом случае коррекция будет работать. 😃
И еще.
Нам даже не надо брать три значения подряд. Нам просто надо три изменения значения. 😃
PS. Возражения не принимаются, так как алгоритм проверен и реально работает. 😃
На чем проверен? на самолете в полете?
Восторженные оценки алгиритма приветствуются!
Возражения не принимаются, так как алгоритм проверен и реально работает.
Выражаю свой восторг, от возражений не удержусь… 😃
На чем проверен? на самолете в полете?
Полет не обязателен… Можно и на столе убедиться, что при некоторых углах ориентации (вполне реальных для полета), гироскопы неизбежно поплывут. Одна надежда, что пилот или ветер будет все время менять курс.
Алгоритм конечно красивый и похоже правильный, но мне кажется тут изобретен велосипед. Неужели этот алгоритм никто еще не реализовывал?
Как бы то ни было, до готового устройства, способного достойно управлять самолетом, еще очень далеко.
Буду рад если ошибаюсь…
Алгоритм конечно красивый и похоже правильный, но мне кажется тут изобретен велосипед.
Это нормальная реакция. Когда рассказали как, все стало очивидно, и возникает вопрос, а почему раньше этого не сделали, это-же очивидно? 😃
Неужели этот алгоритм никто еще не реализовывал?
Именно этот, конкретно для коррекции гироскопов я нигде не видел. Все что я видел в интернете про коррекцию, относилось к загадочному фильтру Калмана, который якобы должен где-то работать. 😃
А вообще, подобным образом сделан алгоритм пеленгации. Точнее, алгоритм определения координат объекта с помощью одного пеленгатора (это то-же возможно). 😃
Одна надежда, что пилот или ветер будет все время менять курс.
Тут дело вот в чем. Нам нужна ориентация, чтобы управлять самолетом. При прямолинейном полете мы самолетом не рулим (теоритически). Но как только гироскоп начинает плыть, так мы сразу детектируем изменение положения и начинаем корректировать курс. Корректируем положение самолета, компас изменяется. Компас изменяется, гироскоп корректируется. Положение по откорректированному гироскопу снова изменяется. Мы снова приводим все в норму. И т.д.
Т.е. в случае автопилота, система будет сама себя корректировать независимо ни от чего. Тут просто отрицательная обратная связь. 😃
дело в том что каждый из сенсоров - по сути источников информации может давать ложные данные
например у гирика и акселя может быть ошибка изза дескретности измерения по времени в условиях вибрации,
то есть пошел микроудар - фильтрующий кондер подзарядился и ацп считал ошибку через микросекунду уровень восстановился но ацп будет следующее значение считывать скажем через 10 микросекунд (к примеру) к этому времени может так сложиться что будет второй удар и ошибка накопится, не будет учтено что между замерами были провалы, с акселем тоже самое.
3д Компас я полагаю был бы идеальным сенсором, который бы заменил гирик еслиб не одно “но” магнитные и электромагнитные поля, если воспринимать его данные как чистую монету, можно словить глюк в реальных полетах вблизи ЛЭП и местах залегания магнитных руд
ЖПС неплохой сенсор который дает достаточно точный 3d вектор курса на скорости, при большом количестве спутников.
собственно о чем я хотел сказать, что какие бы небыли сенсоры у них есть параметр точность, и он не статичен и было бы логично учитывать точность того или иного сенсора в зависимости от совокупности условий, в качестве весовых коэффициентов при получении вычисляемого положения модели.
к примеру если гпс на скорости 30км/ч при 11 спутниках говорит что курс север, а компас показывает восток, то вероятна трабла с локальными магнитными полями, если гирик не отклоняется а компас резко уходит в сторону - вероятно рядом точечный магнитный полюс. Инаоборот если спутников к примеру 3 - координаты могут сильно прыгать, а аксель гирик и компас дают показания которые согласуются - тут логичнее не поверить гпс.
зы. а вообще молодец автор, такую махину задумал в одиночку сделать…
Т.е. в случае автопилота, система будет сама себя корректировать независимо ни от чего.
При полете на север-юг (и наоборот) поплывет крен, действительно изменится курс, полетит хоть переменными галсами, но еще терпимо… При полете запад-восток (и наоборот) поплывет тангаж… курс может изменится, когда будет слишком поздно…
дело в том что каждый из сенсоров - по сути источников информации может давать ложные данные
Основная проблема коррекции по компасу- вектор почти все время находится очень близко к плоскости горизонтального полета (в отличии от акселя…). Поэтому ситуации когда он будет давать 0-вые проекции на две оси совсем ненадуманные…
я понимаю речь о 3-д компасе,
нулевая проекция чем плоха? значит компас как раз в горизонте… хотя это очень дилетанское мнение ибо практического опыта с работы с компасом пока нет, микрухи 3-д цифрового компаса тока вчера привезли, пока еще не успел опробовать
Основная проблема коррекции по компасу- вектор почти все время находится очень близко к плоскости горизонтального полета (в отличии от акселя…).
Да не проблема это.
Коррекция происходит во всем диапазоне углов, в любой плоскости. Главное чтобы значение углов компаса менялось хоть чуть-чуть.
Так не важно, какой вектор коррекции вы взяли. с GPS голым было бы то же самое ( считаем что вектор скорости параллелен продольной оси.) и с гравитацией также.
Конечно, коррекция работает ПО ВСЕМ ОСЯМ, если только проекции корректирующего вектора не оси не нулевые. Но вращение вектора вокруг оси измерить нечем.
Это НИКАК не преодолеть, нет информации. И компоненты вращения вокруг некорректируемой оси будут неизбежно “влезать” в описание вращения.
Для коррекции этого нужен второй вектор, например GPS. Проще объяснить у меня не получается.
Вы сами сказали - в вашем случае у вас ДВА вектора. (компас и GPS). Хорошо. на широте Москвы, Питера. или на северном полюсе - вообще хорошо. ( правда GPS плохо ловится там 😃)
На экваторе будет хуже, там компас почти параллельно земле.
Если добавить еще и гравитацию- будет еще точнее.
И не надейтесь, что датчики будут работать также идеально когда вы к ним приложите вибрацию 😃
Конечно, коррекция работает ПО ВСЕМ ОСЯМ, если только проекции корректирующего вектора не оси не нулевые. Но вращение вектора вокруг оси измерить нечем.
Это НИКАК не преодолеть, нет информации. И компоненты вращения вокруг некорректируемой оси будут неизбежно “влезать” в описание вращения.
Сергей, Вы просто не поняли принцип, поэтому и возражаете. Суть в том, что имея траекторию точки и траекторию оценки, можно определить постоянную ошибку оценки.
Еще раз, другими словами:
Когда самолет летить идеально прямо, коррекция не работает, так как в этом случае действительно у нас недостаточно данных (одной оси не хватает). Как только самолет начинает маневрировать, так сразу можно определить постоянную ошибку.
Не верите, вот уравнение:
C[n] = D(a[n]-e1,b[n]-e2,c[n]-e3)*C[n-1];
распишите его, и все станет ясно. 😃
Я совершенно ответственно заявляю, что второго вектора для оценки ошибки гироскопа не нужно (в формуле никакого второго вектора нет). Нам нужно значение компаса x,y,z (точка, причем нормированная, т.е амплитуда компаса нам по барабану).
ГПС мной упоминался как коррекция компаса, которая к гироскопу отношения не имеет. Дальше я еще говорил, что коррекция гироскопа возможна даже имея просто угол компаса от ГПС.
Широта и проекция тут вообще непричем. Алгоритм одинакого хорошо будет работать как на экваторе, так и на Северном полюсе.
Коррекция возможна как абсолютная (компенсировать абсолютный угол), так и относительная (компенсировать уплыв за промежутки времени). 😃
Гравитация для данного алгоритма не нужна. Она будет использована в равномерном и прямолинейном полете. А это значит что скорость, и курс - константы. Но чтобы этого добиться, самолет надо застабилизировать. А чтобы застабилизировать, надо откорректировать гироскопы и определить положение.
Для коррекции при маневрировании акселерометр использовать нельзя.
Ладно, пытаюсь по порядку разобраться в алгоритме… Непонимание с первой фразы:
"Суть в том, что имея траекторию точки (что имеется в виду? Истинная траектории полета ЛА? Откуда нам она известна?) траекторию оценки (тоже непонятно- что это за траектория?), можно определить постоянную ошибку оценки (оценки ошибки траектории? почему постоянную? как вообще соотнести траекторию полета с ориентацией ЛА?).
ЗЫ Пол сезона пытался сделать коррекцию пирогоризонта по скорости изменения курса GPS. Очевидная математика, все просто (почти) и однозначно… Отлично работает на бумаге и в симуляторе… Увы… неизбежное скольжение ЛА на вираже, ветер и др. сложно учитываемые факторы, сделали эту коррекцию бессмысленной. Пришлось признать, что от такой коррекции больше вреда, чем пользы… Из этого опыта, поверю в ваш алгоритм после его хотя бы пробного тестирования в реальном полете.
пытался сделать коррекцию пирогоризонта
Вот эта фраза- неясна: зачем делать коррекцию того, что и так никуда не уходит?
Работал с Ко Пилотом довольно много: при отсутствии осадков и низкой облачности- горизонт у него стоит (то есть- у самолета, летящего под его управлением), как вкопанный 😃
А по поводу “супер-пупер алгоритма”- согласен…
“Чепчики вверх бросать” будет можно только после проверки в полёте 😦
Когда самолет летить идеально прямо
так сразу можно определить постоянную ошибку.
Математики-народ романтичный, взять хотя бы две этих фразы…
По ним- два вопроса:
1-а как автопилот это узнаЁт? (В реале- только анализ быстрого ЖПС (курс-высота) может дать такую информацию).
2-“Ошибка” (дельта, то есть) определяется, как разница значений: истинного и требуемого…
А что такое “постоянная ошибка”, если самолёт меняет положение и скорость ежесекундно.
О каком “постоянстве” идёт речь?
Да простит меня Дмитрий за офф, отвечу не совсем по его теме…
зачем делать коррекцию того, что и так никуда не уходит?
Сколько смотрел видео с OSD индикацией пирогоризонта, - ровный горизонт, совпадающий с визуальным, редкое исключение… Неравномерный нагрев самих сенсоров, деталей ЛА, попадающих в их сектор чувствительности, изменение облачности во время полета итд, итп. Другое дело что эти отклонения не фатальны, тк. ошибки не накапливаются и система стабилизации с ними обычно без проблем справляется… Долго и настойчиво пытался приблизиться к совершенству… тем более после каждого полета при анализе видео/логов казалось- “вот почти хорошо, еще только тут постоянную времени увеличить… тут только коэффициент изменю… тут фильтр поставлю…”… На том увлекательном занятии и прошло пол сезона… А когда все выкинул, оказалось работает лучше без всех этих наворотов… 😃
ЗЫ Дмитрий, надеюсь вы понимаете, что самолет может лететь с неизменным курсом при некотором крене и наоборот? То что он может менять высоту при нулевом тангаже и наоборот? Речь о совершенно “правильном”, триммированном ЛА…
Неравномерный нагрев самих сенсоров
Ну, я то с Ко Пилотом довольно много полетал…
А то, что Вы описываете-отсутствует при правильной его установке 😃
А версия с вертикальной парой пиродатчиков-ёщё и самокалибруется в полёте.
Отвечу по алгоритму коррекции. Все остально пока вторично.
И так, имеем слудующее.
Имеется 3Д компас, который выдает вектор магнотного поля. Сам по себе вектор - константа (север). Значение компаса изменяется только при именении положения сомолета в пространстве. С[n] - значение компаса в различные моменты времени.
Есть гироскоп, который выдает скорости вращения по осям a[n],b[n],c[n], но выдает с ошибкой E (e1,e2,e3).
Задача: как имея значения компаса C[n] в различные моменты времени, значения гироскопа a,b,c[n] в различные моменты времени, найти ошибку E.
Другими словами. У нас есть значения гироскопа в моменты времени. Эти значения содержат ошибку, т.е. a[n] = ИдеальнаяСкоростьХ[n] + e1, b[n] = ИдеальнаяСкоростьY[n] + e2, c[n] = ИдеальнаяСкоростьZ[n] + e3.
Нам надо найти e1,e2 и e3.
Все.
Как из имеющихся данных найти ошибки я уже написал выше.
что имеется в виду? Истинная траектории полета ЛА? Откуда нам она известна?
Я говорю сейчас только о коррекции гироскопа. Все. Траекторию, высоту, ГПС и все остальное будем обсуждать потом. Даже положение самолета в пространтсве сейчас не будем обсуждать.
Обсуждаем только ошибку гироскопа. 😃
Изначально я сказал что смогу написать алгоритм коррекции значений гироскопа на основинии значений компаса. Мне возразили, что это сделать невозможно.
Я этот алгоритм написал. 😃
Дмитрий, надеюсь вы понимаете, что самолет может лететь с неизменным курсом при некотором крене и наоборот? То что он может менять высоту при нулевом тангаже и наоборот? Речь о совершенно “правильном”, триммированном ЛА…
Да. Я это понимаю. Поэтому в формулах, нигде не упоминается значение крена и все остальные параметры. Я работаю сейчас только с показаниями датчиков. Как летит самолет, это тема для следующих обсуждений. 😃
Сейчас у меня есть “идеальный” гироскоп, который никуда не плывет, и компас. Следубщим этапом будет определение положения самолета в пространстве.
Дмитрий, я понял идею коррекции так:
Вы сравниваете две оценки положения по “скоростям” - трехмерной производной положения, так сказать, по- гироскопам и по компасу, так? исходя из того, что вектор скорости в среднемпо компасу ошибки не имеет, вы вычитаете ошибку из гироскопов.
Правильно?
ы сравниваете две оценки положения по “скоростям” - трехмерной производной положения, так сказать, по- гироскопам и по компасу, так?
Не совсем.
Я использую значение компаса чтобы найти ошибку гироскопов. Скорость вращения по компасу я не вычисляю. Я вычисляю только “постоянную” ошибку гироскопов. Постоянную, в смысле постоянную на интервале наблюдения, в нашем случае - три отсчета (хотя вообще 6). 😃
Поэтому в формулах, нигде не упоминается значение крена и все остальные параметры. Я работаю сейчас только с показаниями датчиков. Как летит самолет, это тема для следующих обсуждений. Сейчас у меня есть “идеальный” гироскоп, который никуда не плывет, и компас. Следубщим этапом будет определение положения самолета в пространстве.
Значит это только калибровка гиры (смещение нуля по каждой оси),
а не “Артему Силкину - привет”. 😃
А у магнитометра смещение нуля отсутствует ? (от него зависят e1, …)