Сигма-точечный фильтр Калмана

Если кому-то нужен протестированный код Unscented Kalman Filter (UKF) то предлагаю попробовать мою реализацию. Проверял на нескольких задачах, и сегодня проверил на модели BLDC, для которого никак не могу отладить EKF. На двигателе фильтр показывает себя хорошо, EKF так работать заставить не могу. Хотя он должен быть не на много хуже. Проблема только в поиске ошибок.

Кратко. Фильтру нужно задать две функции. Первая (F) будет определять как измениться состояние системы к следующему такту, при известном состоянии на текущем такте и известных воздействиях, если такие имеются. Вторая (H) определяет значения измеряемых выходов для заданного состояния. Для двигателя например в векторе состояния шесть элементов: ток по прямой оси, ток по квадратурной оси, косинус угла положения ротора, синус угла положения ротора, угловая скорость ротора, момент нагрузки. Из этих величин и уравнений двигателя не сложно определить функцию F. Ну а функция H преобразует ток из подвижной системы координат в неподвижную через косинус и синус угла ротора из вектора состояния т.к. не зная положения ротора можно измерять ток только в неподвижных осях.

Кому лень смотреть весь код, вот заголовок.


typedef struct {

	/* Size.
	 */
	int		N;
	int		M;

	/* Filter State.
	 */
	double		*P;
	double		*X;

	/* Noise.
	 */
	double		*Q;
	double		*R;

	/* Equality.
	 */
	void		(* pF) (double *Y, const double *X, const double *U);
	void		(* pH) (double *Z, const double *X);
	
	/* Kalman Gain.
	 */
	double		*K;
	
	/* Temporal.
	 */
	double		*YL, *ZL;
	double		*Z, *PZZ, *PYZ;
	
	/* End of memory.
	 */
	void		*pEnd;
}
ukfT;

ukfT *ukfAlloc(void *pMem, int N, int M);
void ukfForecast(ukfT *Kf, const double *U);
void ukfUpdate(ukfT *Kf);
void ukfCorrect(ukfT *Kf, const double *Z);


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


	ukf = ukfAlloc(malloc(1048576), 6, 2);

	ukf->pF = &pmcF;
	ukf->pH = &pmcH;

	LOW(ukf->Q, 0, 0) = pm->kQ[0];
	LOW(ukf->Q, 1, 1) = pm->kQ[1];
	LOW(ukf->Q, 2, 2) = pm->kQ[2];
	LOW(ukf->Q, 3, 3) = pm->kQ[2];
	LOW(ukf->Q, 4, 4) = pm->kQ[3];
	LOW(ukf->Q, 5, 5) = pm->kQ[4];

	LOW(ukf->R, 0, 0) = pm->kR;
	LOW(ukf->R, 1, 1) = pm->kR;

...
// и для каждого измерения

	Z[0] = iX;
	Z[1] = iY;

	ukfCorrect(ukf, Z);

	L = (3.f - X[2] * X[2] - X[3] * X[3]) * .5f;
	X[2] *= L;
	X[3] *= L;
	
	ukfForecast(ukf, 0);

	L = (3.f - X[2] * X[2] - X[3] * X[3]) * .5f;
	X[2] *= L;
	X[3] *= L;

	ukfUpdate(ukf);
	
	pm->kX[0] = X[0];
	pm->kX[1] = X[1];
	pm->kX[2] = X[2];
	pm->kX[3] = X[3];
	pm->kX[4] = X[4];
	pm->kX[5] = X[5];


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

Чем больше размерность и сложность уравнений системы, тем больше бережет время такой фильтр. Здесь не нужно аналитически вычислять частные производные F и H. Сам использую для проверок «а будет ли это работать» или «можно ли по этим измерениям оценить то и с какой точностью».

Выбор сигма точек у меня сделан так как мне кажется правильным, а не как писано в многочисленных статьях. А именно, я не умножаю строки матрицы S (которая корень из P) на множитель содержащий N размерность системы, только для того, чтобы ковариация считалась по обычной формуле. Это не правильно.

Код UKF и BLDC
Статья по UKF
Статья по разложению Холецкого
Хабропост о UKF

Код был написан отдельно, не для BLDC контроллера. И написан довольно грязно, а добавлен в симулятор BLDC еще более грязно.

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

Errata
1. Были замечены проблемы с разложением Холецкого на матрицах с диагональными элементами близкими к нулю.
  • +15
  • 15 ноября 2014, 00:16
  • amaora

