Как стать автором
Поиск
Написать публикацию
Обновить

Использование фильтра Калмана для определения производных измеряемой величины

Время на прочтение3 мин
Количество просмотров17K
Недавно занимался решением задачи передачи вектора состояния из имеющейся модели движения в специальное устройство формирования навигационного сигнала. При этом существовали следующие ограничения:
  • модель движения примерно периодически отправляет ранее рассчитанные координаты и скорость объекта с меткой времени в известном формате по UDP;
  • имитатор навигационного сигнала умеет устанавливать TCP-соединение и через него принимать вектор состояния, включающий кроме координат и скоростей еще ускорения и джерки — производные ускорения или третьи производные координат;
  • при скоростях до 10^4 м/с возмущающее ускорение не превышает 0.001 м/с2;
  • координаты можно считать независимыми;
  • в имитатор навигационного сигнала должен поступать прогноз вектора состояния на заданный момент в будущем.

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

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

В качестве инструмента решения поставленной узконаправленной задачи был выбран Python.

Поиск по запросу «kalman filtering python» показал, что существуют стандартные реализации filterpy и pykalman, а кроме того можно довольно просто реализовать калмановскую фильтрацию с помощью широко распространенного пакета numpy. Поскольку скрипт должен был работать на виндусовой машине не имеющей подключения к интернету, я решил остановиться на последнем варианте.

Опираясь на scipy cookbook и статью Википедии про калмановскую фильтрацию реализовал пробную программку, выполняющую требуемую функциональность с заранее заданным набором входных данных.

Исходники для третьей версии питона приведены ниже. Обозначения совпадают с приведенными в статье Википедии.

При отладке наступил на следующие грабли: задал нулевым начальное значение ковариационной матрицы оценки вектора состояния P при неверных равных нулю начальных значениях ускорения и джерка (они неизвестны, помните?). После исправления данной ошибки получил блестящее совпадение оригинальных и предсказанных данных.

Использовал стандартный питоновский профилировщик для оценки скорости работы алгоритма и в первую очередь — обращения матрицы ковариации вектора отклонения S.

Исходный код пробной программы:
import numpy as np
import pylab
import profile

# число итераций
n_iter = 5000

# размерность ВС
dim = 5
# размерность наблюдаемого ВС
dim_o = 2

# вычислить матрицу эволюции для заданного временного шага
def calcF( t ):
    res = np.identity( dim )
    _t = 1.0
    for i in range( 1, dim ):
        _t *= t / i
        for j in range( 0, dim-i ):
            res[ j ][ i+j ] = _t
    return res

# вычислить матрицу эволюции и матрицу управления
def calcFG( t ):
    F = np.identity( dim )
    G = np.zeros( ( dim, 1 ) )
    _t = 1.0
    for i in range( 0, dim ):
        for j in range( 0, dim-i ):
            F[ j ][ i+j ] = _t
        if i <= dim_o:
            G[ dim_o - i ] = _t
        _t *= t / ( i+1 )
    return F, G


# истинные значения координаты и производных
xtruth = np.zeros( ( dim, 1 ) )
xtruth[0][0] = 15.3
xtruth[1][0] = 8.7
xtruth[2][0] = -0.3
xtruth[3][0] = 0.3
xtruth[4][0] = -1.0

# константная матрица наблюдения
H = np.zeros( ( dim_o, dim ) )
for i in range( dim_o ):
    H[i][i] = 1.0
H_t = H.transpose()

# константная матрица шума наблюдения
R = 1e-10 * np.identity( dim_o )

# времена, на которые задан ВС
t = 0.1 * np.arange( n_iter ) + np.random.normal( 0.0, 0.02, size=( n_iter, ) )

# дисперсия управляющего воздействия
D = 13.3 * 0.05 / 7000 * 2 / 60.0

# ВС, определенный на начальный момент времени
x = np.zeros( ( dim, 1 ) )

# погрешности на каждом шаге
dx = np.zeros( ( dim_o, n_iter ) )

# начальное значение ковариационной матрицы
P = 10.0 * np.identity( dim )

# наблюдения ВС
z = np.zeros( ( n_iter, dim_o, 1 ) )
for i in range( 0, n_iter ):
    z[i] = H.dot( calcF( t[ i ] ) ).dot( xtruth )

# заранее вычислим матрицы F и D^2*G*G^T
F = np.zeros( ( n_iter, dim, dim ) )
GGt = np.zeros( ( n_iter, dim, dim ) )
for i in range( 1, n_iter ):
    dt = t[ i ] - t[ i-1 ]
    F[i], G = calcFG( dt )
    GGt[i] = D*D * G.dot( G.transpose() )

# основная функция
def calc():
    global t, x, P, D, z, H, R, dx
    for i in range( 1, n_iter ):
        xpred = F[i].dot( x )
        Ppred = F[i].dot( P ).dot( F[i].transpose() ) + GGt[i]
        
        y = z[i] - H.dot( xpred )
        S = H.dot( Ppred ).dot( H_t ) + R

        K = Ppred.dot( H_t ).dot( np.linalg.inv( S ) )

        x = xpred + K.dot( y )
        P = ( np.identity( dim ) - K.dot( H ) ).dot( Ppred )

        # относительные погрешности
        dx[0][i] = y[0][0] / x[0][0]
        dx[1][i] = y[1][0] / x[1][0]

profile.run( 'calc()' )

# выводим погрешности
pylab.figure()
pylab.plot( t, dx[0], label='x' )
pylab.plot( t, dx[1], label='v' )
pylab.legend()
pylab.show()

Резюме


Не бойтесь математики, порой она способна значительно упростить вашу жизнь!
Теги:
Хабы:
Всего голосов 16: ↑15 и ↓1+14
Комментарии7

Публикации

Ближайшие события