Создание собственной системы стабилизации
И контроллер в BGA корпусе
вполне может и 4 слоя - незнаю, а контроллер не BGA, а обычный RGT6 - лежат два без дела, пока это всё вилами по воде и проект в MicroXplorer с набросками в орле 😃
Одного раза мало, тем более тут ещё надо учитывать снос от ветра.
Сто раз уже думал совсем отказаться от магнитометра (и проще и надежнее…) размышляя так:
“если надо куда-то лететь, то летим по GPS однозначно”, а висеть на месте и знать где север конечно хорошо но зачем ??, компас чтоль имитировать ? 😃
В конце концов всеж жадность заставила оставить этот LSM на плате… и понеслась всякая там “hard/soft iron”…, ды еще его надо калибровать чуть ли не перед каждым полетом… тьфу… (вопрос пока открыт для меня, поиграюсь и выкину…)
Усложним задачу 😃 летали мы летали над морем и тут ЖПС выключили, радио через раз ловится - ну далеко залетели и куда без компаса - по солнцу аль по звёздам? Так хоть есть шанс в зону уверенного приёма вернуться, а не сгинуть в пучине морской 😃
Мы тут какбы о магии Кальмана беседы вели, так вот, чем больше входных данных, тем вернее результат получится…
не ну конечно можно радиокомпас с направленной антенной(про возврат на базу) - но тоже как-то не то… этож надо на какой-то частоте вещать что-то, музыку например…
и тут ЖПС выключили
По моему если “выключат” GPS то надо подумать: “а не пора ли заканчивать полеты…?”
чем больше входных данных, тем вернее
Если данные наглухо противоречат реальности (ЛЭП, аномалии всякие,… ды просто i2c завис,…) то дешевле их не учитывать, чем напрягать бедного калмана их сводить… 😃
Тут еще “научил” свою плату разговаривать через DAC и надо бы нормальный динамик поставить, так компас не разрешает…, а через пьезо, голос “Анны” звучит как у пропитого Федора… 😃
По поводу уменьшения размеров, вычислительной мощи и прочего будущего. Сейчас тренд умных вещей. Вангую. Как вещи они мне не очень интересны, но стоит обратить внимание на “умные” часы. Есть несколько моделей даже китайские, по сути это смартфон в корпусе часов. Получается интересная плата размером ~40x40 c 2-4 ядерным арм процессором, памятью 512-1024Мб, gps, 3g, КАМЕРА, usb … выходов конечно нет. Моё имхо, год, два и мы увидим полетный контролер таких же размеров и функционала. Зачем оно надо, затем же зачем сейчас 4 ядра в смартфонах 😃 но тенденция однако.
То что контролеры должны умнеть я не сомневаюсь. Что бы не было таких ситуаций как зарулился врезался в землю, дерево, стену, провод. И появятся преимущества полет по точкам с огибанием рельефа местности…
то дешевле их не учитывать, чем напрягать бедного калмана их сводить…
[заглянул в будущее] Датчики с глюком автоматически выкидываются из уравнения [/заглянул в будущее]
динамик да - подальше от компаса, у меня на стойке лыж висит…
По моему если “выключат” GPS то надо подумать:
рвать когти домой - нефиг думать 😃
Есть несколько моделей даже китайские, по сути это смартфон в корпусе часов. Получается интересная плата размером ~40x40 c 2-4 ядерным арм процессором, памятью 512-1024Мб, gps, 3g, КАМЕРА, usb … выходов конечно нет. Моё имхо, год, два и мы увидим полетный контролер таких же размеров и функционала. Зачем оно надо, затем же зачем сейчас 4 ядра в смартфонах 😃 но тенденция однако.
Не нужен здесь такой многоядерник, на телефонах и планшетах это многозадачность и универсальность, а здесь специфика. Да и с серьезной обработкой видео и т.д. в реалтайме не справится ARM даже 4 ядра. Для обычного полетного контроллера как-то много, а для серьезного с большим функционалом маловато будет.
Можно поставить Zynq-7000 там и мозг и мозжечек, но дороговато выйдет)))
Дрон, ну че, был на ввц? Поведай публике вкратце че там интересного было?
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
Андрей, авторские права бы обсудить, а? 😃
проГПС : это нормально от ублокса такую запись получить?
$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 строчек 😃 ).