Комментарии (20)

RSS свернуть / развернуть
Ничего не понял, но это круто!
+3
А что такое «сигма-точечый фильтр калмана», UKF и EKF? И чем последние два различаются?
0
  • avatar
  • Vga
  • 15 ноября 2014, 09:13
Cигма-точечый это Unscented то есть UKF, ну не «без запаха» же его переводить. А Extended то есть EKF это расширенный или обобщенный, в разных источниках по разному.

В EKF вычисляются матрицы частных производных уравнений системы, обычно аналитически. В UKF задаются только сами уравнения, а необходимую информацию фильтр получает путем пробы этих уравнений в нескольких точках в окрестности имеющейся оценки. Хотя фильтр все еще остается линейным и так же вычисляет матрицу усиления K. Не нужно думать, что задача в UKF решается перебором, это лишь другой способ линеаризации. То есть фильтр не может отрабатывать несколько гипотез одновременно, на это есть «более другие» методы.
0
Ничего не понял, но это круто!
Пиши исчо)
0
Почитайте у Simo Särkkä или у Rudolph van der Merwe хорошо описан UKF. Можете еще в оригинале у Julier Uhlmann.
0
Пожалуйста, дайте ссылки, если можно.
+1
Ну я не подозревал, что могут быть трудности с гуглением :)
1. Simo Särkkä. Я читал его диссертацию, а эту книгу только по-диагонали. Думаю, что в книге будет даже больше полезной информации. Методы представлены начиная с описания байесовского подхода, как раз для тех, кто ничего не понял :) На Aalto (институт где работает Särkkä) выложен тулбокс в Матлаб с отличным описанием. Сегодня сервер сервер лежит, пробуйте в другое время погуглить.
Кстати, у него хорошо реализован сглаживающий фильтр (обратный проход по накопленным данным).
2. van der Merwe
3. Julier&Uhlmann. Впервые UKF был представлен этими авторами в 1995 году. Вроде это та самая статья, а если и нет, то содержание точно то же.
+2
Вот спасибо.
Дело не в трудности гугления, а… как бы это сказать… Если это не коммерческая информация, то почему бы ей не поделиться?
0
Да, пожалуйста.
0
Пиши исчо)
Орфографически правильнее надо: пеши исчо!

Аффтар из категории тех людей, коих люто ненавидит бывший студент кало — чела просто прет от кол-ва вумных терминов непонятных публике. Некий садизм над публикой и одновременно эксгибиционизм. И это все на pop-сайте для «дебилов»!
-2
У меня нет желания, возможности и способности писать pop-обяснение на сотню страниц. Мне показалось здесь бывают те кому этот код может быть полезен и в сыром виде. Пытался про метод наименьших квадратов, но так в черновиках и лежит.
+3
У меня нет желания, возможности и способности писать pop-обяснение на сотню страниц
Пытался про метод наименьших квадратов, но так в черновиках и лежит.
А жаль. Очень жаль. Это как раз то, чего не хватает)
Орфографически правильнее надо: пеши исчо!
Это компромисс, на котором договорились внутренние GN с падонком)
0
Ну, стоило (хотя бы из уважения к остальному плебсу) указать, к примеру, область применения сей хитрой фильтрации. Не растекаясь мыслию по древу, т.ск. :)
0
Спасибо.
0
по-простому, зачем нужен фильтр калмана для BLDC? чтобы сделать из него сервопривод без явного канала обратной связи?
0
Мне достаточно того, что это интересная задача. Ожидаемая польза может быть такой:

1. Большая эффективность, меньше вибрации и шума, т.к. векторное управление.
2. Хорошая работа в переходных режимах, это именно от точной модели и хорошего фильтра.
3. Возможность оценки сопротивления обмоток и как следствие температуры, но это не просто.
4. Возможность «горячего» старта без предварительных процедур по выравниванию ротора.
5. Возможность быстрого возобновления работы после ресета контроллера (пропало питание например), следствие п4.

Сервоприводы не делаю, пока в планах только воздушные винты в качестве нагрузки.
0
Большое спасибо за статью.
+2
Да не за что. Говорю же не умею хорошие статьи делать, меня хватает только на небольшое описание.
0
Не подскажите хорошую статью, в которой можно почитать о фильтре Калмана и расчёте коэффициентов фильтра по параметрам динамической системы.
0
Большое спасибо!
0
Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.