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