Создание собственной системы стабилизации

oleg70
mahowik:

так в нем реально поддельный калман с ошибками, т.е. подогнали на результат, а по сути не рабочий

Давно были подозрения, что толком мало кто разбирается в этом (поэтому и объяснить не могут), а набуробить “супер пупер” кода всегда можно, а наглядных результатов практически нет…

mahowik
SergDoc:

Привет! Давно тебя не видно!!!

здаров!

oleg70:

Давно были подозрения, что толком мало кто разбирается в этом (поэтому и объяснить не могут), а набуробить “супер пупер” кода всегда можно, а наглядных результатов практически нет…

это даже не фильтр, а скорее вычислитель и суть фильтра как раз понятна была, когда разбирался… помню понравилось обьяснение с форума avrfreaks.net/…/tut-theory-what-kalman-filter?name… (4й пункт)…

oleg70
mahowik:

это даже не фильтр, а скорее вычислитель

Вот вот… Как я понял, это скорей некоторый “предсказатель поведения” работающий на основе анализа поведения на предыдущих итерациях (?), ну и как следствие - как бы “фильтрует” неправильные реакции… (во загнул… 😃)

SergDoc

Это не гадатель )))
Вот по данному примеру:
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-х местах за уши притянута к ответу 😦

oleg70
SergDoc:

Это не гадатель )))

Любую непонятную вещь можно объяснить понятно… Мне так и не понятно пока: суть метода в чём ? общая идея, так сказать… (?) а матрицы, шматрицы - это уже потом…

SergDoc

суть проста - мы из скорости и пути посчитанных на предыдущем шаге получаем прогнозируемые данные о нынешнем шаге, далее через регулятор корректируем их полученными нынешними (неточными) данными и подстраиваем регулятор для последующей интерации, на основе уже полученных данных… вся фишка тут в самонастраивающемся регуляторе…

oleg70
SergDoc:

мы из скорости и пути посчитанных на предыдущем шаге

Скорость и путь ЧЕГО мы считаем (или меряем) вообще ? “На руках” у нас только данные с: гиры,акселя,магнитометра,барометра ну и GPS - это все исх. данные, и что мы в результате хотим получить ?
Стабилизацию горизонта ? Или глобальное положение “тела” в пространстве ? Цель не ясна…

SergDoc

Я по данной задачке объяснял для простоты понимания, а вообще всё можно задействовать и почти по тем же формулам - добавить в матрицы свои входные данные добавить вектор и матрицу управления, добавить матрицу поворота из локальных координат в глобальные - вот в принципе и всё - одна проблема, пока всё линейно - будет работать даже та хрень по ссылке, в остальных случаях - крах полный…
вот ещё что, различают дискретный и постоянного времени фильтры - там тоже есть различия…

oleg70:

Стабилизацию горизонта ? Или глобальное положение “тела” в пространстве ? Цель не ясна…

чем стабилизировать? мы просто дальше подаём полученные значения в ПИД…

oleg70
SergDoc:

мы просто дальше подаём полученные значения в ПИД…

Т.е. целевая функция - стабильная платформа или тупо стабилизация… Ух тогда насколько более стабильным должен получиться коптер, по сравнению скажем с КУКом например…, учитывая мощь применяемого мат/аппарата…
ждем результатов, интересно…

SergDoc

посмотрим, я щас пытаюсь запустить всё это дело, посмотреть как оно, но где-то, что-то накосячил пока датчики не стартуют )))

okan_vitaliy

Всем привет. Прошу помощи у создателей систем стабилизации. У меня случился глюк в проекте. Проект обросший кучей связей и искать баг было трудно. Повырезал все что мог и это не помогло. Короче суть проблемы в следующем. Пользую леченный таблеткой кейл 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;
}
//-----------------------------------------------

oleg70
okan_vitaliy:

то результатом функции sprinf является 0.

Очень похоже на нехватку памяти (стэка, кучи и пр.), больше ничего не могу сказать…

okan_vitaliy
oleg70:

Очень похоже на нехватку памяти (стэка, кучи и пр.), больше ничего не могу сказать…

Стек и кучу увеличивал. Думал, что с опциями в проекте что то не так. Взял даже эксемпл для дискавери и вставил в него этот код. Никаких изменений. Я уже не знаю что и думать, то ли у меня баг, то ли в самом кейле. Хотя кейлы - солидная контора. Получается, что sprintf с float опцией можно только в основном цикле при таком раскладе использовать. Причем с целочисленным выводом sprintf работает нормально. Я уже за коптер свой начинаю переживать, может еще чего в компиляторе вылезет и прямой дорогой в китай полетим к праздникам.

rual
okan_vitaliy:

Прошу помощи у создателей систем стабилизации. У меня случился глюк в проекте.

Сомнительная инициализация глобальной переменной temp, лучше сделать так

char temp[30];

oleg70
okan_vitaliy:

может еще чего в компиляторе вылезет и прямой дорогой в китай полетим к праздникам.

У меня тоже до определенного момента “всё летало”, но с постепенным разрастанием кода, начались аналогичные баги, например структура от SD карты перестала заполняться (или затираться ?), тоже вот копаюсь…
А на Keil грешить - последнее дело, тут разбираться уже с программированием надо.

okan_vitaliy
oleg70:

У меня тоже до определенного момента “всё летало”, но с постепенным разрастанием кода, начались аналогичные баги, например структура от SD карты перестала заполняться (или затираться ?), тоже вот копаюсь…
А на Keil грешить - последнее дело, тут разбираться уже с программированием надо.

Я раньше баги в кодевижене находил, а кейл вроде солидный но на 100 процентов не уверен в его правильности. Просто спринтф при выводе в прерывании флоатом очень настораживает. Можешь сам глянуть .
stm32vldiscovery_package.zip
Короче пока баг в прикрепленном файле не найду - не успокоюсь. Нарушается концепция программирования, когда начинаешь костыли вставлять.

oleg70
okan_vitaliy:

Просто спринтф при выводе в прерывании флоатом очень настораживает

Тут ведь, надо понимать, что пишем на старом добром СИ который эволюционировал со времен создания, и не факт, что реализация sprintf была грамотно адаптирована, там одних определений/переопределений типов вагон, короче только дебаггер или дизассемблер…

okan_vitaliy:

пока баг в прикрепленном файле не найду - не успокоюсь

Постулат программиста: “каждая последняя ошибка в коде является предпоследней” 😃

GAV
okan_vitaliy:

Короче пока баг в прикрепленном файле не найду - не успокоюсь. Нарушается концепция программирования, когда начинаешь костыли вставлять.

спросил знакомого спеца - он отправил читать эррату.
>> 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);
}

okan_vitaliy
GAV:

спросил знакомого спеца - он отправил читать эррату.
>> 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 МАЯ. Счастья,Здоровья,Мира ВСЕМ!!!

oleg70
okan_vitaliy:

что спринт вообще не использует указатель вывода

Я, как в воду смотрел… 😃

okan_vitaliy
oleg70:

Я, как в воду смотрел… 😃

И не только в воду смотрел, но и… Эта сцуко еще и по указателю при определенных условиях пишет очень длинные последовательности, короче больше чем буфер. В итоге хардфаулт. Я вообще че то теперь очкую по полной программе. Причем хард может вылететь через полчаса. Облазил инет и нашел на забугорном форуме у чела было похожее дело, но дело ничем не решилось. Ему сказали что спринтф в флоате тяжелая и трудная функция итд и типа пишите Кейлам. Посему вопрос - как прикрутить к кейлу гцц. Хочу попробовать другой компилятор.