def add_new_lm(self, particle, z):
"""
Initializes a yet unknown landmark using measurement
:param particle: Particle that will be updated
:param z: Measurement
:return: Particle with a new landmark location and landmark uncertainty added
"""
r = z[0]
b = z[1]
measured_x = cos(normalize_angle(particle.theta + b))
measured_y = sin(normalize_angle(particle.theta + b))
# Calculate landmark location
new_lm = np.array(
[particle.x + r * measured_x,
particle.y + r * measured_y]).reshape(1, self.landmark_state_size)
particle.lm = np.vstack((particle.lm, new_lm))
# Calculate initial covariance
Gz = np.array([[measured_x, -r * measured_y],
[measured_y, r * measured_x]])
particle.lmP = np.vstack((particle.lmP, Gz @ self.sensor_noise @ Gz.T))
particle.id.append(z[2])
return particle
def motion_model(x, u, dt):
"""
Noise-free motion model method
:param x: The robot's pose
:param u: Motion command as a tuple of translational and angular velocities
:param dt: (Discrete) Time for which the motion command is executed
:return: Resulting robot's pose
"""
# No angular velocity means following a straight line
if u[1, 0] == 0:
B = np.array([[dt * cos(x[2, 0]) * u[0, 0]],
[dt * sin(x[2, 0]) * u[0, 0]], [0.0]])
# Otherwise the robot follows a circular arc
else:
B = np.array([[
u[0, 0] / u[1, 0] *
(sin(x[2, 0] + dt * u[1, 0]) - sin(x[2, 0]))
],
[
u[0, 0] / u[1, 0] *
(-cos(x[2, 0] + dt * u[1, 0]) + cos(x[2, 0]))
], [u[1, 0] * dt]])
res = x + B
res[2] = normalize_angle(res[2])
return res
def correction_step(self, z):
"""
Update the predicted state and uncertainty using the sensor measurements.
:param z: List of sensor measurements. A single measurement is a tuple of measured distance, measured angle and identify.
"""
# Iterate through all sensor readings
for i, measurement in enumerate(z):
# Only execute if sensor observed landmark
if not self.supervisor.proximity_sensor_positive_detections()[i]\
or measurement[2] == -1: # not a feature
continue
if not self.landmark_correspondence_given:
lm_id = self.data_association(self.mu, self.Sigma,
measurement[0:2])
else:
lm_id = self.data_association_v2(self.mu, measurement[2])
nLM = self.get_n_lm(self.mu)
if lm_id == nLM: # If the landmark is new
self.add_new_landmark(measurement[0:2], measurement[2])
lm = self.get_landmark_position(self.mu, lm_id)
innovation, Psi, H = self.calc_innovation(lm, self.mu, self.Sigma,
measurement[0:2], lm_id)
K = (self.Sigma @ H.T) @ np.linalg.inv(Psi)
self.mu += K @ innovation
# Normalize robot angle so it is between -pi and pi
self.mu[2] = normalize_angle(self.mu[2])
self.Sigma = (np.identity(len(self.mu)) - (K @ H)) @ self.Sigma
def __init__( self, *args ):
if len( args ) == 2: # initialize using a vector ( vect, theta )
vect = args[0]
theta = args[1]
self.x = vect[0]
self.y = vect[1]
self.theta = math_util.normalize_angle( theta )
elif len( args ) == 3: # initialize using scalars ( x, y theta )
x = args[0]
y = args[1]
theta = args[2]
self.x = x
self.y = y
self.theta = math_util.normalize_angle( theta )
else:
raise TypeError( "Wrong number of arguments. Pose requires 2 or 3 arguments to initialize" )
def execute(self, theta_d):
"""
Executes the controllers update during one simulation cycle
:param theta_d: The angle in which the robot should drive
"""
theta = self.supervisor.estimated_pose().theta
e = math_util.normalize_angle(theta_d - theta)
omega = self.k_p * e
self.supervisor.set_outputs(1.0, omega)
def calc_innovation(self, lm, mu, Sigma, z, LMid):
"""
Calculates the innovation, uncertainty and Jacobian
:param lm: Position of observed landmark
:param mu: Combined state vector
:param Sigma: Covariance matrix
:param z: Measurement, consisting of tuple of measured distance and measured angle
:param LMid: Id of the observed landmark
:return: The innovation, the uncertainty of the measurement and the Jacobian
"""
delta = lm - mu[0:2]
q = (delta.T @ delta)[0, 0]
zangle = atan2(delta[1, 0], delta[0, 0]) - mu[2, 0]
expected_measurement = np.array([[sqrt(q), normalize_angle(zangle)]])
innovation = (z - expected_measurement).T
innovation[1] = normalize_angle(innovation[1])
H = self.jacob_sensor(q, delta, self.get_n_lm(mu), LMid)
Psi = H @ Sigma @ H.T + self.sensor_noise
return innovation, Psi, H
def __init__(self, *args):
if len(args) == 2: # initialize using a vector ( vect, theta )
vect = args[0]
theta = args[1]
self.x = vect[0]
self.y = vect[1]
self.theta = math_util.normalize_angle(theta)
elif len(args) == 3: # initialize using scalars ( x, y theta )
x = args[0]
y = args[1]
theta = args[2]
self.x = x
self.y = y
self.theta = math_util.normalize_angle(theta)
else:
raise TypeError(
"Wrong number of arguments. Pose requires 2 or 3 arguments to initialize"
)
def update_landmark(self, particle, z, lm_id):
"""
Updates the estimated landmark position and uncertainties as well as the particles importance factor
:param particle: Particle that is being updated
:param z: Measurement
:param lm_id: Id of the landmark that is associated to the measurement
:return: Updated particle
"""
landmark = np.array(particle.lm[lm_id, :]).reshape(2, 1)
landmark_cov = np.array(particle.lmP[2 * lm_id:2 * lm_id + 2, :])
# Computing difference between landmark and robot position
delta_x = landmark[0, 0] - particle.x
delta_y = landmark[1, 0] - particle.y
# Computing squared distance
q = delta_x**2 + delta_y**2
sq = sqrt(q)
# Computing the measurement that would be expected
expected_measurement = np.array([
sq, normalize_angle(atan2(delta_y, delta_x) - particle.theta)
]).reshape(2, 1)
# Computing the Jacobian
H = np.array([[delta_x / sq, delta_y / sq],
[-delta_y / q, delta_x / q]])
# Computing the covariance of the measurement
Psi = H @ landmark_cov @ H.T + self.sensor_noise
# Computing the innovation, the difference between actual measurement and expected measurement
innovation = z.reshape(2, 1) - expected_measurement
innovation[1, 0] = normalize_angle(innovation[1, 0])
landmark, landmark_cov = self.ekf_update(landmark, landmark_cov,
innovation, H, Psi)
particle.lm[lm_id, :] = landmark.T
particle.lmP[2 * lm_id:2 * lm_id + 2, :] = landmark_cov
# Multiplying importance factors, since this is just the weight for a single sensor measurement
particle.w *= self.compute_importance_factor(innovation, Psi)
return particle
def motion_model(x, u, dt):
"""
Noise-free motion model method
:param x: The robot's pose
:param u: Motion command as a tuple of translational and angular velocities
:param dt: (Discrete) Time for which the motion command is executed
:return: Resulting robot's pose
"""
v, w = u[0, 0], u[1, 0]
s1, s12 = sin(x[2, 0]), sin(x[2, 0] + dt * w)
c1, c12 = cos(x[2, 0]), cos(x[2, 0] + dt * w)
# No angular velocity means following a straight line
if w == 0:
B = np.array([[dt * c1 * v], [dt * s1 * v], [0.0]])
# Otherwise the robot follows a circular arc
else:
r = v / w
B = np.array([[-r * s1 + r * s12], [r * c1 - r * c12], [w * dt]])
x = x + B
x[2] = normalize_angle(x[2])
return x
def execute(self, theta_d):
theta = self.supervisor.estimated_pose().theta
e = math_util.normalize_angle(theta_d - theta)
omega = self.k_p * e
self.supervisor.set_outputs(1.0, omega)
def normalize_angles(self, vertices):
for v in vertices:
if isinstance(v, PoseVertex):
v.pose[2, 0] = normalize_angle(v.pose[2, 0])
def supdate(self, x, y, theta):
self.x = x
self.y = y
self.theta = math_util.normalize_angle(theta)
def vupdate(self, vect, theta):
self.x = vect[0]
self.y = vect[1]
self.theta = math_util.normalize_angle(theta)
def execute( self, theta_d ):
theta = self.supervisor.estimated_pose().theta
e = math_util.normalize_angle( theta_d - theta )
omega = self.k_p * e
self.supervisor.set_outputs( 1.0, omega )
def supdate( self, x, y, theta ): self.x = x self.y = y self.theta = math_util.normalize_angle( theta )
def vupdate( self, vect, theta ): self.x = vect[0] self.y = vect[1] self.theta = math_util.normalize_angle( theta )
