def kalman_filter(self):
# for plotting
self.plotter = Plotter(self.times)
self.plotter.save_iteration_data(self, 0, None)
self.plotter.x_cov_times.append(0)
self.plotter.x_cov_vals.append(self.sigma[1 , 1])
for timestep in range(1,self.control_inputs.size):
c_input = self.control_inputs[0 , timestep]
c_input = np.reshape(c_input, (1,1))
z = self.measurements[0 , timestep]
# prediction
mu_bar = mm(self.A, self.mu) + mm(self.B, c_input)
sigma_bar = mm(self.A, mm(self.sigma, np.transpose(self.A))) + self.R
self.plotter.x_cov_times.append(self.times[timestep])
self.plotter.x_cov_vals.append(sigma_bar[1 , 1])
# correction
c_transpose = np.transpose(self.C)
matrix_one = mm(sigma_bar, c_transpose)
matrix_two = mat_inv(mm(self.C, mm(sigma_bar, c_transpose)) + self.Q)
k = mm(matrix_one, matrix_two)
mu = mu_bar + mm(k, z - mm(self.C, mu_bar))
sigma = mm(np.identity(k.shape[0]) - mm(k, self.C), sigma_bar)
# update the model's belief for the next filter iteration
self.mu = mu
self.sigma = sigma
self.plotter.save_iteration_data(self, timestep, k)
self.plotter.x_cov_times.append(self.times[timestep])
self.plotter.x_cov_vals.append(sigma[1 , 1])
def calc_prefilter(a_mat, b_mat, c_mat, k_mat=None):
"""
Calculate the prefilter matrix
.. math::
\\boldsymbol{V} = - \\left[\\boldsymbol{C} \\left(\\boldsymbol{A} -
\\boldsymbol{B}\\boldsymbol{K}\\right)^{-1}\\boldsymbol{B}\\right]^{-1}
Args:
a_mat (:obj:`numpy.ndarray`): system matrix
b_mat (:obj:`numpy.ndarray`): input matrix
c_mat (:obj:`numpy.ndarray`): output matrix
k_mat (:obj:`numpy.ndarray`): control matrix
Return:
:obj:`numpy.ndarray`: Prefilter matrix
"""
a_mat = np.atleast_2d(a_mat)
b_mat = np.atleast_2d(b_mat)
c_mat = np.atleast_2d(c_mat)
k_mat = np.atleast_2d(k_mat)
# check dimension of matrices A, B and C
if a_mat.shape[0] != a_mat.shape[1]:
raise ValueError("A is not square")
if a_mat.shape[0] != b_mat.shape[0]:
raise ValueError("Dimension of A and B does not match")
if a_mat.shape[0] < b_mat.shape[1]:
raise ValueError("Dimension of A and B does not match")
if a_mat.shape[0] != c_mat.shape[1]:
raise ValueError("Dimension of A and C does not match")
if a_mat.shape[0] < c_mat.shape[0]:
raise ValueError("Dimension of A and C does not match")
if k_mat[0, 0] is not None:
try:
# prefilter: V = -[C(A-BK)^-1*B]^-1
v = -mat_inv(c_mat @ (mat_inv(a_mat - b_mat @ k_mat) @ b_mat))
except np.linalg.linalg.LinAlgError:
raise ValueError("Cannot calculate V due to singularity.")
else:
v = np.array([[1]])
return v
def calc_prefilter(a_mat, b_mat, c_mat, k_mat=None):
"""
Calculate the prefilter matrix
.. math::
\\boldsymbol{V} = - \\left[\\boldsymbol{C} \\left(\\boldsymbol{A} -
\\boldsymbol{B}\\boldsymbol{K}\\right)^{-1}\\boldsymbol{B}\\right]^{-1}
Args:
a_mat (:obj:`numpy.ndarray`): system matrix
b_mat (:obj:`numpy.ndarray`): input matrix
c_mat (:obj:`numpy.ndarray`): output matrix
k_mat (:obj:`numpy.ndarray`): control matrix
Return:
:obj:`numpy.ndarray`: Prefilter matrix
"""
a_mat = np.atleast_2d(a_mat)
b_mat = np.atleast_2d(b_mat)
c_mat = np.atleast_2d(c_mat)
k_mat = np.atleast_2d(k_mat)
# check dimension of matrices A, B and C
if a_mat.shape[0] != a_mat.shape[1]:
raise ValueError("A is not square")
if a_mat.shape[0] != b_mat.shape[0]:
raise ValueError("Dimension of A and B does not match")
if a_mat.shape[0] < b_mat.shape[1]:
raise ValueError("Dimension of A and B does not match")
if a_mat.shape[0] != c_mat.shape[1]:
raise ValueError("Dimension of A and C does not match")
if a_mat.shape[0] < c_mat.shape[0]:
raise ValueError("Dimension of A and C does not match")
if k_mat[0, 0] is not None:
try:
# prefilter: V = -[C(A-BK)^-1*B]^-1
v = -mat_inv(c_mat @ (mat_inv(a_mat - b_mat @ k_mat) @ b_mat))
except np.linalg.linalg.LinAlgError:
raise ValueError("Cannot calculate V due to singularity.")
else:
v = np.array([[1]])
return v
def __getitem__(self, index):
start = self.batch_size * index
end = min(len(self.data), start + self.batch_size)
size = end - start
a = np.zeros((size, ) + config.img_shape, dtype=K.floatx())
b = np.zeros((size, 4), dtype=K.floatx())
for i, (p, coords) in enumerate(self.data[start:end]):
img, trans = read_for_training(p)
coords = coord_transform(coords, mat_inv(trans))
x0, y0, x1, y1 = bounding_rectangle(coords, img.shape)
a[i, :, :, :] = img
b[i, 0] = x0
b[i, 1] = y0
b[i, 2] = x1
b[i, 3] = y1
return a, b
return a, b
def __len__(self):
return (len(train) + self.batch_size - 1) // self.batch_size
val_a = np.zeros((len(val), ) + img_shape,
dtype=K.floatx()) # Preprocess validation images
val_b = np.zeros((len(val), 4), dtype=K.floatx()) # Preprocess bounding boxes
for i, (p, coords) in enumerate(tqdm(val)):
try:
img, trans = read_for_validation(p)
except FileNotFoundError:
print("file not found")
continue
coords = coord_transform(coords, mat_inv(trans))
x0, y0, x1, y1 = bounding_rectangle(coords)
val_a[i, :, :, :] = img
val_b[i, 0] = x0
val_b[i, 1] = y0
val_b[i, 2] = x1
val_b[i, 3] = y1
def build_model(with_dropout=True):
kwargs = {'activation': 'relu', 'padding': 'same'}
conv_drop = 0.2
dense_drop = 0.5
inp = Input(shape=img_shape)
x = inp
def main():
data = get_train_data()
# Train data
train, val = train_test_split(data, test_size=200, random_state=1)
train += train
train += train
train += train
len(train),len(val)
# Valid data
val_a = np.zeros((len(val),)+config.img_shape,
dtype=K.floatx()) # Preprocess validation images
val_b = np.zeros((len(val),4),dtype=K.floatx()) # Preprocess bounding boxes
for i,(p,coords) in enumerate(tqdm(val)):
img,trans = read_for_validation(p)
coords = coord_transform(coords, mat_inv(trans))
x0,y0,x1,y1 = bounding_rectangle(coords, img.shape)
val_a[i,:,:,:] = img
val_b[i,0] = x0
val_b[i,1] = y0
val_b[i,2] = x1
val_b[i,3] = y1
# Train using cyclic learning rate
for num in range(1, 4):
model_name = 'cropping-%01d.h5' % num
model = build_model()
print(model_name)
model.compile(Adam(lr=0.032), loss='mean_squared_error')
model.fit_generator(
TrainingData(train), epochs=50, max_queue_size=12, workers=4, verbose=1,
validation_data=(val_a, val_b),
callbacks=[
EarlyStopping(monitor='val_loss', patience=9, min_delta=0.1, verbose=1),
ReduceLROnPlateau(monitor='val_loss', patience=3, min_delta=0.1, factor=0.25, min_lr=0.002, verbose=1),
ModelCheckpoint(model_name, save_best_only=True, save_weights_only=True),
])
model.load_weights(model_name)
model.evaluate(val_a, val_b, verbose=0)
# Now choose which model to use
model.load_weights('cropping-1.h5')
loss1 = model.evaluate(val_a, val_b, verbose=0)
model.load_weights('cropping-2.h5')
loss2 = model.evaluate(val_a, val_b, verbose=0)
model.load_weights('cropping-3.h5')
loss3 = model.evaluate(val_a, val_b, verbose=0)
model_name = 'cropping-1.h5'
if loss2 <= loss1 and loss2 < loss3: model_name = 'cropping-2.h5'
if loss3 <= loss1 and loss3 <= loss2: model_name = 'cropping-3.h5'
model.load_weights(model_name)
# Variance normalization
model2 = build_model(with_dropout=False)
model2.load_weights(model_name)
model2.compile(Adam(lr=0.002), loss='mean_squared_error')
model2.evaluate(val_a, val_b, verbose=0)
# Recompute the mean and variance running average without dropout
for layer in model2.layers:
if not isinstance(layer, BatchNormalization):
layer.trainable = False
model2.compile(Adam(lr=0.002), loss='mean_squared_error')
model2.fit_generator(TrainingData(), epochs=1, max_queue_size=12, workers=6, verbose=1, validation_data=(val_a, val_b))
for layer in model2.layers:
if not isinstance(layer, BatchNormalization):
layer.trainable = True
model2.compile(Adam(lr=0.002), loss='mean_squared_error')
model2.save('cropping.model')
# Generate bounding boxes
tagged = [p for _,p,_ in pd.read_csv(config.train_csv).to_records()]
submit = [p for _,p,_ in pd.read_csv(config.sample_submission).to_records()]
join = tagged + submit
# If the picture is part of the bounding box dataset, use the golden value.
p2bb = {}
for i,(p,coords) in enumerate(data): p2bb[p] = bounding_rectangle(coords, read_raw_image(p).size)
len(p2bb)
# For other pictures, evaluate the model.
p2bb = {}
for p in tqdm(join):
if p not in p2bb:
img,trans = read_for_validation(p)
a = np.expand_dims(img, axis=0)
x0, y0, x1, y1 = model2.predict(a).squeeze()
(u0, v0),(u1, v1) = coord_transform([(x0,y0),(x1,y1)], trans)
img = read_raw_image(p)
u0 = max(u0, 0)
v0 = max(v0, 0)
u1 = min(u1, img.size[0])
v1 = min(v1, img.size[1])
p2bb[p] = (u0, v0, u1, v1)
with open('./input/cropping.csv', 'w') as f:
f.write('Image, x0, y0, x1, y1\n')
for p in p2bb:
u0, v0, u1, v1 = p2bb[p]
f.write('{},{},{},{},{}\n'.format(str(p),
str(u0),
str(v0),
str(u1),
str(v1)))
if not seen_lm[lm_idx, k]:
seen_lm[lm_idx, k] = True
# initialize mean
r = z_tr[0, 0]
bearing = z_tr[1, 0]
lm_x_bar = bel_x + (r * cos(bearing + bel_theta))
lm_y_bar = bel_y + (r * sin(bearing + bel_theta))
lm_loc_estimates_x[lm_idx, k] = lm_x_bar
lm_loc_estimates_y[lm_idx, k] = lm_y_bar
# calculate jacobian
diff_x = lm_x_bar - bel_x
diff_y = lm_y_bar - bel_y
H = np.array([[r * diff_x, r * diff_y], [-diff_y, diff_x]])
H *= 1 / (r * r)
# initialize covariance
H_inv = mat_inv(H)
sigma = mm(H_inv, mm(Q_t, np.transpose(H_inv)))
lm_sig_i = 2 * lm_idx
p_sig_i = 2 * k
lm_uncertanties[lm_sig_i:lm_sig_i + 2,
p_sig_i:p_sig_i + 2] = sigma
# default importance weight
weight *= p0
else:
# measurement prediction
lm_x_bar = lm_loc_estimates_x[lm_idx, k]
lm_y_bar = lm_loc_estimates_y[lm_idx, k]
diff_x = lm_x_bar - bel_x
diff_y = lm_y_bar - bel_y
q = (diff_x * diff_x) + (diff_y * diff_y)
r = np.sqrt(q)
np.transpose(meas_diff))
# (get the true measurement for the given landmark)
true_x = x_pos_true[0, t_step]
true_y = y_pos_true[0, t_step]
true_theta = theta_true[0, t_step]
z_true = np.zeros(z_hat.shape)
x_diff = lm_x[i] - true_x
y_diff = lm_y[i] - true_y
q = (x_diff * x_diff) + (y_diff * y_diff)
z_true[0, 0] = np.sqrt(q) + randn(scale=std_dev_range)
z_true[1, 0] = arctan2(
y_diff, x_diff) - true_theta + randn(scale=std_dev_bearing)
# update mean (belief) and covariance
K_t = mm(sigma_t, mat_inv(S_t))
mu = mu_bar + mm(K_t, z_true - z_hat)
sigma = sigma_bar - mm(K_t, mm(S_t, np.transpose(K_t)))
# save belief, covariances and kalman gains for plot later
mu_x[0, t_step] = mu[0, 0]
mu_y[0, t_step] = mu[1, 0]
mu_theta[0, t_step] = mu[2, 0]
bound_x.append(np.sqrt(sigma[0, 0]) * 2)
bound_y.append(np.sqrt(sigma[1, 1]) * 2)
bound_theta.append(np.sqrt(sigma[2, 2]) * 2)
k_r_x.append(K_t[0, 0])
k_r_y.append(K_t[1, 0])
k_r_theta.append(K_t[2, 0])
k_b_x.append(K_t[0, 1])
k_b_y.append(K_t[1, 1])
state = np.array([[-5], [0], [np.pi / 2]])
x_pred = [state[0, 0]]
y_pred = [state[1, 0]]
th_pred = [state[2, 0]]
# initial uncertainty & lists of parameter uncertanties (95% confidence interval)
sigma = np.array([
[1, 0, 0], # x
[0, 1, 0], # y
[0, 0, .1] # theta
])
bound_x = [np.sqrt(sigma[0, 0]) * 2]
bound_y = [np.sqrt(sigma[1, 1]) * 2]
bound_theta = [np.sqrt(sigma[2, 2]) * 2]
info_matrix = mat_inv(sigma)
info_vector = mm(info_matrix, state)
# noise for control inputs
std_dev_v = .15 # m/s
std_dev_om = .1 # rad/s
# noise for measurement
std_dev_range = .2 # m
std_dev_bearing = .1 # rad
var_range = std_dev_range * std_dev_range
var_bearing = std_dev_bearing * std_dev_bearing
Q_t = np.array([[var_range, 0], [0, var_bearing]])
# information vector information
info_x = [info_vector[0, 0]]
diff_y = m_j_y - real_y
q_true = (diff_x**2) + (diff_y**2)
z_true = np.array([[np.sqrt(q_true)],
[arctan2(diff_y, diff_x) - real_theta]])
z_true += make_noise(Q_t)
# figure out kalman gain for the given landmark and then update belief
diff_x = m_j_x - bel_x
diff_y = m_j_y - bel_y
q = (diff_x**2) + (diff_y**2)
z_hat = np.array([[np.sqrt(q)],
[arctan2(diff_y, diff_x) - bel_theta]])
H_t = np.array([[-diff_x / np.sqrt(q), -diff_y / np.sqrt(q), 0],
[diff_y / q, -diff_x / q, -1]])
S_t = mm(H_t, mm(sigma_bar, np.transpose(H_t))) + Q_t
K_t = mm(sigma_bar, mm(np.transpose(H_t), mat_inv(S_t)))
mu_bar = mu_bar + mm(K_t, z_true - z_hat)
sigma_bar = mm((np.identity(sigma_bar.shape[0]) - mm(K_t, H_t)),
sigma_bar)
# update belief
mu = mu_bar
sigma = sigma_bar
mu_x[0, i] = mu[0, 0]
mu_y[0, i] = mu[1, 0]
mu_theta[0, i] = mu[2, 0]
# save covariances and kalman gains for plot later
bound_x.append(np.sqrt(sigma[0, 0]) * 2)
bound_y.append(np.sqrt(sigma[1, 1]) * 2)
bound_theta.append(np.sqrt(sigma[2, 2]) * 2)