Создание собственной системы стабилизации
Прошу прощения за невежество, но вроде как в коде нет оценки правильности подбора коэффициента и его корректировки.
Совершенно верно.
Далее о другом примере:
float Q_angleX = 0.001;
float Q_gyroX = 0.003;
float R_angleX = 0.03;
float x_angle = 0;
float x_bias = 0;
float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
float dtX, yX, SX;
float KX_0, KX_1;
float kalmanCalculateX(float newAngle, float newRate,int looptime)
{
dtX = float(looptime)/1000;
x_angle += dtX * (newRate - x_bias);
PX_00 += - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
PX_01 += - dtX * PX_11;
PX_10 += - dtX * PX_11;
PX_11 += + Q_gyroX * dtX;
yX = newAngle - x_angle;
SX = PX_00 + R_angleX;
KX_0 = PX_00 / SX;
KX_1 = PX_10 / SX;
x_angle += KX_0 * yX;
x_bias += KX_1 * yX;
PX_00 -= KX_0 * PX_00;
PX_01 -= KX_0 * PX_01;
PX_10 -= KX_1 * PX_00;
PX_11 -= KX_1 * PX_01;
return x_angle;
}
Это почти тоже самое что было ранее (наверное еще один „гениальный Калмановский“ код brm?). Ладно 😃. Посмотрите где в коде меняются Q_angleX, Q_gyroX и R_angleX. Нигде! Значит это константы.
А теперь посмотрите где в коде PX_00, PX_01, PX_10, PX_11, SX, KX_0 и KX_1 зависит от x_angle или newRate или newAngle или x_bias т.е. от данных. Опять нигде! Значит и это будут константы (не совсем, так как они зависят от времени, см. дальше).
В этом коде добавлена зависимость ФВЧ от времени (если sampling rate непостоянный). Вот и все.
Я хочу сказать что матричные элементы PX, SX и KX зависит только от констант Q_angleX, Q_gyroX, R_angleX ну и от dtX. От данных они НЕ ЗАВИСЯТ! Здесь нет прогноза данных и нет ни какой физической или мат. модели процесса. И быт ни может, потому что нельзя предсказать ни гиро ни акселя из их же самих предыдущих данных.
SegDoc, eсли что не ясно, то посмотрите тот код, что был ранее. Он проще (только 5 строчек 😃 ).
Q_angleX, Q_gyroX
это погрешности датчиков… они и есть константы и будут зависеть только от датчиков
далее да согласен, и момент, угол с акселя также можно назвать константой, он ничем не корректируется и не проверяется…
это погрешности датчиков… они и есть константы и будут зависеть только от датчиков
далее да согласен, и момент, угол с акселя также можно назвать константой, он ничем не корректируется и не проверяется…
Тогда зачем надо делать такую кучю вычислений, когда можно одич раз вычислить КХ и применить простую формулу low-pass фильтра? 😃
Ответ я знаю 😉 - чтобы написать что это “фильтр Калмана”. Но это смешно 😆.
P. S.
Это не Вы смешной, это автор кода 😉.
это автор кода
А у меня в закромах ещё парочка таких примеров где-то валяется надо поискать 😃
Так вот сижу читаю кучу статей разных, а везде такое вот 😦
далее да согласен, и момент, угол с акселя также можно назвать константой, он ничем не корректируется и не проверяется…
У меня впечатление что Вы меня не понимаете. Угол с акселя x_angle и x_bias это не константы. KX_0 и KX_1- константы. Вот их не надо все время вычеслять. Ладно, завтра или после завтра я вам напишу простой пример. Тогда надеюсь что будет ясно 😃.
НМЕА именно в этом пакете координаты передаёт, и они там есть, когда приемнику хватает спутников чтоб местоположение своё определить, но если фикс пропадает в полях появляются пропуски
Да наверное будут пропуски, можно в пакете $GPGGA,143451.20,0,0099.99,*62 искать выделенный байт и смотреть позишнфикс или нет и игнорировать координаты если отсутствует, но как-то это муторно.
NMEA еще передает координаты в пакете
$GPRMC,083559.00,A,4717.11437,N,00833.91522,E,0.004,77.52,091202,A*57
$GPRMC,hhmmss,status,latitude,N,longitude,E,spd,cog,ddmmyy,mv,mvE,mode*cs<CR><LF>
там статус позишнфикс идет перед координатами, проще его найти в пакете, даже если при отсутствии спутников будут пропуски координат и просто игнорировать их.
Armazila там была чёт очень смахивающая на Дринкер-пилот STM32F103RE CPU (32bit ARM Cortex M3, 72MHz, 512K flash. LQFP 64 10x10mm packages) L3GD20 2000 degrees/second 3-axis digital gyro LSM303DLHC 3-axis digital accelerometer and magnetometer LPS331AP digital MEMS pressure sensor Андрей, авторские права бы обсудить, а?
Вздор. Дринкер-пелот на стм32f4, аксель и гира такие-же, баро мс5611 и гпс навиа 8080… Мало чего общего.
Более того, там если код посмотреть затасканный до дыр алгоритм иму студента магвика.
Угол с акселя x_angle
он ничем не корректируется!!! он берётся как есть с акселя, не важно правильный он или нет - вот я о чём…
я вам напишу простой пример.
Это было бы очень замечательно 😃
Более того, там если код посмотреть затасканный до дыр алгоритм иму студента магвика.
код Вия, в реинкорнации Таймкопа…
код Вия, в реинкорнации Таймкопа…
Код может и вия, но алгоритьм иму магвика.
Да какая разница, тоже самое, чем я занимался в позапрошлом году на 103-м датчики мучал на этом же кодятнике 😃
Кстати по EKF github.com/PX4/…/attitude_estimator_ekf_main.cpp
мог бы попробовать и подлетнуть на этом, но там нет трёхи и я так и не понял как у них микшер устроен 😦
Да какая разница, тоже самое, чем я занимался в позапрошлом году на 103-м датчики мучал на этом же кодятнике
Ну это примитивно как-то.
А че щас в проекте происходит? Какие-то околонаучные дебаты идут.
мог бы попробовать и подлетнуть на этом, но там нет трёхи и я так и не понял как у них микшер устроен
Он тоже не учитывает полноту налитого стакана ))) В том смысле, что не учтены абсолютные координаты и высота.
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]’ */
матрица маленькая, нет координат ГПС и баро и нет динамики по ГПС (курс, скорость) и баро. И кроме того, как мне кажется должна быть ещё моделька атмосферы, вектор сноса от внешних сил (ветра).
Копни дальше 😃 это только положение. Есть и удержание позиции github.com/PX4/Firmware/blob/…/KalmanNav.cpp
там столько параметров можно крутить через станцию - волосы дыбом встают и когда я её себе запускал - так и не добился толку - плавало всё мама не горюй, возможно из-за того что мы датчики разные пользуем, там же (PX4) тоже MPU не основной датчик…
Ну это примитивно как-то.
Когда сам для себя балуешся это как бы нормально, а вот когда на продажу и по загнутым ценникам - это уже не смешно 😦 ну конечно комманда разработчиков же работала - кушать хочецца. Так ух купили бы чё-нить дорогое (скинулись, они же команда) распилили и содрали, а так баловство…
А че щас в проекте происходит?
у меня каша в голове, пытаюсь математику понять 😃 и что самое обидное, раньше то получалось…
у меня каша в голове, пытаюсь математику понять и что самое обидное, раньше то получалось…
Не пугай общественность…Врятли можно доверять таким поделкам, если автор признаецца что сам не понимает что дерет. Лучше заявлять мол все понятно и ежу, ну или посмотрел, улучшил, и нет теперь равных моему мегакоду! Вот как надо.
Копни дальше это только положение. Есть и удержание позиции
Нет, Сергей, это не то… Нормальный фильтр должен объединять все вместе датчики вместе, из песни слова не выкинешь, если что то не учтено, значит система нормально будет работать ТОЛЬКО в особых условиях, типа: на месте не крутись, по кругу не летай, бочки не делай. Без получения внутри фильтра текущего положения по ГПС (или другого абсолютного датчика положения) невозможно правильно разнести линейные ускорения и ошибку отклонения искусственного горизонта. Интегралы угловой и линейный надо постоянно проверять и корректировать, иначе “утекут”…
Так вот и надо заняться нормальным фильтром, а значит не всё потеряно 😃 есть что изобретать 😃 а на данном этапе есть порт Арду (это всмысле на чём летать и с чем сравнивать)
а я пока не могу собрать полной картинки…
Не пугай общественность…
Ну нету супермосха, есть тенденция к развитию, а если кто-то понимает как всё есть на самом деле пусть покажет, а я говорю так, как есть на данный момент - каша из предположений… Построится мат. модель будет и алгоритм… А кричать на весь мир “Да я тут самый умный” - это просто по детски нелепо…
Без получения внутри фильтра текущего положения по ГПС (или другого абсолютного датчика положения) невозможно правильно разнести линейные ускорения и ошибку отклонения искусственного горизонта.
Ни в одном проекте этого нет:) везде эти понятия разделены, а также обычно и удержание высоты отделено…
Вопрос в другом, а как будет себя вести, предполагаемый пока, алгоритм без GPS?
Вернулся, крутилки. Было бы не плохо если не на самой плате иметь крутилки, то хотябы три адс вывести для подключения внешнего блока крутилок. Мало ли че нужно подстраивать - от пидов до приведения камеры к реальному горизонту, или частотой пвм на регули менять. Практика показала что крутилка незаменима для игр с альтхольдом для каждой конкретной конструкции коптера (один тяжелее другой легче и т.д)
то хотябы три адс вывести для
есть свободные выведены на штырьки…
3io 3adc - линейка выведена сбоку (ну +5 и земля в комплекте) как и на старой плате… подписаны A1 A2 A3 D1 D2 D3
Кстати там что подписано SPI1 на самом деле SPI3 - это глюк…
Практика показала что крутилка незаменима
а как же адаптивный алгоритм?
а как же адаптивный алгоритм?
Ага, если нужно камеру на подвесе например к реальному горизонту привести, то какой нафиг алгоритм? Например.
А при экспериментах в поле? Новые фичи отрабатывать.
Нужны они, крутилки.
А при экспериментах в поле? Новые фичи отрабатывать.
на аппе крутилки есть 😃 можно крутить не сажая…
Про 3DR Radio не стоит забывать. При тестировании без ноута все равно не обойтись думаю.