import numpy as np

import math

import matplotlib.pyplot as plt

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

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 = 50.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):

"""

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]

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):

while(angle > math.pi):

angle = angle - 2.0 * math.pi

while(angle < -math.pi):

angle = angle + 2.0 * math.pi

return angle

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()