Альфа-бета фильтр Калмана: фильтр «Hello world!»

Disclaimer: данная статья подойдёт, на мой взгляд, прежде всего для новичков, которые ничего не понимают в фильтрации, а стандартный и адаптивный фильтры Калмана вызывают недоумение. Прошу отнестись к ней с этой оговоркой.

Введение

Очень часто получаемая нами информация может не соответствовать действительности, из-за чего её использование в чистом виде может быть некорректно. Такое несоответствие как правило вызвано теми или иными шумами, но чаще всего при моделировании за шум принимают АБГШ (аддитивный белый гауссовский шум), имеющий относительно стабильную амплитуду. Фильтрация позволяет решить эту проблему.

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

Именно поэтому на начальных порах можно пренебречь некоторыми моментами и упростить фильтр Калмана до уровня α-β фильтра. Если же у читателя нет ни времени, ни желания на своё продвижение в этой теме, то рекомендуется пропустить следующую главу и перейти сразу к α-β фильтру.

Фильтр Калмана

Про фильтр Калмана можно писать очень много и очень долго, но я попробую описать всё тезисно. При этом, главная цель этого раздела -- показать, что сходу залезть в эту тему не так-то уж и просто, а вот реализовать α-β фильтр куда легче.

Итак, приступим. Пусть динамическая система описывается следующими линейными уравнениями:

$z_k -вектор\ измерения\ системы\ на\ шаге\ k$

$H - матрица\ перехода\ измерения$

$v_k -вектор\ шума\ измерения\ на\ шаге\ k$

Тогда состояние объекта будем оценивать следующим образом:

$x_{k+1}=Fx_k+w_k,$

$x_k -вектор\ системы\ системы\ на\ шаге\ k$

$F - матрица\ перехода\ состояния$

$w_k -вектор\ шума\ процесса\ на\ шаге\ k$

При этом, шумы и имеют ковариационные матрицы и соответственно. При этом, истинные значения этих матриц нам неизвестны, то есть нам нужно будет их оценивать самостоятельно.

Если Вам знакомо слово "ковариационные", а Ваши математические познания хоть сколько-то в порядке, то этот фильтр вполне реально осилить, прочитав другие статьи на этом прекрасном сайте, либо же прочитав соответствующую литературу. Фильтр Калмана можно изучать очень-очень долго, по-скольку там тоже есть свои нюансы, а можно остановиться на чём-то достигнутом -- мы будем довольствоваться самым тривиальным, но тем не менее достаточно действенным.

α-β фильтр Калмана

Описание фильтра:

Фильтрация позволяет нам получить как само значение на данном шаге , так и скорость его изменения, а помимо этого -- экстраполяцию. Экстраполяция -- это предсказание. То есть, исходя из того, что мы уже имеем, мы можем предполагать что будет дальше. Всё это можно использовать по Вашему усмотрению.

Итак, для работы фильтра нам потребуется определить следующие коэффициенты:

$α = \frac{2(2 k − 1)}{k(k + 1)},\ β = \frac{6}{k(k + 1)}$

Они понадобятся нам для фильтрации значения и скорости его изменения. Важно понимать, что в роли этого значения может выступать что угодно, например: координата тела по x, по y или по z, дальность, угол, скорость, ускорение -- что угодно. Обратимся к формулам:

$x_k=x_{k|k-1}+α(z_k-x_{k|k-1}),\\ \dot{x}_k=\dot{x}_{k|k-1}+\frac{β}{T_0}(z_k-x_{k|k-1})$

$z_k- измеренное\ значение\ на\ шаге\ k$

$x_k - отфильтрованное\ значение\ на\ шаге\ k$

$x_{k|k-1} - прогнозируемое\ значение\ на\ шаг\ k\ с\ шага\ {k-1}$

$\dot{x}_k - отфильтрованное \ значение \ скорости\ на\ шаге\ k$

$\dot{x}_{k|k-1} - прогнозируемое\ значение\ скорости\ на\ шаг\ k\ с\ шага\ k-1$

$T_0 -временной\ интервал\ между\ измерениями$

$α, β - коэффициенты\ фильтра$

Экстраполяция, то есть прогноз, делается следующим образом:

$x_{k|k-1}=x_{k-1}+T_0\dot{x}_k,\\ \dot{x}_{k|k-1}=\dot{x}_{k-1}$

