Создание собственной системы стабилизации
В этом коде state->p, state->q и state->r есть КОНСТАНТЫ, которые не зависят ни от времени ни от данных. Они зависят только одна от другой.
Всё верно, это обычный ФВЧ .
фильтр определился с погрешностями датчика, подобрал для него константы (адаптировался) а дальше да low-pass filter но уже с подобраными коэффициентами.
Неа, в том коде коэффициент от данных НИКАК не зависит, Сарунас верно подметил. Прошу прощения за невежество, но вроде как в коде нет оценки правильности подбора коэффициента и его корректировки.
Значит есть над чем работать 😃
рассмотрим другой пример:
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; // XXXXXXX arevoir
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;
}
Дрон, ну че, был на ввц? Поведай публике вкратце че там интересного было?
Не получилось поехать, семейные проблемы
проГПС : это нормально от ублокса такую запись получить?
Странно, не должен пропуски давать, может ублокс просто не передает координаты в этом пакете. Какой ублокс? А чем $GPRMC координаты не устраивают?
Странно, не должен пропуски давать, может ублокс просто не передает координаты в этом пакете.
НМЕА именно в этом пакете координаты передаёт, и они там есть, когда приемнику хватает спутников чтоб местоположение своё определить, но если фикс пропадает в полях появляются пропуски ((( ублокс нео-м-0-0001
Прошу прощения за невежество, но вроде как в коде нет оценки правильности подбора коэффициента и его корректировки.
посмотри что тут творится? rcopen.com/forum/f134/topic224458/3822 туда загоняется на сколько я понял угол с акселя (посчитаный) и угловая скорость с гиры… или опять развод?
короче просчитал вручную две интерации, получается:
x_bias(ошибка) выходит из нуля на третей интерации… а как работает надо запустить наверно и посмотреть - давно было 😦
Разбор:
должна быть фундаментальная матрица (данные натыканые пальцем в небо) - чего-то не хватает
float Q_angleX = 0.001;
float Q_gyroX = 0.003;
float R_angleX = 0.03;
матрица ковариаций
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;
/////////////////////////////////////////////////////
PX_00 -= KX_0 * PX_00;
PX_01 -= KX_0 * PX_01;
PX_10 -= KX_1 * PX_00;
PX_11 -= KX_1 * PX_01;
тут где-то запрятана матрица ошибок коэффициенты и собственно сам угол
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;
Ругайте 😃 что и как называется мог и перепутать 😃
не засунуть ли это в удержание высоты?
вместо угла по гире подсунуть скорость с акселя, а вместо угла по акселю скорость с баро?
Прошу прощения за невежество, но вроде как в коде нет оценки правильности подбора коэффициента и его корректировки.
Совершенно верно.
Далее о другом примере:
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 не основной датчик…
Ну это примитивно как-то.
Когда сам для себя балуешся это как бы нормально, а вот когда на продажу и по загнутым ценникам - это уже не смешно 😦 ну конечно комманда разработчиков же работала - кушать хочецца. Так ух купили бы чё-нить дорогое (скинулись, они же команда) распилили и содрали, а так баловство…
А че щас в проекте происходит?
у меня каша в голове, пытаюсь математику понять 😃 и что самое обидное, раньше то получалось…
у меня каша в голове, пытаюсь математику понять и что самое обидное, раньше то получалось…
Не пугай общественность…Врятли можно доверять таким поделкам, если автор признаецца что сам не понимает что дерет. Лучше заявлять мол все понятно и ежу, ну или посмотрел, улучшил, и нет теперь равных моему мегакоду! Вот как надо.