def _implicit_fuse(x_bar, P_bar, x_hat, P, x_ref, C, R, delta, h_x_hat,
h_x_bar, h_x_ref, angle_meas):
mu = h_x_hat - h_x_bar
Qe = np.dot(C, np.dot(P_bar, C.T)) + R
alpha = h_x_ref - h_x_bar
Qf = lambda x: 1 - normaldist.cdf(x)
if angle_meas:
nu_minus = normalize_angle(-delta + alpha - mu) / np.sqrt(Qe)
nu_plus = normalize_angle(delta + alpha - mu) / np.sqrt(Qe)
else:
nu_minus = (-delta + alpha - mu) / np.sqrt(Qe)
nu_plus = (delta + alpha - mu) / np.sqrt(Qe)
tmp = (normaldist.pdf(nu_minus) -
normaldist.pdf(nu_plus)) / (Qf(nu_minus) - Qf(nu_plus))
z_bar = tmp * np.sqrt(Qe)
tmp2 = (nu_minus * normaldist.pdf(nu_minus) - nu_plus *
normaldist.pdf(nu_plus)) / (Qf(nu_minus) - Qf(nu_plus))
curly_theta = tmp**2 - tmp2
K = np.dot(P, np.dot(C.T, inv(np.dot(C, np.dot(P, C.T)) + R)))
x_hat = x_hat + K * z_bar
P = P - curly_theta * np.dot(K, np.dot(C, P))
return x_hat, P
def _fuse_azimuth_from_untracked(x_hat, P, startx1, position, meas_value,
R):
NUM_STATES = x_hat.shape[0]
x1 = x_hat[startx1, 0]
y1 = x_hat[startx1 + 1, 0]
x2 = position[0]
y2 = position[1]
delta_pred = np.array([[x1 - x2], [y1 - y2]])
if norm(delta_pred) < 1e-3:
delta_pred[0, 0] = 1e-3
delta_pred[1, 0] = 1e-3
pred = np.arctan2(delta_pred[1, 0], delta_pred[0, 0])
dadx = -delta_pred[1, 0] / norm(delta_pred)**2
dady = delta_pred[0, 0] / norm(delta_pred)**2
H = np.zeros((1, NUM_STATES))
H[0, startx1] = dadx
H[0, startx1 + 1] = dady
innovation = normalize_angle(meas_value - pred)
if abs(innovation) < np.radians(90):
x_hat, P = KalmanFilter._fuse(x_hat, P, H, R, innovation)
else:
print("Modem azimuth innovation too large, rejecting: {}".format(
innovation))
return x_hat, P
def _fuse_azimuth_tracked(x_hat, P, startx1, startx2, meas_value, R):
pred, H = KalmanFilter._predict_azimuth(x_hat, startx1, startx2)
innovation = normalize_angle(meas_value - pred)
x_hat, P = KalmanFilter._fuse(x_hat, P, H, R, innovation)
return x_hat, P
def scan_control(scan_angle, my_pos, agent_dict, prototrack, scan_size,
ping_thresh, lost_thresh):
"""
scan_angle in inertial frame
Returns:
- float: the start scan region in inertial frame
- bool: whether scanning 360
"""
# Determine if an agent needs pinging
lost_agent = False
for agent in agent_dict:
mean = agent_dict[agent][0]
cov = agent_dict[agent][1]
if np.trace(cov) > lost_thresh:
lost_agent = True
elif np.trace(cov) > ping_thresh: # Agent is lost and attempt to scan
print("Pinging: {}".format(agent))
return scan_agent(mean, my_pos, scan_size), False
# Check if we have a lost agent, scan 360 deg
if lost_agent:
if prototrack is not None: # If we have any prototracks scan there first
print("Prototrack started, rescanning")
mean = prototrack[0]
return scan_agent(mean, my_pos, scan_size), False
else:
print("Agent lost: scanning")
return normalize_angle(scan_angle + scan_size), True
else:
# ping agent with highest uncertainty
agents, unc = [], []
for agent in agent_dict:
cov = agent_dict[agent][1]
agents.append(agent)
unc.append(np.trace(cov))
max_index = np.argmax(unc)
agent = agents[max_index]
print("Pinging: {}".format(agent))
mean = agent_dict[agent][0]
return scan_agent(mean, my_pos, scan_size), False
def _gradian2radian(self, grad):
return normalize_angle((np.pi / 200.0) * (200 - grad))
def scan_agent(mean, my_pos, scan_size):
delta = mean[:2] - np.reshape(my_pos[:2], (-1, 1))
x_delta = delta[0]
y_delta = delta[1]
world_angle = np.arctan2(y_delta, x_delta)
return normalize_angle(world_angle + scan_size / 2.0)
def pull_buffer(self, mults, delta_dict, position_process_noise,
velocity_process_noise, modem_loc, buffer_size):
"""
mults : list of floats/ints
delta_dict : dict{"meas_type" -> base_delta }
"""
middle_index = int(len(mults) / 2)
mult = mults[middle_index]
x_common_start = self.x_common
P_common_start = self.P_common
ledger_mat = KalmanFilter._get_ledger_mat(self.ledger)
while True:
x_common = x_common_start
P_common = P_common_start
share_buffer = []
# Loop through all time indices
for index in range(self.last_share_index, self.index + 1):
measurements = self._get_measurements_at_time(
ledger_mat, index)
x_common, P_common = KalmanFilter._propogate(x_common, P_common, \
position_process_noise, velocity_process_noise, self.BLUE_NUM, self.RED_NUM,\
self.LANDMARK_NUM)
x_common_bar = x_common
P_common_bar = P_common
for mi in range(measurements.shape[0]):
meas = measurements[mi, :]
meas_type = MEAS_TYPES_INDICES[int(
meas[MEAS_COLUMNS.index("type")])]
startx1 = int(meas[MEAS_COLUMNS.index("startx1")])
startx2 = int(meas[MEAS_COLUMNS.index("startx2")])
data = meas[MEAS_COLUMNS.index("data")]
R = meas[MEAS_COLUMNS.index("R")]
# ["modem_range", "modem_azimuth", "sonar_range", "sonar_azimuth", "sonar_range_implicit", "sonar_azimuth_implicit"]
if meas_type == "modem_range":
x_common, P_common = KalmanFilter._fuse_range_from_untracked(
x_common, P_common, startx1, modem_loc, data, R)
# Don't add to shared_buffer
elif meas_type == "modem_azimuth":
x_common, P_common = KalmanFilter._fuse_azimuth_from_untracked(
x_common, P_common, startx1, modem_loc, data, R)
# Don't add to shared_buffer
elif meas_type == "sonar_range":
pred, H = KalmanFilter._predict_range(
x_common, startx1, startx2)
innovation = data - pred
delta = delta_dict["sonar_range"] * mult
# Fuse explicitly
if abs(innovation) > delta:
x_common, P_common = KalmanFilter._fuse_range_tracked(
x_common, P_common, startx1, startx2, data, R)
type_ind = MEAS_TYPES_INDICES.index("sonar_range")
meas_row = [
type_ind, index, startx1, startx2, data, R
]
share_buffer.append(meas_row)
else: # Fuse implicitly
x_ref = x_common
h_x_hat = pred
h_x_bar, H_ = KalmanFilter._predict_range(
x_common_bar, startx1, startx2)
h_x_ref, H_ = KalmanFilter._predict_range(
x_ref, startx1, startx2)
x_common, P_common = KalmanFilter._implicit_fuse(
x_common_bar, P_common_bar, x_common, P_common,
x_ref, H, R, delta, h_x_hat, h_x_bar, h_x_ref,
False)
np.linalg.matrix_rank(
P_common) # Just check matrix isn't singular
type_ind = MEAS_TYPES_INDICES.index(
"sonar_range_implicit")
meas_row = [
type_ind, index, startx1, startx2, 0.0, R
]
share_buffer.append(meas_row)
elif meas_type == "sonar_azimuth":
pred, H = KalmanFilter._predict_azimuth(
x_common, startx1, startx2)
innovation = normalize_angle(data - pred)
delta = delta_dict["sonar_azimuth"] * mult
# Fuse explicitly
if abs(innovation) > delta:
x_common, P_common = KalmanFilter._fuse_azimuth_tracked(
x_common, P_common, startx1, startx2, data, R)
type_ind = MEAS_TYPES_INDICES.index(
"sonar_azimuth")
meas_row = [
type_ind, index, startx1, startx2, data, R
]
share_buffer.append(meas_row)
else: # Fuse implicitly
x_ref = x_common
h_x_hat = pred
h_x_bar, H_ = KalmanFilter._predict_azimuth(
x_common_bar, startx1, startx2)
h_x_ref, H_ = KalmanFilter._predict_azimuth(
x_ref, startx1, startx2)
x_common, P_common = KalmanFilter._implicit_fuse(
x_common_bar, P_common_bar, x_common, P_common,
x_ref, H, R, delta, h_x_hat, h_x_bar, h_x_ref,
True)
np.linalg.matrix_rank(
P_common) # Just check matrix isn't singular
type_ind = MEAS_TYPES_INDICES.index(
"sonar_azimuth_implicit")
meas_row = [
type_ind, index, startx1, startx2, 0.0, R
]
share_buffer.append(meas_row)
else:
raise ValueError("Unrecognized measurement type: " +
str(meas_type))
if not share_buffer: # We didn't take any shareable measurements!
return mults[0], [], 0, 0
# Check if share_buffer overflowed
share_buffer_mat = KalmanFilter._get_ledger_mat(share_buffer)
cost, explicit_cnt, implicit_cnt = self._get_buffer_size(
share_buffer_mat)
if cost > buffer_size:
if len(mults) == 1: # There were no matches!
raise Exception("There were no matching deltatiers!")
mults = mults[middle_index + 1:]
else:
if len(mults) == 1: # We've found our multiplier!
break
mults = mults[:middle_index + 1]
# Pick the middle delta
middle_index = int(len(mults) / 2) - 1
mult = mults[middle_index]
self.x_common = x_common
self.P_common = P_common
self.last_share_index = self.index + 1 # Don't share this index again
return mult, share_buffer, explicit_cnt, implicit_cnt