- модель движения примерно периодически отправляет ранее рассчитанные координаты и скорость объекта с меткой времени в известном формате по 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()
Резюме
Не бойтесь математики, порой она способна значительно упростить вашу жизнь!
Комментарии (7)
Stas911
17.08.2015 15:54И какие, собственно выводы? Ваша реализация быстрее/медленнее?
StarHunter
17.08.2015 19:43+1Моя реализация позволяет достичь требуемого результата, не подключать библиотеки типа matplotlib, а так же проводить пакетную обработку нескольких параметров с одними и теми же операторами эволюции и управления, вычисляя их единожды на каждый шаг, а не на каждый шаг и каждый параметр.
Возможно я недостаточно понятно написал, целью работы было не создать новую универсальную библиотеку, а разработать в той или иной мере оптимальное средство решения весьма конкретной задачи. Что касается скорости, на не очень быстром десктопе при обработке одного параметра удается сделать 50e3 итераций за 10 секунд, что меня устраивает. К сожалению отчет о скорости работы других реализаций фильтрации дать не могу — не хватило терпения удовлетворить их зависимости.
HomoLuden
18.08.2015 11:04def 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 for i in range( 0, n_iter ): z[i] = H.dot( calcF( t[ i ] ) ).dot( xtruth )
Складывается впечатление, что рассматривается задача нелинейного ФК (EKF). Матрица динамики вычисляется на каждом шагу интегрирования (как Якобиан — матрица частных производных от системы нелинейных диф. уравнений).
1. Приведите в статье для каких исходных уравнений решалась данная задача. В CalcF я наблюдаю только некую степенную функцию от t. Похоже на n-ую производную от t^m.
2. Судя по коду выше, рассматривается задача с постоянным вектором измерения без шумов и каким-то непонятным вектором времени (массив случайных величин).
3. Случайный вектор времени требует встраивания в calcF (вычисление Якобиана) некоего алгоритма дискретизации (наподобие Рунге-Кутта). Хотя я вообще не представляю насколько корректно будет работать нелинейный ФК с нерегулярной сеткой времени. Цифровой ФК строится по непрерывным диф. уравнениям с последующей дискретизацией матрицы динамики и управляющего воздействия (последнего, судя по коду, нет в изложенной в статье задаче).HomoLuden
18.08.2015 11:13+1Дополнение:
Не уверен, что в случае постоянного вектора измерения корректно говорить о предсказании с использованием ФК.
Тут рассматривается просто оценивание одного зафиксированного во времени измерения. Если предположить, что данное измерение не содержит аддитивной погрешности, то теряется смысл использования ФК, т.к. можно было бы просто подставить эти два значения (величина и ее первая производная) в исходную систему нелинейных уравнений.
Если же предположить что в измерении величины и ее первой производной есть какая-то погрешность случайная, то прогонка в 100 раз этого зашумленного измерения через ФК лишь притянет вектор состояния к зашумленному измерению.
В статье не описана методика определения точности оценивания (что с чем сравнивалось).
Корректной можно было бы считать такая постановка, когда значения из xtruth складывались с массивом шумов и значения xtruth + noize[t] подавались бы в ФК в качестве зашумленных измерений. Тогда по выходному вектору ФК и его вектору состояния можно было бы судить о точности настройки.
Задача предсказания с помощью ФК — это когда у вас вектор измерения от объекта меняется и Вы хотите прогонкой ФК несколько раз спрогнозировать в какое состояние объект перейдет через сколько-то шагов интегрирования. В случае постоянного воздействия прогноз становится тривиальной задачей.StarHunter
18.08.2015 19:06Уважаемый,
Обратите, пожалуйста, внимание на название статьи и на вступительную часть, ту что расположена перед исходным кодом программы. Прогнозирование величины производится при помощи разложения в степенной ряд, а фильтрация применяется для определения производных, т.е. коэффициентов этого разложения. Более того, в приведенной программе приведена только фильтрация, но не прогнозирование.
Далее постараюсь отвечать по порядку.
В пробной программе в качестве исходной величины — изменяющегося истинного вектора состояния — применялись векторы calcF(t[i]).dot(xtruth), определенные на моменты времени t, причем моменты времени случайны.
Здесь матрица, возвращаемая calcF(t[i]), обеспечивает расчет эволюции системы по полиномиальному закону, т.е. calcF(t[i]).dot(xtruth) имеет следующий вид:
[[ xtruth[0][0] + xtruth[1][0]*t[i] + xtruth[2][0]*t[i]*t[i]/2 + xtruth[3][0]*t[i]*t[i]*t[i]/6 + xtruth[4][0]*t[i]*t[i]*t[i]*t[i]/24 ], [ xtruth[1][0] + xtruth[2][0]*t[i] + xtruth[3][0]*t[i]*t[i]/2 + xtruth[4][0]*t[i]*t[i]*t[i]/6 ], [ xtruth[2][0] + xtruth[3][0]*t[i] + xtruth[4][0]*t[i]*t[i]/2 ], ... ]
В реальной ситуации закон изменения истинной величины будет другим, а именно квазигармоническим. Но поскольку период велик, я посчитал допустимым в качестве модели задавать степенной ряд и для генерации истинного состояния вектора в пробной программе ограничиться также степенным рядом.
Время покажет, верно ли такое приближение.
Шумов в задаче кроме неточного момента времени (записанного в t[i]) не будет, поскольку исходные данные — модель движения, — выдает незашумленные величины и снабжает их меткой времени.
Другое дело, что я не могу использовать эти данные непосредственно, поскольку мне нужно знать производные до 3й (модель выдает только 1ю) и на основе их спрогнозировать вектор состояния на некоторое небольшое время вперед (в пределах единиц секунд).
В связи с этим я не вижу необходимости искусственно зашумлять вектор состояния.
HomoLuden
Спасибо за статью!
* Из описания не понятно на сколько шагов вперёд удалось предсказать «с идеальным совпадеинем».
* Не понятно какие данные у Вас использовались в качестве «входного воздействия», а какие как фильтруемый вектор измерения. Возможно ответ можно найти анализом кода, но для удобства не мешает текстом описать хотя бы в кратце.
StarHunter
Спасибо за критику!
В первую очередь меня интересовала точность определения производных. Здесь за десяток итераций погрешность становилась 0,1%, за сотню итераций — порядка 10^-7. Вероятно можно достичь более быстрого уменьшения погрешности путем изменения начального значения ковариационной матрицы оценки вектора состояния P. Планирую пробовать различные начальные значения при работе с реальными данными.
В тестовой программе в качестве входных данных использовались точные значения величины и ее первой производной, рассчитанные из полиномиальной функции с известными коэффициентами. Т.е. входные данные о величине и первой производной были даны точно, а вторая и третья производные определялись при помощи фильтра. Они и были выходными данными, они и определялись с указанной выше точностью.