여러 종류의 칼만필터(Family members of Kalman Filter)(EnKF)

Ensemble Kalman Filter

Ensemble Kalman Filter 는 PythonRobotics 예제코드 위주로 진행해보겠습니다.

def main():
    print(__file__ + " start!!")

    time = 0.0

    # RF_ID positions [x, y]
    RF_ID = np.array([[10.0, 0.0],
                      [10.0, 10.0],
                      [0.0, 15.0],
                      [-5.0, 20.0]])

    # State Vector [x y yaw v]'
    xEst = np.zeros((4, 1))
    xTrue = np.zeros((4, 1))
    px = np.zeros((4, NP))  # Particle store of x

    xDR = np.zeros((4, 1))  # Dead reckoning

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue

    while SIM_TIME >= time:
        time += DT
        u = calc_input()

        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RF_ID)

        xEst, PEst, px = enkf_localization(px, z, ud)

        # store data history
        hxEst = np.hstack((hxEst, xEst))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))

main입니다. 이전 예제에서와 같이 변수초기화 이후에 루프를 돌며 EnKF 알고리즘을 돌리는 것을 확인할 수 있습니다. 여기서 다른 칼만필터와 차이점은 RF_ID(Reference Identification)라는 변수가 있는데 랜드마크를 의미하는 것 같습니다. 그리고 px라는 변수는 Particle store of x 라고 되어있는데 어떤 역할을 할지 코드를 보면서 확인해봐야 할 것 같습니다.

 

def calc_input():
    v = 1.0  # [m/s]
    yaw_rate = 0.1  # [rad/s]
    u = np.array([[v, yaw_rate]]).T
    return u

로봇으로 들어오는 컨트롤

 

def motion_model(x, u):
    F = np.array([[1.0, 0, 0, 0],
                  [0, 1.0, 0, 0],
                  [0, 0, 1.0, 0],
                  [0, 0, 0, 0]])

    B = np.array([[DT * math.cos(x[2, 0]), 0],
                  [DT * math.sin(x[2, 0]), 0],
                  [0.0, DT],
                  [1.0, 0.0]])

    x = F.dot(x) + B.dot(u)
    #x = F@x + B@u
    return x

정의된 motion model 입니다

 

def observation(xTrue, xd, u, RFID):
    xTrue = motion_model(xTrue, u)

    z = np.zeros((0, 4))

    for i in range(len(RFID[:, 0])):
        dx = RFID[i, 0] - xTrue[0, 0]
        dy = RFID[i, 1] - xTrue[1, 0]
        d = math.hypot(dx, dy)
        angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
        
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5  # add noise
            angle_with_noise = angle + np.random.randn() * Q_sim[1, 1] ** 0.5
            zi = np.array([dn, angle_with_noise, RFID[i, 0], RFID[i, 1]])
            z = np.vstack((z, zi))

    # add noise to input
    ud = np.array([[
        u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5,
        u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5]]).T

    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud

observation입니다. 관측은 실제 로봇의 위치에서 관측하는 것이기 때문에 ground truth state xTrue 를 이용해줍니다. 루프를 돌며 관측범위 내의 랜드마크를 관찰합니다. d는 로봇과 랜드마크의 거리, angle은 로봇의 각도와 로봇-랜드마크가 이루는 각입니다. 관측시 오차가 발생하기때문에 거리와 각도에 노이즈를 넣어주고 관측 z를 만들어줍니다.

관측은 실제 로봇의 위치에서 하는 것이지만 로봇은 자신위 위치를 정확하게 알 수 없기 때문에 control u에 노이즈를 넣고 노이즈가 섞인 xd를 만들어줍니다. 

 

이제 추정 state vector xd와 관측 z가 준비되었으니 Kalman Filter 알고리즘을 돌릴 준비가 되었습니다.

 

def enkf_localization(px, z, u):
    """
    Localization with Ensemble Kalman filter
    """
    pz = np.zeros((z.shape[0] * 2, NP))  # Particle store of z
    for ip in range(NP):
        x = np.array([px[:, ip]]).T

        #  Predict with random input sampling
        ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
        ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
        
        ud = np.array([[ud1, ud2]]).T
        x = motion_model(x, ud)
        px[:, ip] = x[:, 0]
        z_pos = observe_landmark_position(x, z)
        pz[:, ip] = z_pos[:, 0]
    
    x_ave = np.mean(px, axis=1)
    x_dif = px - np.tile(x_ave, (NP, 1)).T

    z_ave = np.mean(pz, axis=1)
    z_dif = pz - np.tile(z_ave, (NP, 1)).T

    U = 1 / (NP - 1) * x_dif @ z_dif.T
    V = 1 / (NP - 1) * z_dif @ z_dif.T

    K = U @ np.linalg.inv(V)  # Kalman Gain

    z_lm_pos = z[:, [2, 3]].reshape(-1, )

    px_hat = px + K @ (np.tile(z_lm_pos, (NP, 1)).T - pz)

    xEst = np.average(px_hat, axis=1).reshape(4, 1)
    PEst = calc_covariance(xEst, px_hat)

    return xEst, PEst, px_hat

최초에 px는 0 행렬로 초기화되어 있습니다. 루프에서 뭘 하는지 보면 \(x_{0}\)에서 노이즈가 섞여있을 때 갈 수 있는 20개의 샘플을 추리고 그 샘플에서 보여질 관측샘플도 20개를 구해줍니다. 현재는 particle x가 전부 0 벡터지만 iteration이 돌면서 particle이 뿌려지면서 가우시안 covariance가 커지듯 particle이 찍히는 영역이 넓어질것이라는 추측을 해볼 수 있겠네요.

 

산술평균 x_ave, z_ave를 구하고 x_dif, z_dif를 구해줍니다. 이후로는 모든 칼만필터가 그렇듯이 Kalman Gain 을 구해주고 state vector x를 보정해줄 것입니다. z는 실제 우리가 관측한 랜드마크 위치, pz는 우리가 particle x에서 보일것이라고 생각한 랜드마크 위치이고 그 차이를 통해 다시 \(\hat{px}\) 을 구한 것을 볼 수 있습니다. 최종 EnKF는 \(\hat{px}\)의 평균으로 구해집니다.

 

EnKF 를 통해 추정한 로봇의 경로 plot입니다.