Создание собственной системы стабилизации
Да примеры в тырнете скорее правильные, но большинство из них приведены для линейных процессов, чего к сожалению практически нет в жизни. А вот с нелинейными уже сложнее, где нужно проводить линеаризацию модели через частные производные, джакобины (jacobians) или как их там…
Из курса тогда так и не понял мат. сути как получается главный рег. коэф. т.е. kalman gain. Мне тоже нужно что бы на пальцах было понятно, либо в крайнем случае кубиками мозайки могут быть четкие ясные абстракции ))) А разобрался, раскажи всем 😉
P.s. раньше копался с этим примерчиком, так в нем реально поддельный калман с ошибками, т.е. подогнали на результат, а по сути не рабочий)))
www.cs.utexas.edu/~teammco/misc/kalman_filter/
Привет! Давно тебя не видно!!!
Самый прикол в том что оно вроде всё не так и сложно, но на пальцах рассказать как-то пока не могу )))
Короче на реальном железе попробую - тогда может что и расскажу)))
на данный момент просчитываю 3 координаты, 3 скорости, без матрицы поворота и без вектора и матрицы управления…
да и 5 листов в “аналоговом” виде как то в “цифру” переводить влом…
Я расписывал матрицы и смотрел, что в них получается и к чему это приводит считал их по одной тут matrixcalc.org
самая проблема - это ковариации (шум) самого фильтра - тут без бубна никак…
а вот регулятор берётся разный в зависимости, что собрался делать: прогнозировать, фильтровать или сглаживать…
так в нем реально поддельный калман с ошибками, т.е. подогнали на результат, а по сути не рабочий
Давно были подозрения, что толком мало кто разбирается в этом (поэтому и объяснить не могут), а набуробить “супер пупер” кода всегда можно, а наглядных результатов практически нет…
Привет! Давно тебя не видно!!!
здаров!
Давно были подозрения, что толком мало кто разбирается в этом (поэтому и объяснить не могут), а набуробить “супер пупер” кода всегда можно, а наглядных результатов практически нет…
это даже не фильтр, а скорее вычислитель и суть фильтра как раз понятна была, когда разбирался… помню понравилось обьяснение с форума avrfreaks.net/…/tut-theory-what-kalman-filter?name… (4й пункт)…
это даже не фильтр, а скорее вычислитель
Вот вот… Как я понял, это скорей некоторый “предсказатель поведения” работающий на основе анализа поведения на предыдущих итерациях (?), ну и как следствие - как бы “фильтрует” неправильные реакции… (во загнул… 😃)
Это не гадатель )))
Вот по данному примеру:
x = (A * x) + (B * c) - это тупо интегрирование скорости по времени - получаем путь (Координату)
P = (A * P * AT) + Q - рассчитываем ошибку интегрирования - понятно что при первой интерации будет равна Q - якобы ковариации самого фильтра, но мы прекрасно видим что матрицу Q задали и забыли…
дальше
S = (H * P * HT) + R - вспомогательная матрица - из неё потом найти надо обратную
K = P * HT * S-1 - рассчёт коэффициента усиления так вот тут и выше матрица H - Jacobian matrix, а нифига не нарисованная от руки…
дальше
y = m - (H * x) - вспомогательный вектор для уточнения выходного вектора из показаний датчика но тут не H*x, а отдельная функция h(x) (считается отдельно)
x = x + (K * y) - кстати опять интегрирование только с уточнёнными данными т.е, новый x - вектор на выходе
P = (I - (K * H)) * P - уточнение ошибки
т.е. получается задачка в 2-х местах за уши притянута к ответу 😦
Это не гадатель )))
Любую непонятную вещь можно объяснить понятно… Мне так и не понятно пока: суть метода в чём ? общая идея, так сказать… (?) а матрицы, шматрицы - это уже потом…
суть проста - мы из скорости и пути посчитанных на предыдущем шаге получаем прогнозируемые данные о нынешнем шаге, далее через регулятор корректируем их полученными нынешними (неточными) данными и подстраиваем регулятор для последующей интерации, на основе уже полученных данных… вся фишка тут в самонастраивающемся регуляторе…
мы из скорости и пути посчитанных на предыдущем шаге
Скорость и путь ЧЕГО мы считаем (или меряем) вообще ? “На руках” у нас только данные с: гиры,акселя,магнитометра,барометра ну и GPS - это все исх. данные, и что мы в результате хотим получить ?
Стабилизацию горизонта ? Или глобальное положение “тела” в пространстве ? Цель не ясна…
Я по данной задачке объяснял для простоты понимания, а вообще всё можно задействовать и почти по тем же формулам - добавить в матрицы свои входные данные добавить вектор и матрицу управления, добавить матрицу поворота из локальных координат в глобальные - вот в принципе и всё - одна проблема, пока всё линейно - будет работать даже та хрень по ссылке, в остальных случаях - крах полный…
вот ещё что, различают дискретный и постоянного времени фильтры - там тоже есть различия…
Стабилизацию горизонта ? Или глобальное положение “тела” в пространстве ? Цель не ясна…
чем стабилизировать? мы просто дальше подаём полученные значения в ПИД…
мы просто дальше подаём полученные значения в ПИД…
Т.е. целевая функция - стабильная платформа или тупо стабилизация… Ух тогда насколько более стабильным должен получиться коптер, по сравнению скажем с КУКом например…, учитывая мощь применяемого мат/аппарата…
ждем результатов, интересно…
посмотрим, я щас пытаюсь запустить всё это дело, посмотреть как оно, но где-то, что-то накосячил пока датчики не стартуют )))
Всем привет. Прошу помощи у создателей систем стабилизации. У меня случился глюк в проекте. Проект обросший кучей связей и искать баг было трудно. Повырезал все что мог и это не помогло. Короче суть проблемы в следующем. Пользую леченный таблеткой кейл 4.23 и выше. Использую форматированный вывод sprintf с для float. Когда производится вывод в прерывании, а в основном цикле или в в прерывании с более низким уровнем крутится функция delayMicroseconds(500);, то результатом функции sprinf является 0. Если заремарить delayMicroseconds, то все работает. Пробовал на 100 и 103 камушке и разных версиях компилятора - результат не меняется. Уровень оптимизации компилятора нулевой, оптимизация по скорости отключена. Пробовал запрещать прерывания при выполнении sprinf - результата нет. Уже 8 дней ищю. Подскажите плиз если есть мысли по решению этой проблемы. Текст майна прилагаю -
#include “stm32f10x.h”
#include “stdio.h”
char temp[]=“Просто текст от нефик делать”;
float a=2.780;
//-----------------------------------------------------
uint32_t micros(void)
{
static uint32_t i=0;
i++;
return i;
}
//-----------------------------------------------------
void delayMicroseconds(uint32_t us){
uint32_t now = micros();
while ((micros() - now) < us);
}
//-----------------------------------------------
int main(void)
{
__disable_fiq();
SysTick_Config(SystemCoreClock / 1000);
SysTick->VAL = 0;
__enable_fiq();
while (1)
{
delayMicroseconds(500);
}
}
//-----------------------------------------------
void SysTick_Handler(void)
{
sprintf(temp,“%05.1f”,a);
sprintf(temp,“%u”,SystemCoreClock);
a+=1.8753456;
}
//-----------------------------------------------
то результатом функции sprinf является 0.
Очень похоже на нехватку памяти (стэка, кучи и пр.), больше ничего не могу сказать…
Очень похоже на нехватку памяти (стэка, кучи и пр.), больше ничего не могу сказать…
Стек и кучу увеличивал. Думал, что с опциями в проекте что то не так. Взял даже эксемпл для дискавери и вставил в него этот код. Никаких изменений. Я уже не знаю что и думать, то ли у меня баг, то ли в самом кейле. Хотя кейлы - солидная контора. Получается, что sprintf с float опцией можно только в основном цикле при таком раскладе использовать. Причем с целочисленным выводом sprintf работает нормально. Я уже за коптер свой начинаю переживать, может еще чего в компиляторе вылезет и прямой дорогой в китай полетим к праздникам.
Прошу помощи у создателей систем стабилизации. У меня случился глюк в проекте.
Сомнительная инициализация глобальной переменной temp, лучше сделать так
char temp[30];
может еще чего в компиляторе вылезет и прямой дорогой в китай полетим к праздникам.
У меня тоже до определенного момента “всё летало”, но с постепенным разрастанием кода, начались аналогичные баги, например структура от SD карты перестала заполняться (или затираться ?), тоже вот копаюсь…
А на Keil грешить - последнее дело, тут разбираться уже с программированием надо.
У меня тоже до определенного момента “всё летало”, но с постепенным разрастанием кода, начались аналогичные баги, например структура от SD карты перестала заполняться (или затираться ?), тоже вот копаюсь…
А на Keil грешить - последнее дело, тут разбираться уже с программированием надо.
Я раньше баги в кодевижене находил, а кейл вроде солидный но на 100 процентов не уверен в его правильности. Просто спринтф при выводе в прерывании флоатом очень настораживает. Можешь сам глянуть .
stm32vldiscovery_package.zip
Короче пока баг в прикрепленном файле не найду - не успокоюсь. Нарушается концепция программирования, когда начинаешь костыли вставлять.
Просто спринтф при выводе в прерывании флоатом очень настораживает
Тут ведь, надо понимать, что пишем на старом добром СИ который эволюционировал со времен создания, и не факт, что реализация sprintf была грамотно адаптирована, там одних определений/переопределений типов вагон, короче только дебаггер или дизассемблер…
пока баг в прикрепленном файле не найду - не успокоюсь
Постулат программиста: “каждая последняя ошибка в коде является предпоследней” 😃
Короче пока баг в прикрепленном файле не найду - не успокоюсь. Нарушается концепция программирования, когда начинаешь костыли вставлять.
спросил знакомого спеца - он отправил читать эррату.
>> Cortex-M3 LDRD with base in list may result in incorrect base register
>> when interrupted or faulted
короче, где-то в теле sprintf затесалась эта самая LDRD.
лечится заменой компилятора или выравниванием стека на 16 перед вызовом sprintf
void SysTick_Handler(void)
{
uint32_t oldSP = __get_MSP();
__set_MSP(oldSP & 0xFFFFFFF0);
sprintf(temp,“%05.1f”,a);
sprintf(temp,“%u”,SystemCoreClock);
a+=1.8753456;
__set_MSP(oldSP);
}
спросил знакомого спеца - он отправил читать эррату.
>> Cortex-M3 LDRD with base in list may result in incorrect base register
>> when interrupted or faultedкороче, где-то в теле sprintf затесалась эта самая LDRD.
лечится заменой компилятора или выравниванием стека на 16 перед вызовом sprintf
void SysTick_Handler(void)
{
uint32_t oldSP = __get_MSP();
__set_MSP(oldSP & 0xFFFFFFF0);
sprintf(temp,“%05.1f”,a);
sprintf(temp,“%u”,SystemCoreClock);
a+=1.8753456;
__set_MSP(oldSP);
}
Спасибо. В этом наверно и кроется проблема. Вставил сохранение стека в начале моей функции и восстановления в конце моей функции - усэ працюе. А вот если сразу перед спринтом и сразу после него , то выдает черти что. Причем такое впечатление, что спринт вообще не использует указатель вывода. Буфер остается нетронутым. Короче я че то очкую такую конструкцию применять. Повысил уровень оптимизации - вроде без этого костыля работает, иногда правда нулевой результат вываливает. При высокой оптимизации по времени и коду проект виснет. Короче нужно отдохнуть и бухнуть, что бы разгрузить голову. Короче… ВСЕХ С 1 МАЯ. Счастья,Здоровья,Мира ВСЕМ!!!