5D-фильтр Калмана не работает, и мы не уверены, где ошибаемся.

Прямо сейчас мы пытаемся создать 5-мерный фильтр Калмана, который получает на вход координаты x и y маленького жука, который бродит по коробке. Мы наблюдаем за этой ошибкой, когда она совершает примерно 2000 движений, а затем прогнозируем ее положение с этого момента. Размеры: x-координата, y-координата, скорость, курс и угловое ускорение. Ниже приведен код, который у нас есть до сих пор.

#x is a list of five variables - x, y, velocity, angularVelocity, angularAcceleration
#currentmeasurement is the x and y that were observed
def kalman_filter(x, P, currentmeasurement, lastMeasurement = None):
        prevmeasurement = []

    #if there is a lastMeasurement argument, it becomes measurement
    if lastMeasurement:
        prevmeasurement = [lastMeasurement[0], lastMeasurement[1], x.value[3][0]]

    #if there is no lastMeasurement argument, the current measurement becomes measurement.
    else:
        prevmeasurement = [x.value[0][0], x.value[1][0], x.value[3][0]]

    #Prediction Step
    a = x.value[3][0]
    F = matrix([
        [1., 0., cos(a), 0., 0.],
        [0., 1., sin(a), 0., 0.],
        [0., 0., 1., 0., 0.],
        [0., 0., 0., 1., dt],
        [0., 0., 0., 0., 1.]])

    x = (F * x) + u
    P = F * P * F.transpose()

    #we can calculate the heading from our observations.
    heading = atan2(currentmeasurement[0][1] - prevmeasurement[1], currentmeasurement[0][0] - prevmeasurement[0])
    while(abs(heading - prevmeasurement[2]) > pi):
        heading = heading + 2*pi*((prevmeasurement[2]-heading)/abs(prevmeasurement[2]-heading))

    #perhaps the velocity should also be calculated? 

    # measurement update
    dt = 1.
    u = matrix([[0.], [0.], [0.], [0.], [0.]])      # external motion
    H =  matrix([[1., 0., 0., 0., 0.],              #the measurement function
             [0., 1., 0., 0., 0.],
             [0., 0., 0., 1., 0.]])

    R =  matrix([[1., 0., .0],                      #measurement uncertainty
             [0., 1., .0],
             [0., 0., 1.]])
    I =  matrix([[]])                               #a 5x5 identity matrix
    I.identity(5)


    prevmeasurement = [currentmeasurement[0][0], currentmeasurement[0][1], heading]
    Z = matrix([prevmeasurement])
    y = Z.transpose() - (H * x)
    S = H * P * H.transpose() + R
    K = P * H.transpose() * S.inverse()
    x = x + (K * y)
    P = (I - (K * H)) * P

    return x,P

Это приводит к крайне неверным оценкам. Мы не уверены, что мы здесь делаем неправильно - я думаю, что мы правильно выполняем все шаги, но не уверен, что у нас есть все необходимые матрицы. Любой вклад будет полезен!


python artificial-intelligence kalman-filter
person meowsephine    schedule 27.04.2015    source источник
comment
Сократите набор данных до нескольких точек, где четыре параметра являются фиксированными значениями, а пятый следует четко определенной прогрессии, например. x = 1,2,3,4,5, где y=0, v=1, курс=+x-направление, угловой=0. Затем запустите алгоритм и посмотрите, что у вас получится.   -  person user3386109    schedule 28.04.2015
comment
В дополнение к структурным проблемам, на которые я указал в своем ответе, вам следует подумать, действительно ли ваша модель ошибок без трения, постоянной скорости и постоянной угловой скорости отражает реальность. Шаг обновления идеально подходит для чего-то вроде шайбы на воздушном хоккейном столе.   -  person Ben Jackson    schedule 28.04.2015

Ответы (3)


arrow_upward
1
arrow_downward

В верхнем комментарии неправильное описание. Ваше состояние должно быть x, y, velocity, angle, angularVelocity

Вам не хватает Q, ковариации процесса. Он должен отражать, насколько ваше состояние может измениться между обновлениями, и добавляется к вашему обновлению P на этапе прогнозирования.

Вы строите EKF (поскольку ваше обновление требует триггера, оно нелинейно). Вы построили матрицу F, которая выполняет обновление вашего состояния, но для обновления ковариации процесса вам нужен якобиан вашей функции обновления. В вашем случае это выглядит так:

J = matrix([
    [1., 0., cos(a), -sin(a), 0.],
    [0., 1., sin(a), cos(a), 0.],
    [0., 0., 1., 0., 0.],
    [0., 0., 0., 1., dt],
    [0., 0., 0., 0., 1.]])

Если единственным прямым измерением является положение, вам не следует вычислять курс и использовать его в качестве измерения. Пусть этим занимается КФ.

Вы должны установить R как реальную погрешность измерения. Шум, представленный Q, R и распространяемый в P, более важен, чем ваше состояние x, если вы хотите, чтобы KF работал.

person Ben Jackson    schedule 28.04.2015

arrow_upward
1
arrow_downward

Я работал над очень похожей проблемой и имел точно такие же проблемы. Учитывая зашумленные измерения (x,y) положения робота и знание того, что он вращается с постоянной скоростью по кругу постоянного радиуса, предсказать его следующее положение.

Я также попробовал 5D EKF, который взорвался после нескольких итераций. Мое состояние состояло из:

[x, y, distance_per_timestep, heading, heading_delta_per_timestep]

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

Мое окончательное решение, которое работает на удивление хорошо, состояло в том, чтобы использовать 2D-фильтр Калмана для позиций x и y со скоростью, курсом и скоростью поворота, рассчитанными по предыдущим трем позициям и используются в качестве управляющих входных данных для обновления оценки состояния. Я пробовал другие вариации на эту тему (состояние [x, y, vx, vy, ax, ay] и так далее, но ничто не сравнится с подходом KISS (будь проще, глупее).

Сначала я использовал необработанные шумовые измерения для расчета скорости, курса и дельта_курса. Я попытался использовать свои оценки состояния x и y после того, как фильтр проработал некоторое время, но это тоже привело к нестабильности — очень медленное начало, но плавно нарастающее переплетение назад. и вперед по наземной дорожке правды, которая в конечном итоге превратилась в произведение искусства спирографа! Позже я обнаружил, что если сначала обновить измерения, а затем использовать эти значения состояния (после измерения, но до обновления состояния), я получу более высокую производительность, чем необработанные измерения.

Я не уверен, что мой подход решит вашу проблему (я продолжаю получать шумные измерения, и мне нужно всего лишь предсказать несколько ходов вперед, чтобы «поймать» мой побег), но возьмите из этого то, что вы можете.

Мне также очень интересно узнать, почему мой EKF взорвался, а также как сформулировать правильную матрицу F в такой нелинейной задаче — «правильный» якобиан , если я правильно рассчитал, почти сразу взрывает EKF!

Мне было бы очень интересно услышать ваше решение или любые другие достижения, которые вы, возможно, сделали! Удачи!

person Max O'Lydian    schedule 13.05.2015

arrow_upward
0
arrow_downward

  1. Добавьте Q матрицу шума модели
  2. Your H matrix should be the Jacobian. Not your Q matrix.
    1. Don't estimate anything for the filter. Let the Kalman do it for you and you stick to the observed space If you send the main script I can help more.
person user4833143    schedule 01.05.2015