Сигма точков филтър на Калман

Ако някой се нуждае от тествания код на филтъра без аромат на Калман (UKF), тогава предлагам да опита моята реализация. Проверих го на няколко задачи и днес го проверих на модела BLDC, за който не мога да дебъгвам EKF по никакъв начин. Филтърът се показва добре на двигателя, не мога да накарам EKF да работи така. Въпреки че не би трябвало да е много по-лошо. Единственият проблем е намирането на грешки. Накратко. Филтърът се нуждае от две функции. Първият (F) ще определи как ще се промени състоянието на системата до следващия цикъл, като се има предвид известното състояние на текущия цикъл и известните влияния, ако има такива. Вторият (H) определя стойностите на измерените изходи за даденото състояние. За един двигател, например, има шест елемента във вектора на състоянието: ток по правата ос, ток по квадратурната ос, косинус на ъгъла на положение на ротора, синус на ъгъла на положение на ротора, ъглова скорост на ротора, момент на натоварване. От тези величини и уравнения на двигателя не е трудно да се определи функцията F. Е, функцията H преобразува тока от движеща се координатна система във фиксирана през косинуса и синуса на ъгъла на ротора от вектора на състоянието, защото без да се знае позицията на ротора, е възможно да се измерва токът само във фиксирани оси.

Който го мързи да погледне целия код, ето заглавието.

Използва се така, между извикванията векторът на въртене се нормализира (синус и косинус на ъгъла). ukfUpdate изчислява нова матрица на усилване (K), която е необходима при извършване на корекция в ukfCorrect. Е, ukfForecast дава прогноза за очакването на вектора на състоянието и неговата ковариация (което всъщност вече се разглежда в ukfUpdate, така че е възможно да се нормализира векторът на състоянието след ukfForecast). Функциите F и H се извикват в ukfForecast.

Моделът на шума е адитивен. Матриците P, Q, R трябва да бъдат посочени както във всички подобни филтри. Ковариационните матрици се съхраняват като по-нискитриъгълна матрица и елементът може да бъде достъпен чрез макроса LOW.

Колкото по-големи са размерите и сложността на уравненията на системата, толкова повече време спестява такъв филтър. Тук не е необходимо аналитично изчисляване на частните производни на F и H. Аз самият го използвам, за да проверя „дали ще работи“ или „възможно ли е да се оцени от тези измервания и с каква точност“.

Кодът е написан отделно, не за BLDC контролера. И написан доста мръсен, и добавен към BLDC симулатора още по-мръсен.

Според BLDC контролера нищо добро не се е случило с опростен филтър. Връщам се към втория кръг с филтъра EKF, в предишното изпълнение бяха открити значителни недостатъци. UKF е перфектен в сравнение с онзи стар EKF, но няма да пасне като производителност. Необходимо е да се направи нова оптимизирана (и работеща) версия на EKF.