# Untitled

unknown
plain_text
2 years ago
8.2 kB
2
Indexable
Never
```# define imports
import math
import matplotlib.pyplot as plt
import numpy as np

# define constants
ROBOT_WIDTH = np.nan  # measure and put
TOTAL_TIME = 1000  # total simulation time [s]

X_INIT = [0, 0, 0.5*np.pi]
MU = X_INIT.T
COVARIANCE = np.zeros((3, 3))

def get_motor_movement():
# returns the motor movement in this timestep
# return (l, r)
pass

def camera_detections(camera_coords):
# get landmarks from camera image and return a list
# where each item is a landmark observation:
# [[actual_measurement, world_coords, camera_coords, id], ...]
pass

class EKF_SLAM:

def __init__(self):
# initialize the mu and covariance with the defined global constants
self.mu = MU
self.cov = COVARIANCE
# create an empty landmark map where landmark id and count will be stored as key-value pairs
self.landmark_map = dict()
self.total_landmarks = 0

def get_alpha_angle(l, r):
return (r-l)/ROBOT_WIDTH

if alpha != 0:
return l/alpha
return 0

def predict(self, u):
# get l & r from the movement variable u
[l, r] = u
theta = self.mu[2]
cov_u = np.array([[np.var(l), 0],
[0, np.var(r)]])
alpha = self.get_alpha_angle(l, r)
if alpha != 0:
R = l/alpha
else:
R = np.nan
if (l == r):
# compute G, A, B, C, D
G = np.array([[1, 0, ((l/alpha) + ROBOT_WIDTH/2)*(np.cos(theta + alpha) - np.cos(theta))],
[0, 1, ((l/alpha) + ROBOT_WIDTH/2) *
(np.sin(theta + alpha) - np.sin(theta))],
[0, 0, 1]])
A = (1/2)*(np.cos(theta) + (l/ROBOT_WIDTH)*np.sin(theta))
B = (1/2)*(np.sin(theta) - (l/ROBOT_WIDTH)*np.cos(theta))
C = (1/2)*(np.cos(theta) - (l/ROBOT_WIDTH)*np.sin(theta))
D = (1/2)*(np.sin(theta) + (l/ROBOT_WIDTH)*np.cos(theta))
else:
# compute G, A, B, C, D
G = np.array([[1, 0, -l*np.sin(theta)],
[0, 1, ((l/alpha) + ROBOT_WIDTH/2) *
(np.sin(theta + alpha) - np.sin(theta))],
[0, 0, 1]])
A = ((ROBOT_WIDTH * r)/np.square(r - l)) * \
(np.sin(theta + (r-l)/ROBOT_WIDTH) - np.sin(theta))
- (r+l)/(2*(r-l))*np.cos(theta + (r-l)/ROBOT_WIDTH)
B = ((ROBOT_WIDTH * r)/np.square(r - l)) * \
(-np.cos(theta + (r-l)/ROBOT_WIDTH) + np.cos(theta))
- (r+l)/(2*(r-l))*np.sin(theta + (r-l)/ROBOT_WIDTH)
C = -((ROBOT_WIDTH * l)/np.square(r - l)) * \
(np.sin(theta + (r-l)/ROBOT_WIDTH) - np.sin(theta))
+ (r+l)/(2*(r-l))*np.cos(theta + (r-l)/ROBOT_WIDTH)
D = ((ROBOT_WIDTH * l)/np.square(r - l)) * \
(-np.cos(theta + (r-l)/ROBOT_WIDTH) + np.cos(theta))
+ (r+l)/(2*(r-l))*np.sin(theta + (r-l)/ROBOT_WIDTH)

# compute V
V = np.array([[A, C],
[B, D],
[-(1/ROBOT_WIDTH), (1/ROBOT_WIDTH)]])

# compute G_hat and V_hat as directed in the EKFSLAM pdf to be used going forward
G_hat = np.array([[G, np.zeros((self.total_landmarks, self.total_landmarks))],
[np.zeros((self.total_landmarks, self.total_landmarks)), np.eye(self.total_landmarks)]])

V_hat = np.array([[V], [np.zeros((self.total_landmarks, self.total_landmarks))]])

# according to Robot Motion Model of EKFSLAM pdf
x_y = np.array([[self.mu[0]], [self.mu[1]]])
if alpha == 0:
x_prime_y_prime = x_y + \
(l*np.array([[np.cos(theta)], [np.sin(theta)]]))
theta_prime = theta
else:
x_prime_y_prime = x_y + (R+(ROBOT_WIDTH/2)) * np.array([np.sin(
theta + alpha) - np.sin(theta)], [(-np.cos(theta + alpha)) - (-np.cos(theta))])
theta_prime = (theta + alpha) % (2*np.pi)

# Predict new state
self.mu = np.append(x_prime_y_prime, theta_prime, self.mu[3:])

# Predict new covariance
self.cov = G_hat.dot(self.cov).dot(G_hat.T) + \
(V_hat).dot(cov_u).dot(V_hat.T)

# get world coordinates of the newly found landmark
[x_i, y_i] = world_coords
# update the mu
self.mu = np.append(self.mu, x_i, y_i)
cov_i = np.array([[np.var(x_i), 0],
[0, np.var(y_i)]])
# update the covariance
self.cov = np.array([[self.cov, 0],
[0, cov_i]])
# increase the count of total landmarks found
self.total_landmarks += 1
# set the landmark number in the map
self.landmark_map[id] = self.total_landmarks

def correction(self, actual_measurement, id):
# get the actual measured r and alpha
[r_act, alpha_act] = actual_measurement
# compute Q
Q = np.array([[np.var(r_act), 0], [0, np.var(alpha_act)]])
# get landmark index number
landmark_ind = self.landmark_map[id]

# compute h(x, y, theta, x_m, y_m) = [[r], [alpha]]
x_m_minus_x = self.mu[2*landmark_ind+3] - self.mu[0]
y_m_minus_y = self.mu[2*landmark_ind+4] - self.mu[1]
r = np.sqrt(np.square(x_m_minus_x) + np.square(y_m_minus_y))
alpha = np.arctan(x_m_minus_x/y_m_minus_y) - self.mu[3]
h = np.array([[r], [alpha]])

# compute expected observations
denom = (np.square(x_m_minus_x) + np.square(y_m_minus_y))
delta_r_x = - (x_m_minus_x)/np.sqrt(denom)
delta_alpha_x = (y_m_minus_y)/np.sqrt(denom)
delta_r_y = - (y_m_minus_y)/np.sqrt(denom)
delta_alpha_y = - (x_m_minus_x)/np.sqrt(denom)
delta_r_xm = (x_m_minus_x)/np.sqrt(denom)
delta_alpha_xm = (y_m_minus_y)/(denom)
delta_r_ym = (y_m_minus_y)/np.sqrt(denom)
delta_alpha_ym = (x_m_minus_x)/(denom)

# calculate Jacobian
H = np.array([[delta_r_x, delta_r_y, 0, delta_r_xm, delta_r_ym],
[delta_alpha_x, delta_alpha_y, 0, delta_alpha_xm, delta_alpha_ym]], dtype='float')

# calculate Kalman gain
K = self.cov.dot(H.T).dot(np.linalg.inv(H.dot(self.cov).dot(H.T)+Q))

# calculate difference between expected and real observation
m_dif = actual_measurement - h
m_dif = (m_dif + np.pi) % (2*np.pi) - np.pi

# update state vector and covariance matrix
self.mu = self.mu + K.dot(m_dif)

# identity matrix should be with size (3+2N)X(3+2N). Since we are considering
# only one landmark here, N => 1. Therefore, (3+2N) => 5
self.cov = (np.eye(5)-K.dot(H)).dot(self.cov)

def get_robot_pose(self):
return self.mu[:3]

def get_landmark_positions(self):
return self.mu[3:]

slam = EKF_SLAM()

for timestep in range(TOTAL_TIME):

# movements is what is refered to as u = (l, r) in the document
movements = get_motor_movement()
slam.predict(movements)
landmark_observations = camera_detections(slam.get_robot_pose())
for landmark_observation in landmark_observations:
actual_measurement, world_coords, camera_coords, id = landmark_observation

# actual_measurement is the distance r and angle alpha of the landmark
# → relative to the robot.
# world_coords are the world coordinates of the landmark.
# camera_coords are the camera coordinates of the landmark.
# id is a unique number identifying the landmark.
if id not in slam.landmark_map: