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

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

• модель движения примерно периодически отправляет ранее рассчитанные координаты и скорость объекта с меткой времени в известном формате по 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()

Резюме

Не бойтесь математики, порой она способна значительно упростить вашу жизнь!