def test_kalman(): expected_states = np.array([ 0.35454545, 0.42380952, 0.44193548, 0.40487805, 0.3745098, 0.36557377, 0.36197183, 0.37654321, 0.38021978, 0.38712871 ]) expected_covariances = np.array([ 0.09090909, 0.04761905, 0.03225806, 0.02439024, 0.01960784, 0.01639344, 0.01408451, 0.01234568, 0.01098901, 0.00990099 ]) # set parameters Z = data A = np.array([[1]]) xk = np.array([[0]]) B = np.array([[0]]) U = np.zeros((len(Z), 1)) Pk = np.array([[1]]) H = np.array([[1]]) Q = 0 R = 0.1 kf = KalmanFilter(A=A, xk=xk, B=B, Pk=Pk, H=H, Q=Q, R=R) states, covariances = kf.run_filter(Z, U) np.testing.assert_allclose(states, expected_states, rtol=1e-06, atol=0) np.testing.assert_allclose(covariances, expected_covariances, rtol=1e-06, atol=0)
def __init__(self, d): self.initialized = False self.timestamp = 0 self.n = d['number_of_states'] self.P = d['initial_process_matrix'] self.F = d['inital_state_transition_matrix'] self.Q = d['initial_noise_matrix'] self.radar_R = d['radar_covariance_matrix'] self.lidar_R = d['lidar_covariance_matrix'] self.lidar_H = d['lidar_transition_matrix'] self.a = (d['acceleration_noise_x'], d['acceleration_noise_y']) self.kalmanFilter = KalmanFilter(self.n)
def __init__(self): self.mode = rospy.get_param("~mode", default=MODE.CMD_VEL_RGBD) rospy.loginfo("Using mode: " + str(self.mode)) if self.mode == MODE.CMD_VEL_SCAN or self.mode == MODE.CMD_VEL_RGBD: rospy.Subscriber("cmd_vel", Twist, self.on_cmdvel, queue_size=1) elif self.mode == MODE.POSE_SCAN or self.mode == MODE.POSE_RGBD: rospy.Subscriber("pose", PoseStamped, self.on_pose, queue_size=1) else: rospy.logerr("Unrecognized / invalid mode: " + self.mode) exit(1) rospy.Subscriber("scan", LaserScan, self.on_scan, queue_size=1) self.state_pub = rospy.Publisher("state_estimate", PoseWithCovarianceStamped, queue_size=1) self.tf_broadcaster = tf.TransformBroadcaster() outfile = rospy.get_param("~outfile", default="output.csv") self.outfile = open(outfile, "w") self.outfile.write("time, location, uncertainty\n") self.kfilter = KalmanFilter(INITIAL_BELIEF, INITIAL_UNCERTAINTY, STATE_EVOLUTION, MOTION_NOISE, SENSE_NOISE) self.prev_cmdvel_t = None self.prev_cmdvel = None self.cmdvel_count = 0 self.prev_pose = None self.pose_count = 0 self.scan_count = 0 self.prev_wall_loc = None
def kalman_filter(data, col_num, threshold, Q: float, EGM: bool, EGM_window_width=800, verbose=True): ''' ---------------- DESCRIPTION ---------------- Simple implementation of a Kalman filter based on: http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html ''' Measurement = effectiv_trans.gradient_filter(data=data, col_num=col_num, threshold=threshold, verbose=verbose) P = np.diag(np.array(1.0).reshape(-1)) F = np.matrix(1.0) H = F R = np.matrix(0.1**2) Q = Q G = np.matrix(1.0) Q = G * (G.T) * Q Z = np.matrix(Measurement[0]) X = Z kf = KalmanFilter(X, P, F, Q, Z, H, R) X_ = [X[0, 0]] for i in tqdm(range(1, len(Measurement)), desc="Kalman filter...", ascii=False, ncols=75): # Predict (X, P) = kf.predict(X, P, w=0) # Update (X, P) = kf.update(X, P, Z) Z = np.matrix(Measurement[i]) X_.append(X[0, 0]) if EGM: EGM = [X_[0]] n = EGM_window_width for i in tqdm(range(1, len(Measurement) - n + 1), desc="EGM filter...", ascii=False, ncols=80): EGM.append(np.mean(X_[i:i + n])) EGM.extend(X_[len(Measurement) - n + 1:]) return EGM else: return X_
uwb_thread = threading.Thread(target=UWB_dis) uwb_thread.start() print('UWB thread start!') ang_range = 0.05 anc_dis = [0.55, 0.9] string_time = datetime.now().strftime("%H_%M_%S") data_filename = 'follw_data_' + string_time +'.txt' #------------kalman filter parameter-------------------- dt = 1.0/20 F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]]) H = np.array([1, 0, 0]).reshape(1, 3) Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]]) R = np.array([0.5]).reshape(1, 1) kf = KalmanFilter(F = F, H = H, Q = Q, R = R) def _main(): #file_num = int(input('experiment number: ')) try: print('Record initial distance !') time.sleep(2) if (len(dis_queue) > 0): dis_to_tag = dis_queue.popleft() if(dis_to_tag[0] != 0): ini_tag_dis = dis_to_tag[0] # ini_tag_dis = 35 print('--------------ini_tag_dis: ', ini_tag_dis) with open('ini_tag_dis_' + string_time +'.txt', 'a') as fout: json.dump({'time': [start_time], 'ini_tag_dis': [ini_tag_dis]}, fout)