Omazz 14 фев 2023 в 05:35

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

Средний

5 мин

11K

Python*Программирование*Алгоритмы*Математика*Робототехника

Туториал

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

Disclaimer 2.0: репозиторий с реализацией.

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

Введение

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

А оно нам вообще надо?

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

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

$x_k - истинное\ значение\ на\ шаге\ k$

$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:

Представим, что система обновляется каждую минуту. Таким образом, через 2 часа работы системы мы получим значения коэффициентов фильтра:

$α = \frac{2(2 k − 1)}{k(k + 1)} = \frac{2(240− 1)}{120(120 + 1)}=0,0329...$ $β = \frac{6}{k(k + 1)}=\frac{6}{120(120 + 1)}=0,000413...$

Тогда измеренные значения будут учитываться в достаточно малой степени. Для интереса можно измерить значения коэффициентов через 4 или 8 часов работы -- там коэффициенты будут ещё меньше.

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

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

$\dot{x}_1=\frac{z_1-z_0}{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;$

Реализация

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

Итак, сам код:

import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
sns.set()


def ab_filter_step(z, prev_x, prev_V, dt, k):
    if k == 0:
        return z, 0

    if k == 1:
        return z, (z - prev_x) / dt

    a_coef = 2. * (2. * k - 1.) / (k * (k + 1.))
    b_coef = 6. / (k * (k + 1.))

    x_predicted = prev_x + (prev_V * dt)
    V_predicted = prev_V

    x = x_predicted + (a_coef * (z - x_predicted))
    V = V_predicted + (b_coef / dt * (z - x_predicted))

    return x, V


number_steps = 100
std_coord = 10
k_max = 30
x0 = 30.
V = 8.
dt = 0.5

z_arr = []
x_arr = []
V_arr = []
prev_x = 0
prev_V = 0

for i in range(number_steps):
    if i < k_max:
        k = i
    else:
        k = k_max

    curr_z = x0 + (V * i * dt) + np.random.normal(0, std_coord)
    curr_x, curr_V = ab_filter_step(curr_z, prev_x, prev_V, dt, k)
    z_arr.append(curr_z)
    x_arr.append(curr_x)
    V_arr.append(curr_V)
    prev_x = curr_x
    prev_V = curr_V

plt.figure()
plt.plot(z_arr, label='measurements')
plt.plot(x_arr, label='ab filter')
plt.xlabel('Время [с]')
plt.ylabel('Координата [м]')
plt.legend()

# Вычисление среднеквадратичного отклонения от истинного значения:
std_measurs = []
std_filter = []

sum_measurs = 0.
sum_filter = 0.

for i in range(number_steps):
    x_orig = x0 + (V * i * dt)
    sum_measurs += (z_arr[i] - x_orig) ** 2
    sum_filter += (x_arr[i] - x_orig) ** 2
    std_measurs.append(np.sqrt(sum_measurs / (i + 1.)))
    std_filter.append(np.sqrt(sum_filter / (i + 1.)))

plt.figure()
plt.plot(std_measurs, label='measurements')
plt.plot(std_filter, label='ab filter')
plt.xlabel('Время [с]')
plt.ylabel('Среднеквадратичное отклонение [м]')
plt.legend()

plt.show()

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

Исходя из графиков, можно говорить о корректности работы данного алгоритма фильтрации.

Теги:

Хабы: