파이썬 로보틱스 - SLAM, 확장 칼만 필터

EKF SLAM

 확장 칼만필터 슬램은 가장 일찍 나온 슬램 알고리즘 중 하나로

최대 우도 데이터 연관을 사용한 온라인 슬램으라 할수 있습니다.

 

 

EKF SLAM의 가정들

1. 특징 기반 지도

 EKF SLAM에서 사용하는 지도는 점 특징 기반 지도가 사용되고,

여기서 점들의 갯수는 1000개 이하로 적으며, 점들 간에 구분하기 쉬울수록 잘 동작합니다.

EKF SLAM은 특징 검출기가 필요한데, 특징으로 인공 비콘이나 랜드마크를 사용합니다.

 

2. 가우시안 노이즈

 EKF SLAM은 로봇의 동작과 인지 시 가우시안 노이즈를 따른다고 가정하며

사후 확률의 불확실성은 상대적으로 작으나, EKF 선형화시 견딜수 없는 에러가 발생할 수 있습니다.

 

3. 긍정 측정치

 EKF SLAM은 랜드마크에 대한 긍정 정보만을 사용할수 있으며,

랜드마크가 사라지는 것같은 부정 정보를 처리할수 없습니다.

 

 

 

 

 EKF SLAM을 사용하는 경우 대응관계가 주어진 경우와 대응관계가 주어지지 않은 경우가 있으나

일반적으로 대응관계는 주어지지 않으며 파이썬 로보틱스에서 제공하는 예제 또한 그러하므로

이 경우에 대한 시뮬레이션 예제를 살펴보겠습니다.

 

 

대응 관계가 주어지지 않은 EKF SLAM 알고리즘

 이 슬램 알고리즘은 대응 관계를 구하기 위해

증문 최대 우도 추정기 incremental maximum likelihood(ML)을 사용합니다.

 

 

 

 

import numpy as np
import math
import matplotlib.pyplot as plt


# EKF state covariance
Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2

#  Simulation parameter
Qsim = np.diag([0.2, math.radians(1.0)])**2
Rsim = np.diag([1.0, math.radians(10.0)])**2

DT = 0.1  # time tick [s]
SIM_TIME = 60.0  # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range
M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3  # State size [x,y,yaw]
LM_SIZE = 2  # LM srate size [x,y]

show_animation = True

 

늘 하던대로

임포트 라이브러리와 전역변수들을 살펴봅시다.

 

상태 공분산 Cx와 동작, 관측 노이즈 Q,Rsim을 초기화하고,

별도 파라미터들을 설정해 주었습니다.

 

 

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.matrix(np.zeros((STATE_SIZE, 1)))
    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
    PEst = np.eye(STATE_SIZE)

    xDR = np.matrix(np.zeros((STATE_SIZE, 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, RFID)

        xEst, PEst = ekf_slam(xEst, PEst, ud, z)

        x_state = xEst[0:STATE_SIZE]

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

        if show_animation:
            plt.cla()

            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(xEst[0], xEst[1], ".r")

            # plot landmark
            for i in range(calc_n_LM(xEst)):
                plt.plot(xEst[STATE_SIZE + i * 2],
                         xEst[STATE_SIZE + i * 2 + 1], "xg")

            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)


if __name__ == '__main__':
    main()

 메인 함수를 보면, 플로팅 부분을 제외하고 루프 앞까지를 보면

랜드마크로 사용할 RFID 4개를 정의 하였습니다.

 

 이후 추정 상태 변수 xEst, 실제 상태 변수 xTrue

추정 상태 공분산 PEst를 만들어줍니다.

그 다음 추측항법도 비교해줄수 있도록 xDR도 만들고

 

 추측 항법 상태, 추정 상태, 실제 상태들을 담기위해

hx 변수들도 만들어 줍니다.

이제 루프문 내부를 확인하겠습니다.

 

    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]

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

 맨 앞은 우리가 자주보던 입력 구하는 함수 calc_input

 그 다음에는 관측 함수

 ekf slam

 뒤에는 로봇의 상태변수들만 hx 변수에 담는 과정이 되겠습니다.

 

 calc input 함수는 입력값으로 사용할 선속도와 각속도를 반환하니 패스하고

 관측 함수 부터 살펴보겠습니다.

 

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

    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = np.matrix(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.sqrt(dx**2 + dy**2)
        angle = pi_2_pi(math.atan2(dy, dx))
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
            zi = np.matrix([dn, anglen, i])
            z = np.vstack((z, zi))

    # add noise to input
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.matrix([ud1, ud2]).T

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud

 관측 함수에서 처음에는 실제 로봇의 주행 궤적을 구하기 위해 xTrue를 동작시키고,

다음에는 관측치의 노이즈로 사용할 행렬을 생성 합니다.

 

 관측치인 거리와 방위는 실제 로봇의 위치와 랜드마크 사이의 거리로 구할수 있으므로

dx, dy를 통해 대각선 거리 d와 방위 angle을 구합니다.

 

 하지만 측정 가능 거리에 존재하는 랜드마크만 검출할수 있으므로 거리재한을 두고,

이 거리재한 안에 존재하는 랜드마크에 한해 가우시안 노이즈를 추가한 dn, anglen을 관측치 z로 추가합니다.

 

 다음으로 동작 모델을 사용하는데, 기존의 입력에 가우시안 노이즈를 추가하여

추측 항법 상태를 구합니다.

 

    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]

 관측 함수를 수행하였으니

다음으로 ekf slam 함수를 보겠습니다.

 

 여기서 이전 상태와 공분산, 노이즈가 추가된 입력,

관측 함수로 구한 관측치들이 매개변수로 사용됩니다.

 

def ekf_slam(xEst, PEst, u, z):

    # Predict
    S = STATE_SIZE
    xEst[0:S] = motion_model(xEst[0:S], u)
    G, Fx = jacob_motion(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
        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])

        nLM = calc_n_LM(xEst)
        if minid == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(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_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)

        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

 ekf slam 함수에서는 우선 예측 과정을 수행합니다.

여기서 추정할 상태는 x, y, yaw 이 3가지므로

추정 상태 공간으로부터 로봇의 자세 변수만 구하여 사용합니다.

 

 모션 모델 함수에 로봇의 이전 추정 자세와

노이즈 입력을 넣어 예측 추정치를 구하였습니다.

 

 다음 줄은 동작 모델의 자코비안을 구하는 것으로 잠시 살펴보겠습니다.

 

def jacob_motion(x, u):

    Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
        (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))

    jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
                    [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
                    [0.0, 0.0, 0.0]])

    G = np.eye(STATE_SIZE) + Fx.T * jF * Fx

    return G, Fx,

 이 함수에는 동작 모델에서 구한 상태 변수들의

입력 변수에 대한 자코비안을 구한 결과를 반환합니다.

 

def ekf_slam(xEst, PEst, u, z):

    # Predict
    S = STATE_SIZE
    xEst[0:S] = motion_model(xEst[0:S], u)
    G, Fx = jacob_motion(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)

 

 다시 ekf slam의 앞부분으로 돌아오면

방금 구한 자코비안을 통해 로봇의 추정 자세 변수들에 대한 예측 공분산을 구할수 있게 됩니다.

이 아래에는 관측치에 대한 루프를 돌리겠습니다.

 

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

        nLM = calc_n_LM(xEst)
        if minid == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(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_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)

        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

 

 

 이 루프문의 첫번째로

추정 상태와 추정 공분산, 관측이 주어질때 해당 랜드마크의 아이디를 찾는 함수입니다.

 

def search_correspond_LM_ID(xAug, PAug, zi):
    """
    Landmark association with Mahalanobis distance
    """

    nLM = calc_n_LM(xAug)

    mdist = []

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

 

 이 랜드마크 아이디 탐색 함수는

마할라노비스 거리를 이용한 랜드마크 연관으로

매개변수로부터 전달받은 추정 상태를 통해 랜드마크의 갯수를 확인해 봅니다.

 

def calc_n_LM(x):
    n = int((len(x) - STATE_SIZE) / LM_SIZE)
    return n

 초기 추정 상태는 로봇의 자세 3개밖에 없지만

랜드마크가 검출 될수록 각 랜드마크의 위치 x, y 2개씩 추가 되게 됩니다.

그래서 전체 x의 길이 - 로봇 자세 3을 한 후 2를 나누어주면 그동안 관측한 랜드마크 갯수가 나오게 됩니다.

 

def search_correspond_LM_ID(xAug, PAug, zi):
    """
    Landmark association with Mahalanobis distance
    """

    nLM = calc_n_LM(xAug)

    mdist = []

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

 

 추정 상태공간으로부터 랜드마크 갯수를 구한 후

마할라노비스 거리를 담을 텅빈 리스트를 만들어 주고

각 랜드마크에 대해 루프를 돌려봅시다.

 

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

 가장 처음 만나는 함수는 추정 상태공간(로봇의 자세 + 랜드마크 위치들)에서

해당 랜드마크의 위치를 추출하는 함수가 됩니다.

 

def get_LM_Pos_from_state(x, ind):

    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]

    return lm

 이 함수에서 해당 랜드마크를 찾아 랜드마크의 위치 (x, y)를 반환합니다.

 

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

 

 랜드마크 위치, 추정 예측 상태, 추정 예측 공분산, 관측치, 랜드마크 인댁스를 이용해서

개변치 계산 calc_innovation 함수를 살펴보겠습니다.

 

def calc_innovation(lm, xEst, PEst, z, LMid):
    delta = lm - xEst[0:2]
    q = (delta.T * delta)[0, 0]
    zangle = math.atan2(delta[1], delta[0]) - xEst[2]
    zp = [math.sqrt(q), pi_2_pi(zangle)]
    y = (z - zp).T
    y[1] = pi_2_pi(y[1])
    H = jacobH(q, delta, xEst, LMid + 1)
    S = H * PEst * H.T + Cx[0:2, 0:2]

    return y, S, H

 

 개변치 계산 함수에서는 우선 랜드마크의 위치와 로봇의 자세 사이의 거리 delta를 계산하고,

delta의 제곱 q와 랜드마크와 로봇 사이 각인 zangle을 구합니다.

 

 이를 정리하여 예측 로봇 자세와 추정 랜드마크 사이 거리 + 각도로 이루어진 예측 zp를 구하고,

실제 관측치인 z에서 zp를 빼네어 실제 관측과 예측 관측사이의 차인 y를 구합니다.

 

 그 다음 거리 q와 예측 추정, 해당 랜드마크 번호로 관측에 대한 자코비안을 계산하는데

(jacobH 함수 생략) 자코비안 결과와 예측 추정 공분산 PEst, 그리고 노이즈 변수를 사용해

정리하면 실제 관측과 예측 관측사이 차 y, 자코비안 H, 계산한 분모 S를 반환합니다.

 

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid

 

개변치 계산 함수에서 돌아와

y와 S를 이용하여 마할라 노비스 거리를 계산하고,

마할라 노비스 거리 목록에 추가시켜줍니다.

 

 모든 랜드마크에 대한 마할라노비스 거리 목록이 만들어지면

마할라 노비스 거리 임계치도 추가해주고

그 중에 마할라노비스 거리가 가장 작은 값의 인덱스를 찾습니다.

 

 만약 마할라노비스거리 임계치의 인덱스가 나온다면

이는 새로운 랜드마크를 검출한 것이라 볼수 있겠습니다.

 

 아니면 다른 랜드마크에 대한 인덱스가 나온 경우

예측 추정 상태와 관측치로부터 가장 올바른 랜드마크를 찾은것이고

올바른 랜드마크의 아이디를 반환하게 됩니다.

 

 

def ekf_slam(xEst, PEst, u, z):

    # Predict
    S = STATE_SIZE
    xEst[0:S] = motion_model(xEst[0:S], u)
    G, Fx = jacob_motion(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
        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])

        nLM = calc_n_LM(xEst)
        if minid == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(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_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)

        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

 

 이재 랜드마크의 아이디를 찾는 함수를 완료하였으니 다시 내려가 봅시다.

랜드마크의 갯수를 세었는데, 반환받은 아이디와 같다면 새로운 랜드마크를 찾은게 됩니다.

 

 그래서 if 조건문 내부에

기존의 추정 상태 공간과 추정 공분산을 새 랜드마크에 대한 정보를 추가해 줍니다.

 

 예측 추정 상태와 랜드마크 아이디로 해당 랜드마크의 위치를 구하고,

개변치를 계산하고, 칼만 개인을 계산 후

예측 추정 상태에 칼만 개인과 실제 관측과 예측 관측사이 차 y를 더해주고,

예측 공분산에도 똑같이 반영을 해 줍니다.

 

 예측 추정 상태, 공분산에다가 모든 랜드마크에 대한 칼만게인, 관측차 등을 반영해주면

이 값은 실제 추정 상태값이라 할수 있습니다.

대응관계가 주어지지않은 경우 ekf slam를 살펴보았습니다.

 

 

 아래의 시뮬레이션에서

파란선 : 실제 궤적

검은 선 : 추측 항법 궤적

빨간선 : 확장 칼만 필터 추정 궤적

검은 점 : 랜드마크 실제 위치

녹색 x : 랜드마크 추정 위치

 

 

 

 

"""
Extended Kalman Filter SLAM example
author: Atsushi Sakai (@Atsushi_twi)
"""

import numpy as np
import math
import matplotlib.pyplot as plt


# EKF state covariance
Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2

#  Simulation parameter
Qsim = np.diag([0.2, math.radians(1.0)])**2
Rsim = np.diag([1.0, math.radians(10.0)])**2

DT = 0.1  # time tick [s]
SIM_TIME = 60.0  # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range
M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3  # State size [x,y,yaw]
LM_SIZE = 2  # LM srate size [x,y]

show_animation = True


def ekf_slam(xEst, PEst, u, z):

    # Predict
    S = STATE_SIZE
    xEst[0:S] = motion_model(xEst[0:S], u)
    G, Fx = jacob_motion(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
        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])

        nLM = calc_n_LM(xEst)
        if minid == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(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_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
        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


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


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

    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = np.matrix(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.sqrt(dx**2 + dy**2)
        angle = pi_2_pi(math.atan2(dy, dx))
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
            zi = np.matrix([dn, anglen, i])
            z = np.vstack((z, zi))

    # add noise to input
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.matrix([ud1, ud2]).T

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud


def motion_model(x, u):

    F = np.matrix([[1.0, 0, 0],
                   [0, 1.0, 0],
                   [0, 0, 1.0]])

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

    x = F * x + B * u

    return x


def calc_n_LM(x):
    n = int((len(x) - STATE_SIZE) / LM_SIZE)
    return n


def jacob_motion(x, u):

    Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
        (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))

    jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
                    [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
                    [0.0, 0.0, 0.0]])

    G = np.eye(STATE_SIZE) + Fx.T * jF * Fx

    return G, Fx,


def calc_LM_Pos(x, z):

    zp = np.zeros((2, 1))

    zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
    zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])

    return zp


def get_LM_Pos_from_state(x, ind):

    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]

    return lm


def search_correspond_LM_ID(xAug, PAug, zi):
    """
    Landmark association with Mahalanobis distance
    """

    nLM = calc_n_LM(xAug)

    mdist = []

    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)

    mdist.append(M_DIST_TH)  # new landmark

    minid = mdist.index(min(mdist))

    return minid


def calc_innovation(lm, xEst, PEst, z, LMid):
    delta = lm - xEst[0:2]
    q = (delta.T * delta)[0, 0]
    zangle = math.atan2(delta[1], delta[0]) - xEst[2]
    zp = [math.sqrt(q), pi_2_pi(zangle)]
    y = (z - zp).T
    y[1] = pi_2_pi(y[1])
    H = jacobH(q, delta, xEst, LMid + 1)
    S = H * PEst * H.T + Cx[0:2, 0:2]
    #H = np.array(H, dtype=np.float)
    return y, S, H


def jacobH(q, delta, x, i):
    sq = math.sqrt(q)
    G = np.matrix([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
                   [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
    G = G / q
    nLM = calc_n_LM(x)
    F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
    F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
                    np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))

    F = np.vstack((F1, F2))
    H = G * F

    return H


def pi_2_pi(angle):
    return float((angle + math.pi) % (2*math.pi) - math.pi)


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.matrix(np.zeros((STATE_SIZE, 1)))
    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
    PEst = np.eye(STATE_SIZE)

    xDR = np.matrix(np.zeros((STATE_SIZE, 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, RFID)

        xEst, PEst = ekf_slam(xEst, PEst, ud, z)

        x_state = xEst[0:STATE_SIZE]

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

        if show_animation:
            plt.cla()

            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(xEst[0], xEst[1], ".r")

            # plot landmark
            for i in range(calc_n_LM(xEst)):
                plt.plot(xEst[STATE_SIZE + i * 2],
                         xEst[STATE_SIZE + i * 2 + 1], "xg")

            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)


if __name__ == '__main__':
    main()