Оставить фильтр в таком виде было бы, на мой взгляд, ошибкой, по-скольку чем дольше мы фильтруем, тем больше и тем меньше коэффициенты, а значит мы всё меньше доверяем измерениям и всё больше своим прогнозам, что не всегда корректно. Решением этой проблемы является определение величины $k_{max}$ , при достижении которой мы перестаём пересчитывать коэффициенты α, β. Можно взять значение, например, равное 25 или 50.

Работа алгоритма фильтрации по шагам:

По-скольку на первых порах у нас недостаточно информации для работы фильтра, то на первых двух шагах происходит инициализация:

$k=1:x_1=z_1;\ \dot{x}_1=\frac{z_2-z_1}{T_0};\$
$x_{2|1}=x_1+T_0\dot{x}_1;\ \dot{x}_{2|1}=\dot{x}_1$

Далее всё происходит как должно:

Спустя единиц времени получаем новые измерения ;
Вычисляем коэффициенты α, β по соответствующим формулам;
Фильтруем измерения и получаем значения и $\dot{x}_k$ ;
Делаем прогноз на следующий шаг, то есть вычисляем $x_{k|k-1}$ и $\dot{x}_{k|k-1}$ ;

Заметим, что как только величина достигает значения $k_{max}$ мы пропускаем второй шаг и пользуемся уже вычисленными значениями α, β коэффициентов.

Например, при

Получаем
$α=\frac{2(4 − 1)}{2(2 + 1)}=1,\ β= \frac{6}{2(2 + 1)}=1;$
$x_2=x_{2|1}+α(z_2-x_{2|1}),\ \dot{x}_2=\dot{x}_{2|1}+\frac{β}{T_0}(z_2-x_{2|1});$
$x_{3|2}=x_2+T_0\dot{x}_2,\ \dot{x}_{3|2}=\dot{x}_3;$

Реализация

В данной статье достаточно подробно разобраны все шаги алгоритма, приведены все формулы, поэтому реализовать код должно не составить труда, но я всё же приведу фрагмент кода на C++.

Итак, код должен иметь примерно следующий вид:

k=0;
T_0 = 5.0;

// Получаем z_k
void makeStep(double measuredValue) {
  k++;
  // Первые два шага фильтра нужны для его инициализации
  if(k == 1) {
    filteredValue = measuredValue;
    return;
  } else if(k == 2) {
    filteredVelocity = (measuredValue - filteredValue) / T_0;
    filteredValue = measuredValue;
    
    extrapolatedValue = filteredValue + (filteredVelocity * T_0);
    extrapolatedVelocity = filteredVelocity;
    return;
  }
  
  // Вычисляем коэффициенты фильтра
  alpha = (2.0 * (2.0 * k - 1.0)) / (k * (k + 1.0));
  beta = 6.0 / (k * (k + 1));
  
  // Фильтруем пришедшее значение
  filteredValue = extrapolatedValue + (alpha * (measuredValue - extrapolatedValue));
  filteredVelocity = extrapolatedVelocity + (beta / T_0 * (measuredValue - extrapolatedValue));

  // Делаем экстраполяцию, то есть прогнозируем значение на следующий шаг
  extrapolatedValue = filteredValue + (filteredVelocity * T_0);
  extrapolatedVelocity = filteredVelocity;
}

// P.S.: Как вы получите доступ к filteredValue,
// filteredVelocity, extrapolatedValue, extrapolatedVelocity -- дело Ваше.

Источник: https://habr.com/ru/post/716750/

Вернуться к списку

Интересные статьи

Как сделать AMP версию всего сайта на 1С-Битрикс

Задача - сделать AMP версию всего сайта на 1С-Битрикс, чтобы содержание AMP страниц всегда соответствовало оригинальным и изменялось при изменении оригинальных.

Массовая загрузка, обработка и выгрузка изображений в Битрикс на Python3

В этой статье мы популярно объясняем на собственном опыте как организовать массовую выгрузку, обработку и загрузку фотографий товаров из Bitrix, используя Python и минимальное количество SQL. Для проч...

Описание сервиса маркетплейс 1С-Битрикс. Как начать разрабатывать приложения для marketplace?

Маркетплейс – это сервис от 1С-Битрикс, который позволяет разработчикам делиться своими решениями с широкой аудиторией, состоящей из клиентов и других разработчиков.

Быстрая авторизация в Битрикс, или как переключаться между пользователями без ввода пароля

Всем привет! Не так давно на работе в рамках тестирования нового бизнес-процесса мне понадобилась возможность авторизации под разными пользователями. Переход в соответствующий р...

Композитный сайт 1С-Битрикс

Согласно многочисленным исследованиям поведения пользователей на сайте, порядка 25% посетителей покидают ресурс, если страница грузится более 4 секунд.