Создание собственной системы стабилизации
проГПС : это нормально от ублокса такую запись получить?
$GPGGA,143451.20,0,0099.99,*62
В том смысле, что пока он с положением не определился, в координатах пропуски? Навис всегда последние известные ему координаты пхает…
Три раза сегодня из за этого контроллер перешивал, а то при пропадании спутников, он у меня пытается улететь в 0гр СШ и 0 гр ВД.
там была чёт очень смахивающая на Дринкер-пилот
Мне мой первый контроллер напомнило… по возможностям ))
это naze32 harakiri (потому что с мавлинком), кто не верит тот может посмотреть кодятник github.com/ARMAZILA/FlightCode/blob/…/imu.c
стыдно с такой платформой да ещё за такие бабки показываццо - моё ИМХО
Здравствуйте SergDoc. Извиняюсь за свой русский 😃.
Я долго читаю эту ветку и вижу, что Вы пишите про фильтр Калмана на одну ось у Naze32. Тогда мне становиться очень грустно, потому что такой “фильтр Калмана” есть у MultiWii (не Naze32, а MultiWii) уже давно, но там он называется low-pass filter (ФВЧ). Я надеюсь что Вы поймете почему так.
Взгляните на код, которий Вы сами выложили:
/*
* kalman.c
*
* Created on: 25.11.2012
* Author: rob
*/
#include "stdint.h"
#include "kalman_simple.h"
typedef struct
{
float q; // process noise covariance
float r; // measurement noise covariance
float x; // value
float p; // estimation error covariance
} kalman_state;
// accelerometer
kalman_state kax;
kalman_state kay;
kalman_state kaz;
// gyro
kalman_state kgx;
kalman_state kgy;
kalman_state kgz;
void kalman_init(kalman_state* state, float q, float r, float p, float intial_value)
{
state->q = q;
state->r = r;
state->p = p;
state->x = intial_value;
}
void kalman_update(kalman_state* state, float measurement)
{
// prediction update
// omit x = x
float k; // kalman gain
state->p = state->p + state->q;
// measurement update
k = state->p / (state->p + state->r);
state->x = state->x + k * (measurement - state->x);
state->p = (1 - k) * state->p;
}
void initKalmanAccel(float ax, float ay, float az)
{
// small jakub frame
#define Q 0.0625 // process noise covariance
#define R 1.0 // measurement noise covariance
#define P 0.22 // estimation error covariance
kalman_init(&kax, Q, R, P, ax);
kalman_init(&kay, Q, R, P, ay);
kalman_init(&kaz, Q, R, P, az);
#undef Q
#undef R
#undef P
}
void initKalmanGyro(float gx, float gy, float gz)
{
// small jakub frame
#define Q 1.0 // process noise covariance
#define R 0.0625 // measurement noise covariance
#define P 0.22 // estimation error covariance
kalman_init(&kgx, Q, R, P, gx);
kalman_init(&kgy, Q, R, P, gy);
kalman_init(&kgz, Q, R, P, gz);
#undef Q
#undef R
#undef P
}
inline int kalman_filter_step(kalman_state* state, int16_t val)
{
float measurement = (float) val;
kalman_update(state, measurement);
return (state->x + 0.5f);
}
void accKalmanFilterStep(int16_t accels[3])
{
static int _init = 0;
if (!_init)
{
_init = 1;
initKalmanAccel(accels[0], accels[1], accels[2]);
}
else
{
accels[0] = kalman_filter_step(&kax, accels[0]);
accels[1] = kalman_filter_step(&kay, accels[1]);
accels[2] = kalman_filter_step(&kaz, accels[2]);
}
}
void gyroKalmanFilterStep(int16_t gyros[3])
{
static int _init = 0;
if (!_init)
{
_init = 1;
initKalmanGyro(gyros[0], gyros[1], gyros[2]);
}
else
{
gyros[0] = kalman_filter_step(&kgx, gyros[0]);
gyros[1] = kalman_filter_step(&kgy, gyros[1]);
gyros[2] = kalman_filter_step(&kgz, gyros[2]);
}
}
Там нет ничего интересного кроме этой ф-ции (всего 5 строк):
void kalman_update(kalman_state* state, float measurement)
{
// prediction update
// omit x = x
float k; // kalman gain
state->p = state->p + state->q;
// measurement update
k = state->p / (state->p + state->r);
state->x = state->x + k * (measurement - state->x);
state->p = (1 - k) * state->p;
}
В этом коде state->p, state->q и state->r есть КОНСТАНТЫ, которые не зависят ни от времени ни от данных. Они зависят только одна от другой. Потому k (или kalman gain), тоже КОНСТАНТА. Она обретает свое стабильное значение (как и state->p) спустя пару десятков итераций и больше не меняется. Если не верите и не хотите считать сами, то выведите значение k и/или state->p на debug MultiWiiConf и увидите.
Потому что k есть КОНСТАНТА
state->x = state->x + k * (measurement - state->x);
есть обыкновенный low-pass filter как и в MultiWii. Вот Вам и “фильтр Калмана” на одну ось…
Я когда то решил поспорить с brm на эту тему, но увидил что с ним спорить бесполезно.
Так а зачем ему меняться дальше если оно устаканилось? фильтр определился с погрешностями датчика, подобрал для него константы (адаптировался) а дальше да low-pass filter но уже с подобраными коэффициентами. Вся магия Калмана как раз и заключается в самостоятельном подборе коэффициентов и дальнейшей подстановке их в фильтра… (ну это я так понял)
В этом коде 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
мог бы попробовать и подлетнуть на этом, но там нет трёхи и я так и не понял как у них микшер устроен 😦