Недавно занимался решением задачи передачи вектора состояния из имеющейся модели движения в специальное устройство формирования навигационного сигнала. При этом существовали следующие ограничения:
- модель движения примерно периодически отправляет ранее рассчитанные координаты и скорость объекта с меткой времени в известном формате по 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()
Резюме
Не бойтесь математики, порой она способна значительно упростить вашу жизнь!
ссылка на оригинал статьи http://habrahabr.ru/post/264895/
Добавить комментарий