EKF SLAM with Example Code

역시 PythonRobotics 의 예제를 가져와봤습니다. 이전 포스팅의 EKF SLAM이 실제로 어떻게 구현되어있는지, 어떤 차이가 있는지 알아보기 위한 코드 리뷰입니다. 물론 실제 플랫폼, 센서를 갖고 하는 것이 아닌 시뮬레이션이기 때문에 그 부분은 감안 해야하겠습니다. 

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

    time = 0.0

    # RFID positions [x, y]
    RFID = np.array([[10.0, -2.0],
                     [15.0, 10.0],
                     [3.0, 15.0],
                     [-5.0, 20.0]])

    # State Vector [x y yaw v]'
    xEst = np.zeros((STATE_SIZE, 1))	# for predicted state vector
    xTrue = np.zeros((STATE_SIZE, 1)) 	# for ground truth state vector
    PEst = np.eye(STATE_SIZE)		# covariance matrix
    xDR = np.zeros((STATE_SIZE, 1)) 	# dead reckoning state vector
    
    while SIM_TIME >= time:
        time += DT
        u = calc_input()
        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
        xEst, PEst = ekf_slam(xEst, PEst, ud, z)
        x_state = xEst[0:STATE_SIZE]

main문에서의 초기화 코드입니다. 지금껏 봐왔던 Localization을 위한 Kalman Filter 예제코드와 크게 다른부분이 없습니다.  4개의 랜드마크를 설정하고 변수들을 초기화합니다. 그리고 루프를 돌며 SLAM을 수행합니다.

 

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

    # add noise to gps x-y
    z = np.zeros((0, 3))

    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_n = angle + np.random.randn() * Q_sim[1, 1] ** 0.5  # add noise
            zi = np.array([dn, angle_n, i])
            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 입니다. 플랫폼의 motion이 observation이 같이 구현되어있는데 코드를 분리하는게 의미상 더 맞는것 같지만 이부분은 넘어가겠습니다. Observation의 의미는 말 그대로 실제 관측값을 얻는 부분이기 때문에 ground truth state 에서 랜드마크를 관측한 정보 z를 반환합니다. 

 

def ekf_slam(xEst, PEst, u, z):
    # Predict
    S = STATE_SIZE
    G, Fx = jacob_motion(xEst[0:S], u)
    xEst[0:S] = motion_model(xEst[0:S], u)
    PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
    initP = np.eye(2)

    # Update
    for iz in range(len(z[:, 0])):  # for each observation
        min_id = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2])

        nLM = calc_n_lm(xEst)
        if min_id == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_landmark_position(xEst, z[iz, :])))
            PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
                              np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
            xEst = xAug
            PEst = PAug
        lm = get_landmark_position_from_state(xEst, min_id)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], min_id)

        K = (PEst @ H.T) @ np.linalg.inv(S)
        xEst = xEst + (K @ y)
        PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst

    xEst[2] = pi_2_pi(xEst[2])

    return xEst, PEst

Localization은 state의 크기가 플랫폼의 위치나 각도 등을 나타내는 벡터였지만 SLAM에서는 mapping을 같이 해야하기 때문에 \(F_{x}\)를 통해 확장해주었습니다. 하지만 예제에선 루프를 돌며 점차적으로 확장해주는 것을 알 수 있습니다. 

 

Predict의 xEst, PEst 를 구하는 부분은 EKF Localization과 동일합니다. Update는 관측한 랜드마크가 있다면 state vector와 covariance matrix를 확장합니다. 조금 더 자세히 보면 xAug, 즉 state에는 robot의 pose와 robot pose에서 관측값을 토대로 만든 landmark의 위치가 저장됩니다.  칼만필터의 주 목적인 관측값을 통한 보정을 수행하려면 실제 관측값 z와 lm을 이용해 역으로 z_pred를 계산하고 그 차이를 Kalman gain과 곱해야 합니다.