def e_step(self): # initialize parameters transition_matrix = self.stacked_var_param observation_matrix = self.stacked_loading transition_covariance = self.stacked_factor_res_cov observation_covariance = self.obs_res_cov # e_step kf = KalmanFilter(transition_matrices=transition_matrix, observation_matrices=observation_matrix, observation_covariance=observation_covariance, transition_covariance=transition_covariance) #change input format observations = self.observations.T # estimate state with filtering and smoothing smoothed_state_mean, smoothed_state_covariances, smoothing_gain = kf.smooth( observations) # estimated mean and covariances [n_factor,n_time_step], [n_time_step,n_factor,n_factor] self.stacked_factor = smoothed_state_mean.T self.smoothed_state_cov = smoothed_state_covariances #estimated P_{t,t-1}|T self.pairwise_cov = self.pairwise_covariance( smoothed_state_covariances, smoothing_gain)
def __init__(self, config, video_helper): # fps: frame per second self.num_misses = 0 self.max_misses = config.MAX_NUM_MISSING_PERMISSION self.has_match = False # flags: self.delete....... self.kalman = KalmanFilter(video_helper)
def __init__(self, detection, trackID): super(Tracks, self).__init__() self.KF = KalmanFilter() self.KF.predict() self.KF.correct(np.matrix(detection).reshape(3, 1)) self.trace = deque() self.prediction = detection.reshape(1,3) self.trackID = trackID self.skipped = 0
def kinematic_kf(dim, order, dt=1.): """ Returns a KalmanFilter using newtonian kinematics for an arbitrary number of dimensions and order. So, for example, a constant velocity filter in 3D space would be created with kinematic_kf(3, 1) which will set the state `x` to be interpreted as [x, x', y, y', z, z'].T As another example, a 2D constant jerk is created with kinematic_kf(2, 3) Assumes that the measurement z is position in each dimension. If this is not true you will have to alter the H matrix by hand. P, Q, R are all set to the Identity matrix. H is assigned assuming the measurement is position, one per dimension `dim`. Parameters ---------- dim : int number of dimensions order : int, >= 1 order of the filter. 2 would be a const acceleration model. dim_z : int, default 1 size of z vector *per* dimension `dim`. Normally should be 1 dt : float, default 1.0 Time step. Used to create the state transition matrix """ kf = KalmanFilter(dim*order, dim) F = kinematic_state_transition(order, dt) diag = [F] * dim kf.F = sp.linalg.block_diag(*diag) kf.H = np.zeros((dim, dim*order)) for i in range(dim): kf.H[i, i * order] = 1. return kf
def run(self, img): """ Parameters: img : array(shape=(n, 2)) points in the form of (x, y) Returns: labels : array(shape=(n, 1)) labels of every points, returned from DBSCAN. """ labels = extract_patterns(img) n = np.max(labels) x_m = [] # measurements of x y_m = [] for i in range(n): cluster = img[labels == i] x_m.append(np.average(cluster[:, 0])) y_m.append(np.average(cluster[:, 1])) if self.tracks == []: # initialize kalman filters for i in range(n): x0 = np.array([x_m[i], 0]).reshape(2, 1) y0 = np.array([y_m[i], 0]).reshape(2, 1) kf_x = KalmanFilter(self.F, self.H, self.Q, self.R, P=self.P.copy(), x0=x0) kf_y = KalmanFilter(self.F, self.H, self.Q, self.R, P=self.P.copy(), x0=y0) track = Track(kf_x, kf_y) self.tracks.append(track) else: for track in self.tracks: track.predict() row_ind, col_ind = self.associate_track(x_m, y_m) for i, j in zip(row_ind, col_ind): self.tracks[i].correct(x_m[j], y_m[j]) self.maintain_track(row_ind, col_ind) # add(initialize) the tracks if any observations(measurements) are # not assigned with a tracks. for i in set(range(len(x_m))).difference(col_ind): x0 = np.array([x_m[i], 0]).reshape(2, 1) y0 = np.array([y_m[i], 0]).reshape(2, 1) kf_x = KalmanFilter(self.F, self.H, self.Q, self.R, P=self.P.copy(), x0=x0) kf_y = KalmanFilter(self.F, self.H, self.Q, self.R, P=self.P.copy(), x0=y0) track = Track(kf_x, kf_y) self.tracks.append(track) return labels
def __init__(self): # each history entry is numpy array [frame_id, bbox_x1, bbox_y1, bbox_x2, bbox_y2, fvec_1...128] self.history = np.array([]) self.num_misses = 0 # num of missed assignments self.max_misses = const.MAX_NUM_MISSES_TRACK self.has_match = False self.delete_me = False # set for manual deletion self.kalman = KalmanFilter() color = tuple(np.random.random_integers(0, 255, size=3)) self.drawing_color = color self.predicted_next_bb = None self.LENGTH_ESTABLISHED = 1 # number of sequential detections before we consider it a track self.uid = uuid.uuid4()
def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2, prior=0.5, unknown=50.0, decay=0.00001, num_angles=1): """ bounds: the extends of the map, in m resolution: the size of each grid cell, in m """ assert(bounds[1] > bounds[0]) assert(bounds[3] > bounds[2]) assert(resolution > 0) numx = int(math.ceil((bounds[1] - bounds[0])/resolution)) numy = int(math.ceil((bounds[3] - bounds[2])/resolution)) tot_prod = int(numx*numy*num_angles) self.shape = (numy, numx, num_angles) self.bounds = bounds self.resolution = resolution big_covar = unknown #1000.0 small_covar = decay #10^-5 p_occ = logodds(prior) # C, R matrix will be changed for every measurement, these # are just place holder values A = (sp.eye(tot_prod, tot_prod)).tocsr() B = (sp.eye(tot_prod, 1, k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix C = (sp.eye(1,tot_prod,k=-1)).tocsr() x0 = p_occ*numpy.ones((tot_prod,1)) P0 = (big_covar*sp.eye(tot_prod,tot_prod)).tocsr() Q = (small_covar*sp.eye(tot_prod,tot_prod)).tocsr() R = (sp.eye(tot_prod, tot_prod)).tocsr() self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R)
def Demo(data): try: # check if the file exists in the directory named as demo.json if os.path.exists("demo.json"): # if yes open the file and read the data with open('demo.json') as file: data = json.load(file) else: # store the data into a file so that we don't have to request for the data again with open('demo.json', 'w') as file: json.dump(data, file, indent=4) except: sys.exit("There was an Error loading the data") # temp list to store the data m = [] for i in data['Time Series (5min)'].values(): t = np.array([float(i['4. close'])], dtype='float64') m.append(t[0]) # as the data is store in desending order we will reverse it measurements = m[::-1] # initialise the kalman filter kalman = KalmanFilter(closePrice=measurements[0], dt=300) # to store the predictions predictions = [] # to predict the next state store it and update the filter for value in measurements: kalman.update(value) predictions.append(kalman.predict()[0][0]) # calculate the r2 score coefficient_of_dermination = r2_score(measurements, predictions) print(f'R2 score of the filter {coefficient_of_dermination}') plt.figure(figsize=(10, 5)) plt.plot(measurements, label='measurements', marker='*', color="red") plt.plot(predictions, label='prediction', marker=".", color="blue") plt.suptitle("Kalman filter predictions") plt.title( "Intially the filter prediction has noise so prediction can divert") plt.legend() plt.show()
class Instance(object): def __init__(self, config, video_helper): # fps: frame per second self.num_misses = 0 self.max_misses = config.MAX_NUM_MISSING_PERMISSION self.has_match = False # flags: self.delete....... self.kalman = KalmanFilter(video_helper) # self.history def add_to_track(self, tag, bbox): corrected_bbox = self.kalman.correct(bbox) # self.history.append(corrected_bbox) def get_predicted_bbox(self): # get a prediction return self.kalman.get_predicted_bbx()
def __init__(self, config, video_helper, frame): #each history entry is an array with [frame_id, tag, bbx_left, bbx_right, bbx_up, bbx_down] self.history = [] self.history_size = config.HISTORY_SIZE = 10 self.face_id = 'None' # 人脸识别 self.his_face_id = 'None' # 记录上一次预测的人脸ID self.emotion = 'None' # 表情识别 self.his_emotion = 'None' # 记录上一次表情 self.detector = config.detector self.num_misses = 0 # num of missed assignments self.max_misses = config.MAX_NUM_MISSING_PERMISSION self.has_match = False self.delete_duplicate = False self.delete_still = False self.delete_singular = False self.num_of_still = 0 # num of detector num self.kcf = KcfFilter(video_helper, frame) # video_helper here can provide us the fps self.kalman = KalmanFilter(video_helper) # this color is for bbx (color assigned to this instance itself) color = np.random.randint(0, 255, (1, 3))[0] self.color = [int(color[0]), int(color[1]), int(color[2])] # color needs to be a tuple # this color is for central point # because we need to fade the color self.center_color = [] self.center_color.append([int(color[0]), int(color[1]), int(color[2])]) self.predicted_next_bbx = None self.num_established = 1 # num of sequential detections before we consider it a track self.COLOR_FADING_PARAM = config.COLOR_FADING_PARAM # we still need to change it later self.speed = 0 self.direction = 0 # degree of velocity direction with [1, 0] self.still_history = 0
def init(): print("Initializing WiFi AP...") ap = network.WLAN(network.AP_IF) ap.active(True) print("WiFi AP initialized") print(peltier_exact.scan()) print("Setting resolutions...") peltier_exact.set_resolution(12) """peltier_exact.set_resolution(12) incubator_fast.set_resolution(11) incubator_exact.set_resolution(12)""" print("Resolutions set") print("Initializing temperature sensors...") peltier_exact.begin_reading() """peltier_exact.init_read() incubator_fast.init_read() incubator_exact.init_read()""" print("Temperature sensors initialized") print("Waiting for first readings...") utime.sleep_ms(750) print("Wait over.") peltier_exact_z = peltier_exact.begin_reading() print("Initializing Kalman models...") peltier_model = KalmanFilter(peltier_exact_z, 1) incubator_model = KalmanFilter(peltier_exact_z, 1) print("Initializing control loop timers...") fast_timer.init(period=380, mode=machine.Timer.PERIODIC, callback=fast_timer_IRQ) exact_timer.init(period=755, mode=machine.Timer.PERIODIC, callback=exact_timer_IRQ) kalman_timer.init(period=500, mode=machine.Timer.PERIODIC, callback=kalman_timer_IRQ) print("Control loop timers initialized")
def main(): # keep the track the current day transactions transactions = LoadTransactions() # initiate kalman filter for all STOCKS and store the previous closing prices stock_data = dict() # For first 10 iterations we won't predict and let the kalman filter to reduce the noise # But we will update the kalman filter iterations = 0 for stock in STOCKS: stock_price = array([GetLiveStockData(stock)], dtype='float64') # initial Close price = 0 stock_data[stock] = KalmanFilter(closePrice=stock_price[0], dt=300) while (True): for stock in STOCKS: # get the live price of the stock price = array([GetLiveStockData(stock)], dtype="float64") price = price[0] if transactions['onhold'].get(stock) != None: # update the kalman filter to the current state stock_data[stock].update(price) # predict the next state prediction = stock_data[stock].predict()[0][0] # the bought at price for the stock boughtAt = transactions['onhold'][stock]["bought_at"] if ((prediction < price) and (boughtAt < prediction)) or ((boughtAt > prediction) and (prediction < price)): transactions = sell(transactions, stock, price) WriteTransactions(transactions) else: # update the kalman filter to current state stock_data[stock].update(price) # make the predictions using the kalman filter predicted_price = stock_data[stock].predict()[0][0] # for first 10 iterations we will reduce the noise in kalman filterr for better predictions if (predicted_price - price) > 0 and iterations > 10: # sold_at -1 means it is not sold yet or we don't know the selling price transactions = buy(transactions, stock, { "bought_at": price, "sold_at": -1 }) # we will wait for 5 minutes sleep(300) iterations += 1 print(f"Amount {Transaction.amount} at {GetDateTime()['time']}")
class Tracker(object): num_objects = 0 def __init__(self, init_loc): self.X = np.zeros((dim_x,1)) self.miss_frame_count = 0 self.object_id = num_objects num_objects += 1 self.X[:4,1] = init_loc A = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],[0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]]) H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]]) self.kf = KalmanFilter(dim_x=7,dim_z=4,A=A,H=H)
def predict_kalman_seq(seq, feat): # load the model with gzip.open(os.path.join('data', 'models', 'ETR_1_6_3_300.pklz'), 'rb') as fd: rf = pickle.load(fd) X = np.array(feat) y_pred = rf.predict(X) # for i, tree_ in enumerate(rf.estimators_): # with open('tree_' + str(i) + '.dot', 'w') as dotfile: # tree.export_graphviz(tree_, dotfile) x = np.array([[0.], [0.]]) # initial state (location and velocity) P = np.array([[1000., 0.], [0., 1000.]]) # initial uncertainty kf = KalmanFilter() x_out = [x] xabs = 0 for measurement in y_pred: xabs = xabs + measurement x, P = kf.step(x, P, xabs) x_out.append(x) return x_out
def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2, prior=0.5, unknown=50.0, decay=0.00001, num_angles=1): """ bounds: the extends of the map, in m resolution: the size of each grid cell, in m """ assert (bounds[1] > bounds[0]) assert (bounds[3] > bounds[2]) assert (resolution > 0) numx = int(math.ceil((bounds[1] - bounds[0]) / resolution)) numy = int(math.ceil((bounds[3] - bounds[2]) / resolution)) tot_prod = int(numx * numy * num_angles) self.shape = (numy, numx, num_angles) self.bounds = bounds self.resolution = resolution big_covar = unknown #1000.0 small_covar = decay #10^-5 p_occ = logodds(prior) # C, R matrix will be changed for every measurement, these # are just place holder values A = (sp.eye(tot_prod, tot_prod)).tocsr() B = (sp.eye( tot_prod, 1, k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix C = (sp.eye(1, tot_prod, k=-1)).tocsr() x0 = p_occ * numpy.ones((tot_prod, 1)) P0 = (big_covar * sp.eye(tot_prod, tot_prod)).tocsr() Q = (small_covar * sp.eye(tot_prod, tot_prod)).tocsr() R = (sp.eye(tot_prod, tot_prod)).tocsr() self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R)
def initialize_kalman(self, x=0, vx=0, y=0, vy=0, z=0, vz=0, p=10, q=1, r=1): F = lambda dt: np.matrix([ [1, dt, 0, 0, 0, 0], # x' = x + vx*dt [0, 1, 0, 0, 0, 0], # vx' = vx [0, 0, 1, dt, 0, 0], # y' = y + vy*dt [0, 0, 0, 1, 0, 0], # vy' = vy [0, 0, 0, 0, 1, dt], # z' = z + vz*dt [0, 0, 0, 0, 0, 1], # vz' = vz ]) # Control model B = 0 # Measurement model H = np.matrix([ [1, 0, 0, 0, 0, 0], # x [0, 0, 1, 0, 0, 0], # y [0, 0, 0, 0, 1, 0], # z ]) # Initial state vector x0 = np.matrix([x, vx, y, vy, z, vz]).T # Initial state covariance P0 = np.matrix(np.eye(6)) * p # Process error covariance (continuous white noise acceleration model) Q = lambda dt: np.matrix([ [dt**3 / 3.0, dt**2 / 2.0, 0, 0, 0, 0], [dt**2 / 2.0, dt, 0, 0, 0, 0], [0, 0, dt**3 / 3.0, dt**2 / 2.0, 0, 0], [0, 0, dt**2 / 2.0, dt, 0, 0], [0, 0, 0, 0, dt**3 / 3.0, dt**2 / 2.0], [0, 0, 0, 0, dt**2 / 2.0, dt], ]) * q # Measurement error covariance R = np.matrix(np.eye(3)) * r self.tracker = KalmanFilter(F, B, H, x0, P0, Q, R)
""" Uses kalman filter to predict subsequent detection bounding boxes """ import cv2 from ball_detector import BallDetector from kalman import KalmanFilter detector = BallDetector() # detector = CarDetector() kf = KalmanFilter() video_filename = '/home/bob/datasets/video/TrackBalls/2017-05-12-112517.webm' record_flag = False if record_flag: # declare video writer obj fourcc = cv2.VideoWriter_fourcc(*'XVID') out1 = cv2.VideoWriter('/home/bob/datasets/video/TrackBalls/Tracker_balls/1.avi', fourcc, 20.0, (1920, 1080)) vc = cv2.VideoCapture() vc.open(video_filename) scale_factor = 0.75 # reduce frame by this size tracked_bbox = [] # a list of past bboxes max_num_missed_det = 5 num_missed_det = 0 while True: _, img = vc.read() if img is None: break img = cv2.resize(img, (int(img.shape[1]*scale_factor), int(img.shape[0]*scale_factor))) # detect ball in frame (just one for now)
#!/usr/bin/env python from robot import Robot from kalman import KalmanFilter if __name__ == '__main__': robot = Robot() filter = KalmanFilter(10.0, 10.0, 10.0, 10.0, 10.0, 10.0) for i in xrange(1000): robot.move(1.0) filter.update(1.0, robot.sensor()) # Print the robot's actual position vs the filter estimate print robot.x, filter.mean
class Track(object): def __init__(self): # each history entry is numpy array [frame_id, bbox_x1, bbox_y1, bbox_x2, bbox_y2, fvec_1...128] self.history = np.array([]) self.num_misses = 0 # num of missed assignments self.max_misses = const.MAX_NUM_MISSES_TRACK self.has_match = False self.delete_me = False # set for manual deletion self.kalman = KalmanFilter() color = tuple(np.random.random_integers(0, 255, size=3)) self.drawing_color = color self.predicted_next_bb = None self.LENGTH_ESTABLISHED = 1 # number of sequential detections before we consider it a track self.uid = uuid.uuid4() def get_length(self): return self.history.shape[0] def is_singular(self): return True if self.history.shape[ 0] == self.LENGTH_ESTABLISHED else False def is_established(self): return True if self.history.shape[ 0] > self.LENGTH_ESTABLISHED else False def is_empty(self): return True if self.history.shape[0] == 0 else False def is_dead(self): return True if (self.num_misses >= self.max_misses or self.delete_me is True) else False def propagate_track(self, frame_id): # propagate track as if there was a detection, perhaps object is temporarily occluded or not detected # use predicted bb as measurement det = Detection.det_from_numpy_array( self.history[self.history.shape[0] - 1]) det.bbox = self.get_predicted_next_bb() # TODO: adjust bbox to have same width height but only propegate x,y centroid position # pred_c_x, pred_c_y = util.centroid_from_bb(self.get_predicted_next_bb()) # wid, ht = util.wid_ht_from_bb(det.bbox) # det.bbox = np.array([pred_c_x - wid / 2, # pred_c_y - ht / 2, # pred_c_x + wid / 2, # pred_c_y + ht / 2], dtype=np.int32) det.frame_id = frame_id self.add_to_track(det) def get_predicted_next_bb(self): # # get a prediction using the latest history as a measurement # measurement = np.array([self.get_latest_bb()], dtype=np.float32).T return self.kalman.get_predicted_bb() def add_to_track(self, det): # use detection measurement to predict and correct the kalman filter corrected_bb = self.kalman.correct(det.bbox) # use corrected bbox det.bbox = corrected_bb # increment detections number of matches (could be assigned to several tracks, need to keep track of this) det.num_matches += 1 new_history = det.as_numpy_array() # print new_history if self.history.size > 0: self.history = np.vstack((self.history, new_history)) else: self.history = new_history def get_latest_fvec(self): # get feature vector from the latest detection in the track (already computed during detection phase) return self.history[self.history.shape[0] - 1][5:] def get_latest_bb(self): return self.history[self.history.shape[0] - 1][1:5] def draw_history(self, img, draw_at_bottom=False): if self.history.shape[0] > 1: bb_latest = self.get_latest_bb() cv2.rectangle(img, (int(bb_latest[0]), int(bb_latest[1])), (int(bb_latest[2]), int(bb_latest[3])), self.drawing_color, 2) # iterate through detection history prev_bb = self.history[0][1:5] for det in self.history: bb = det[1:5] # cv2.rectangle(img, tuple(bb[:2]),tuple(bb[2:]),(255,0,0),2) centroid = (int(bb[0] + (bb[2] - bb[0]) / 2), int(bb[1] + (bb[3] - bb[1]) / 2)) bottom = (int(bb[0] + (bb[2] - bb[0]) / 2), int(bb[3])) bottom_prev = (int(prev_bb[0] + (prev_bb[2] - prev_bb[0]) / 2), int(prev_bb[3])) # cv2.circle(img, centroid, 5, self.drawing_color, 2) # cv2.circle(img, bottom, 3, self.drawing_color, 2) cv2.line(img, bottom_prev, bottom, self.drawing_color, 4) prev_bb = bb
def __init__(self): self.lidar = HokuyoLX() self.kalman = KalmanFilter(adaptive=False, dt=0.025)
import utime #import picoweb import machine import network from temperature import DS18B20x from kalman import KalmanFilter from spark import Spark ESSID = "Incubator-1" #peltier_fast = DS18B20x(14, ) peltier_exact = DS18B20x(2, b'(D\x12-\t\x00\x00+') #incubator_fast = DS18B20x(16, 11) #incubator_exact = DS18B20x(17, 12) peltier_model = KalmanFilter(28, 400) incubator_model = KalmanFilter(28, 400) spark_controller = Spark(2) fast_timer = machine.Timer(0) exact_timer = machine.Timer(1) kalman_timer = machine.Timer(2) def disable_debug(): esp.osdebug(None) def fast_timer_IRQ(timer): pass
def main(): ## set root directory data_root = "../data/global_flow" save_root = "results/global_flow" # make directory if not os.path.exists(save_root): os.mkdir(save_root) ## set data Tf = 1000 # length of time-series N = 30 # widht, height of images dtype = "float32" obs = xp.asarray(np.load(os.path.join(data_root, "obs.npy")), dtype=dtype) true_xp = xp.asarray(np.load(os.path.join(data_root, "true.npy")), dtype=dtype) ## set data for kalman filter bg_value = 20 skip = 1 # downsampling Nf = int(N / skip) # Number of lines after skip obs = obs[:Tf, ::skip, ::skip].reshape(Tf, -1) true_xp = true_xp[:Tf, ::skip, ::skip].reshape(Tf, -1) + bg_value boundary = xp.zeros((Nf, Nf), dtype=bool) boundary[0] = True boundary[-1] = True boundary[:, 0] = True boundary[:, -1] = True boundary = boundary.reshape(-1) ## set parameters d = 1 # number of adjacency element sigma_initial = 0 # standard deviation of normal distribution for random making update_interval = 10 # update interval for LSLOCK llock_update_interval = 50 # update interval for LLOCK estimation_mode = "forward" eta = 0.8 # learning rate cutoff = 1.0 # cutoff distance for update of transition matrix sigma = 0.2 # standard deviation of gaussian noise Q = sigma**2 * xp.eye(Nf * Nf) R = sigma**2 * xp.eye(Nf * Nf) # Nf x nlines ## record list mse_record = xp.zeros((2, 4, Tf)) mae_record = xp.zeros((2, 4, Tf)) time_record = xp.zeros((2, 3)) all_start_time = time.time() ### Execute F_initial = xp.eye(Nf * Nf) # identity A = xp.asarray(parametric_matrix_cyclic_2d(Nf, d), dtype="int32") ## Kalman Filter filtered_value = xp.zeros((Tf, Nf * Nf)) kf = KalmanFilter(transition_matrix=F_initial, transition_covariance=Q, observation_covariance=R, initial_mean=obs[0], dtype=dtype) for t in range(Tf): filtered_value[t] = kf.forward_update(t, obs[t], return_on=True) xp.save(os.path.join(save_root, "kf_states.npy"), filtered_value) ## LLOCK llock_save_dir = os.path.join(save_root, "llock") if not os.path.exists(llock_save_dir): os.mkdir(llock_save_dir) print("LLOCK : d={}, update_interval={}, eta={}, cutoff={}".format( d, llock_update_interval, eta, cutoff)) llock = LocalLOCK(observation=obs, transition_matrix=F_initial, transition_covariance=Q, observation_covariance=R, initial_mean=obs[0], adjacency_matrix=A, dtype=dtype, estimation_length=llock_update_interval, estimation_interval=llock_update_interval, eta=eta, cutoff=cutoff, save_dir=llock_save_dir, estimation_mode=estimation_mode, use_gpu=False) start_time = time.time() llock.forward() time_record[0, 0] = time.time() - start_time time_record[0, 1] = llock.times[3] time_record[0, 2] = llock.times[3] / llock.times[4] print("LLOCK times : {}".format(time.time() - start_time)) ## LSLOCK lslock_save_dir = os.path.join(save_root, "lslock") if not os.path.exists(lslock_save_dir): os.mkdir(lslock_save_dir) print("LSLOCK : d={}, update_interval={}, eta={}, cutoff={}".format( d, update_interval, eta, cutoff)) lslock = LSLOCK(observation=obs, transition_matrix=F_initial, transition_covariance=Q, observation_covariance=R, initial_mean=obs[0], parameter_matrix=A, dtype=dtype, estimation_length=update_interval, estimation_interval=update_interval, eta=eta, cutoff=cutoff, save_dir=lslock_save_dir, estimation_mode=estimation_mode, method="gridwise", use_gpu=False) start_time = time.time() lslock.forward() time_record[1, 0] = time.time() - start_time time_record[1, 1] = lslock.times[3] time_record[1, 2] = lslock.times[3] / lslock.times[4] print("LSLOCK times : {}".format(time.time() - start_time)) # record error infromation area_list = [xp.ones((Nf * Nf), dtype=bool), ~boundary] for r, area in enumerate(area_list): for t in range(Tf): mse_record[r, 0, t] = mean_squared_error( lslock.get_filtered_value()[t][area], true_xp[t][area]) mae_record[r, 0, t] = mean_absolute_error( lslock.get_filtered_value()[t][area], true_xp[t][area]) mse_record[r, 1, t] = mean_squared_error( llock.get_filtered_value()[t][area], true_xp[t][area]) mae_record[r, 1, t] = mean_absolute_error( llock.get_filtered_value()[t][area], true_xp[t][area]) mse_record[r, 2, t] = mean_squared_error(filtered_value[t][area], true_xp[t][area]) mae_record[r, 2, t] = mean_absolute_error(filtered_value[t][area], true_xp[t][area]) mse_record[r, 3, t] = mean_squared_error(obs[t][area], true_xp[t][area]) mae_record[r, 3, t] = mean_absolute_error(obs[t][area], true_xp[t][area]) ## save error-record if True: xp.save(os.path.join(save_root, "time_record.npy"), time_record) xp.save(os.path.join(save_root, "mse_record.npy"), mse_record) xp.save(os.path.join(save_root, "mae_record.npy"), mae_record) # mse_record = np.load(os.path.join(save_root, "mse_record.npy")) fig, ax = plt.subplots(1, 1, figsize=(8, 5)) for i, label in enumerate(["LSLOCK", "LLOCK", "KF", "observation"]): ax.plot(np.sqrt(mse_record[0, i]), label=label, lw=2) ax.set_xlabel("Timestep", fontsize=12) ax.set_ylabel("RMSE", fontsize=12) ax.legend(fontsize=15) fig.savefig(os.path.join(save_root, "rmse.png"), bbox_to_inches="tight") ## short-term prediction color_list = ["r", "g", "b", "m", "y"] threshold = 200 pred_state = xp.zeros((Tf, Nf * Nf)) llock_pred_state = xp.zeros((Tf, Nf * Nf)) pred_mse = mse_record[0].copy() s = threshold // update_interval F = np.load( os.path.join(lslock_save_dir, "transition_matrix_{:03}.npy".format(s))) state = np.load(os.path.join(lslock_save_dir, "states.npy"))[threshold] pred_state[threshold] = state.reshape(-1) for t in range(threshold, Tf - 1): pred_state[t + 1] = F @ pred_state[t] s = threshold // llock_update_interval F = np.load( os.path.join(llock_save_dir, "transition_matrix_{:02}.npy".format(s))) llock_state = np.load(os.path.join(llock_save_dir, "states.npy"))[threshold] llock_pred_state[threshold] = llock_state.reshape(-1) for t in range(threshold, Tf - 1): llock_pred_state[t + 1] = F @ llock_pred_state[t] kf_state = np.load(os.path.join(save_root, "kf_states.npy"))[threshold] for t in range(threshold, Tf): pred_mse[0, t] = mean_squared_error(pred_state[t], true_xp[t]) pred_mse[1, t] = mean_squared_error(llock_pred_state[t], true_xp[t]) pred_mse[2, t] = mean_squared_error(kf_state.reshape(-1), true_xp[t]) pred_mse[3, t] = mean_squared_error(obs[threshold], true_xp[t]) convlstm_mse = np.load( os.path.join(save_root, "convlstm", "convlstm_mse.npy")) # epoch//save_epoch x 10 fig, ax = plt.subplots(1, 1, figsize=(8, 5)) low = threshold - 4 up = threshold + 6 lw = 2 ax.axvline(threshold, c="k", lw=lw, ls=":") for i, label in enumerate(["LSLOCK", "LLOCK", "KF", "observation"]): ax.plot(range(low, up), np.sqrt(pred_mse[i, low:up]), lw=lw, ls="--", c=color_list[i]) ax.plot(range(low, threshold + 1), np.sqrt(mse_record[0, i, low:threshold + 1]), label=label, lw=lw, c=color_list[i]) ax.plot(range(threshold, up), np.sqrt(convlstm_mse[400 // 50, :len(range(up - threshold))]), label="ConvLSTM", lw=lw, c=color_list[4]) ax.set_xlabel("Timestep", fontsize=12) ax.set_ylabel("RMSE", fontsize=12) ax.legend(bbox_to_anchor=(1.05, 1.0), loc="upper left", fontsize=15) fig.savefig(os.path.join(save_root, "prediction.png"), bbox_inches="tight") all_execute_time = int(time.time() - all_start_time) print("all time (sec): {} sec".format(all_execute_time)) print("all time (min): {} min".format(int(all_execute_time // 60))) print("all time (hour): {} hour + {} min".format( int(all_execute_time // 3600), int((all_execute_time // 60) % 60)))
#from data import measurements #from data import true_velocity from data_test_input import measurements from data_test_input import true_velocity from kalman import KalmanFilter from plot import plot_graphs dt = 0.1 A = np.array([[1, dt], [0, 1]]) # if we measure the velocity instead of position, # we have to modify the measurement matrix(C) from [1, 0] to [0, 1] C = np.array([[0, 1]]) R = np.array([[1, 0], [0, 3]]) Q = np.array([10]) KF = KalmanFilter(A, C, R, Q) filtered_positions = [] filtered_velocity = [] for m in measurements: x = KF.filter(m, dt) filtered_positions.append(x[0]) filtered_velocity.append(x[1]) plot_graphs(measurements, filtered_positions, true_velocity, filtered_velocity)
from data_logging.data_logger import DataLogger from detection.car_detector import CarDetector from detection.detection import Detection from kalman import KalmanFilter from multiple_object_tracker import MultipleObjectTracker # log the data to a csv file filename = 'bb_logs.csv' headers = ['time', 'id', 'x1', 'y1', 'x2', 'y2'] logger = DataLogger(filename=filename, headers=headers) tracker = MultipleObjectTracker() # detector = BallDetector() detector = CarDetector() kf = KalmanFilter() # video_filename = '/home/bob/datasets/video/TrackBalls/2017-05-12-112517.webm' video_filename = '/home/bob/datasets/video/MOVI0003.avi' save_video_filename = '/home/bob/datasets/video/MOVI0003_tracking.avi' record_flag = False vc = cv2.VideoCapture() vc.open(video_filename) scale_factor = 0.75 # reduce frame by this size tracked_bbox = [] # a list of past bboxes max_num_missed_det = 5 num_missed_det = 0 current_frame_position = 0 if record_flag:
from kalman import KalmanFilter if __name__ == "__main__": """ Demo implementation of a Kalman Filter For two hidden state variables x (position) and xv (velocity) with a constant acceleration """ # Initial estimates of position and velocity and (co)-variances stateNaut = OrderedDict([("position (m)", 4000), ("velocity (m/s)", 280)]) processCovarianceNaut = np.diag([400.0, 25.0]) # Create the Kalman Filter with the initial state # The filter implements the physics internally KF = KalmanFilter(stateNaut, processCovarianceNaut) # Add a list of observations to the filter observations = np.array([[4260, 282], [4550, 285], [4860, 286], [5110, 290]]) # And a covariance on the observations observationCovariance = np.diag([625, 36]) for observation in observations: KF.addObservation(observation, observationCovariance) print(KF.currentState) # Add observations as points plt.plot(observations, marker="X", label="_nolegend_", linewidth=0)
def __init__(self, ip="192.168.1.10", port=10940): self.lidar = HokuyoLX(addr=(ip, port)) self.kalman = KalmanFilter(adaptive=False, dt=0.025)
class OccupancyGrid(object): """ Occupancy grid type map """ def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2, prior=0.5, unknown=50.0, decay=0.00001, num_angles=1): """ bounds: the extends of the map, in m resolution: the size of each grid cell, in m """ assert(bounds[1] > bounds[0]) assert(bounds[3] > bounds[2]) assert(resolution > 0) numx = int(math.ceil((bounds[1] - bounds[0])/resolution)) numy = int(math.ceil((bounds[3] - bounds[2])/resolution)) tot_prod = int(numx*numy*num_angles) self.shape = (numy, numx, num_angles) self.bounds = bounds self.resolution = resolution big_covar = unknown #1000.0 small_covar = decay #10^-5 p_occ = logodds(prior) # C, R matrix will be changed for every measurement, these # are just place holder values A = (sp.eye(tot_prod, tot_prod)).tocsr() B = (sp.eye(tot_prod, 1, k=2)).tocsr() #k-diag is ones, but outside, so this is zero matrix C = (sp.eye(1,tot_prod,k=-1)).tocsr() x0 = p_occ*numpy.ones((tot_prod,1)) P0 = (big_covar*sp.eye(tot_prod,tot_prod)).tocsr() Q = (small_covar*sp.eye(tot_prod,tot_prod)).tocsr() R = (sp.eye(tot_prod, tot_prod)).tocsr() self.kf = KalmanFilter(A=A, B=B, C=C, x0=x0, P0=P0, Q=Q, R=R) def __call__(self, sonar_measurement): C = self.calc_C(sonar_measurement.indices) # Convert probabilities to log odds format first prob = logodds(sonar_measurement.prob) prob = prob[:, numpy.newaxis] n = sonar_measurement.var.shape[0] R = sp.spdiags(sonar_measurement.var, 0, n, n).tocsr() return self.kf.meas_update(prob, C, R) def time_update(self): self.kf.time_update() def calc_C(self, indices): nbr_y = numpy.size(indices,0) # C = numpy.zeros((nbr_y, numpy.prod(self.shape))) # for i in range(nbr_y): # # Convert to linear indexing # ind = indices[i, 1]*self.shape[0]+indices[i, 0] # C_old[i,ind] = 1 y = range(nbr_y) x = indices[:, 2]*self.shape[1]*self.shape[0] + indices[:, 1]*self.shape[0]+indices[:, 0] C_data = ((1,)*nbr_y, (y, x)) C_shape = (nbr_y, numpy.prod(self.shape)) C = sp.coo_matrix(C_data, shape=C_shape) return C.tocsr() def get_map(self, axis=None): # Return the mean of the estimates for each angle, # Needs to convert to array first, matrix types seem # to loose the single dimension during reshape tmp = numpy.reshape(numpy.asarray(self.kf.x_new), self.shape, order='F') #return inv_logodds(numpy.mean(tmp,2)) if (axis != None): return inv_logodds(tmp[:,:,axis]) return inv_logodds(numpy.max(tmp,2)) def get_variance(self, axis=None): tmp = self.kf.P.diagonal() tmp = numpy.reshape(tmp, self.shape, order='F') if (axis != None): return tmp[:,:,axis] return numpy.mean(tmp,2) #return numpy.min(tmp,2) def find_visible_cells(self, cone): """ Finds all cells centers of all grids within the cone specified state - (x,y,dir) prec - FOV width, summetric around dir dist - max distance to search """ def course_crop(dim, bounds, cone): """ Rectangular bounds on which indices need to be evaluated """ (xmin, xmax, ymin, ymax) = cone.get_bounding_rect() xleft = math.floor((dim[1]-1)* (xmin-bounds[0])/(bounds[1]-bounds[0])) xright = math.ceil((dim[1]-1)* (xmax-bounds[0])/(bounds[1]-bounds[0])) xleft = max((xleft, 1)) xright = min((xright, dim[1]-1)) yupp = math.floor((dim[0]-1)* (ymin-bounds[2])/(bounds[3]-bounds[2])) ylow = math.ceil((dim[0]-1)* (ymax-bounds[2])/(bounds[3]-bounds[2])) yupp = max((yupp, 1)) ylow = min((ylow, dim[0]-1)) return (int(xleft), int(xright), int(yupp), int(ylow)) (xle, xri, yup, ylo) = course_crop(self.shape, self.bounds, cone) # Evaluate each cell center for the FOV x_ind = numpy.arange(xle, xri+1, dtype=numpy.int) y_ind = numpy.arange(yup, ylo+1, dtype=numpy.int) x, y = numpy.meshgrid(x_ind*self.resolution+self.bounds[0], y_ind*self.resolution+self.bounds[2]) x_ind, y_ind = numpy.meshgrid(x_ind, y_ind) x = x.reshape((-1, 1)) y = y.reshape((-1, 1)) x_ind = x_ind.reshape((-1, 1)) y_ind = y_ind.reshape((-1, 1)) if (x.shape[0] == 0): return None # cells will contains (a,r,y_ind,x_ind) for all visible cells cells = cone(y[:, 0], x[:, 0], numpy.hstack([y_ind, x_ind])) if (cells == None): return None cells.sort(order='r', axis=0) return cells
#!/usr/bin/env python import os, sys import rosbag import matplotlib.pyplot as plt from kalman import SimpleKalmanFilter, KalmanFilter import numpy as np mainDir = os.path.split(os.path.split(sys.path[0])[0])[0] bagDir = mainDir + '/bag/' filename = sys.argv[1] bag = rosbag.Bag(bagDir + filename) tMin = np.inf dist = np.array([2.0, 2.0, 2.0, 2.0]) # initial condition kf_no_imu = KalmanFilter(X=np.concatenate((dist, np.array([0.0, 0.0, 0.0]))).T) kf = KalmanFilter(X=np.concatenate((dist, np.array([0.0, 0.0, 0.0]))).T) t_old = None x_data = [] xi_data = [] p_data = [] pi_data = [] k_data = [] ki_data = [] t_data = [] vel_data = [] dist_data = [] vel_data = [] dt_data = [] for topic, msg, t_nano in bag.read_messages(
def exercise_4(): colors = ['b', 'g', 'm', 'c', 'y'] car = target time = np.linspace(0, car.location[0] / car.v, 900) trajectory = np.array([car.position(t) for t in time]) # 4.2 Measurements transformation fig = plt.figure() for i, radar in enumerate(radars, start=1): measurements = [radar.measure(car, t) for t in time] trans_measures = np.array([ Radar.cartesian_measurement(m) + radar.location[:2] for m in measurements ]) plt.plot(trans_measures[:, 0], trans_measures[:, 1], c=colors[(i % 5) - 1], label='Radar %s Measurements' % i) plt.plot(trajectory[:, 0], trajectory[:, 1], c='r', label='Real trajectory') plt.title('Trajectory and Radars Measurements') plt.legend() plt.xlabel('X-axis (m)') plt.ylabel('Y-axis (m)') plt.show() # 4.3 Kalman Filter time_limit = car.location[0] / car.v kalman = KalmanFilter(radars, car, delta_t=2) kalman.filter(time_limit) fig = plt.figure() plt.plot(trajectory[:, 0], trajectory[:, 1], c='r', label='Target Trajectory') plt.plot(kalman.track[:, 0], kalman.track[:, 1], c='g', label='Track') plt.xlabel('X-axis (m)') plt.ylabel('Y-axis (m)') plt.title('Real Trajectory vs Track using Kalman Filter') plt.legend(loc='upper right') plt.xlim(-1000, 12000) plt.ylim(-2000, 2000) # plt.show() kalman.d_retro(time_limit) # fig = plt.figure() plt.plot(trajectory[:, 0], trajectory[:, 1], c='r', label='Target Trajectory') plt.plot(kalman.no_filtered_track[:, 0], kalman.no_filtered_track[:, 1], c='y', label='Predicted Track') plt.plot(kalman.track[:, 0], kalman.track[:, 1], c='g', label='Filtered Track') plt.plot(kalman.retro_track[:, 0], kalman.retro_track[:, 1], c='b', label='Track with retrodiction') plt.xlabel('X-axis (m)') plt.ylabel('Y-axis (m)') plt.title( 'Real Trajectory vs Track using Kalman Filter vs Track using retrodiction' ) plt.legend(loc='upper right') plt.xlim(-1000, 12000) plt.ylim(-2000, 2000) plt.show() # error calculation: err = np.sum( np.sqrt( np.sum((trajectory[:, 0] - kalman.retro_track[:, 0])**2 + (trajectory[:, 1] - kalman.retro_track[:, 1])**2))) print('Error of the track when applying retrodiction: ') print(err) # error calculation: err = np.sum( np.sqrt( np.sum((trajectory[:, 0] - kalman.track[:, 0])**2 + (trajectory[:, 1] - kalman.track[:, 1])**2))) print('Error of the track without retrodiction: ') print(err)
def test(): m = lambda x: np.array(x) a = m([[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 1, 0], [0, 0, 0, 1]]) u = m([[0.005, 0, 1, 0], [0, 0.005, 0, 1]]).T q = 0.1 * np.eye(4) # q[2,2] = 0.001 # q[3,3] = 0.001 h = 1. * np.eye(4) r = 1. * np.eye(4) p_init = 1. * np.eye(4) x_init = m([[0], [0], [1], [1]]) num_particles = 100 particles_init = np.random.uniform(-1, 1, size=(4, num_particles)) particles_init[0, :] = 0 particles_init[1, :] = 0 running_x = x_init def system(running_x, actuation): sample = np.random.multivariate_normal(np.zeros(4), q, 1).T running_x = a.dot(running_x) + u.dot(actuation) + sample return running_x def proposal(running_x, actuation): sample = np.random.multivariate_normal(np.zeros(4), q, num_particles).T running_x = a.dot(running_x) + u.dot(actuation) + sample return running_x def sensor(position): sample = np.random.multivariate_normal(np.zeros(4), r, 1).T measure = h.dot(position) + sample return measure def sensor_likelihood(position, measurement, normalization=10.): likelihoods = [] all_dist = (position - measurement.reshape(-1, 1)) weights = 1. / ((normalization * all_dist)**2).mean(0) # for i in range(position.shape[1]): # l = normal_density.pdf(measurement.reshape(-1), position[:, i], normalization) # likelihoods.append(l) return weights f = ParticleFilter(num_particles, particles_init, sensor_likelihood, proposal) all_x = [] all_y = [] all_sensor_x = [] all_sensor_y = [] all_particle_x = [] all_particle_y = [] noiseless_x = [] noiseless_y = [] naive_pos = x_init particles_x = [] particles_y = [] all_kalman_x = [] all_kalman_y = [] kalman = KalmanFilter(a, u, q, h, r) kalman.initialize(x_init, p_init) errors_particle = [] errors_kalman = [] for _ in range(1000): print(_) actuation = m([[1], [0]]) running_x = system(running_x, actuation) measurement = sensor(running_x) pos, _ = f.filter(actuation, measurement) # particles_x.append(f.particles[0].copy()) # particles_y.append(f.particles[1].copy()) all_x.append(running_x[0]) all_y.append(running_x[1]) all_sensor_x.append(measurement[0]) all_sensor_y.append(measurement[1]) all_particle_x.append(pos[0]) all_particle_y.append(pos[1]) naive_pos = a.dot(naive_pos) + u.dot(m([[1], [0]])) noiseless_x.append(naive_pos[0]) noiseless_y.append(naive_pos[1]) kalman.time_update(actuation) pos_kalman, p = kalman.measure_update(measurement) all_kalman_x.append(pos_kalman[0]) all_kalman_y.append(pos_kalman[1]) errors_kalman.append(((running_x - pos_kalman)**2).mean()) errors_particle.append(((running_x - pos.reshape(4, 1))**2).mean()) plt.scatter(all_x, all_y) plt.scatter(all_particle_x, all_particle_y, alpha=0.8) plt.scatter(all_sensor_x, all_sensor_y, alpha=0.5) plt.scatter(all_kalman_x, all_kalman_y, alpha=0.8) # plt.scatter(noiseless_x, noiseless_y, 0.2) plt.scatter(m(particles_x).reshape(-1), m(particles_y).reshape(-1), 0.5, alpha=0.3) plt.show() plt.plot(errors_kalman) plt.plot(errors_particle) plt.show() print(np.array(errors_kalman).mean()) print(np.array(errors_particle).mean()) return True
import matplotlib.patches as mpatches num = 100 x_real = [0]*num x_sensor = [0]*num x_filter = [0]*num x_filter2 = [0]*num x_filter3 = [0]*num x_filter4 = [0]*num if __name__ == '__main__': # Question 1 ------------------------------------------------------- robot = Robot(0.0,0.0,0.0) filter = KalmanFilter(1.0, 1.0, 1.0, \ 1.0, 1.0,\ 0.0,10.0) for i in xrange(num): speed = 1.0 - (float(i)/float(num)) robot.move(speed) x_sensor[i] = robot.sensor() filter.update(speed, x_sensor[i]) x_real[i] = robot.x x_filter[i] = filter.mean plt.figure(1,figsize=(12, 8)) plt.title('Perfect movement and perfect sensing (var_move=0, var_sensor=0)') plt.plot(x_real,label = 'Actual position',lw = 2) plt.plot(x_sensor, 'r--',label = 'Observations') plt.plot(x_filter,label = 'Filter estimate') plt.xlabel("time")
from kalman import KalmanFilter import math process_noise = 0.05 # Kalman parameter {R} measurement_noise = 3 # Kalman parameter {Q} use_kalman_filter = 0 kf = KalmanFilter(R = 0.01, Q =3) data = [42.1, 22.2, 34, 44, 56, 44.1, 31.5] data = map(kf.filter, data) print data
detector = Detector() #Image Receiver net_recvd_length = 0 recvd_image = b'' #Test Controller direction = -1 cnt = 0 packed_data = b'' nb_byte_to_receive = int(sz_image / 8) # init kalman filter kalman = KalmanFilter() err_in_row = 0 width_bbox = 60 height_bbox = 60 while True: ####################### # Receive data ####################### while net_recvd_length < sz_image: reply = s.recv(nb_byte_to_receive) recvd_image += reply net_recvd_length += len(reply)
def main(): ## set root directory data_root = "../data/object_moving" save_root = "results/object_moving" # make directory if not os.path.exists(save_root): os.mkdir(save_root) ## set seed seed = 121 xp.random.seed(seed) print("Set seed number {}".format(seed)) ## set data Tf = 100 # length of time-series N = 25 # widht, height of images dtype = "float32" obs = xp.asarray(np.load(os.path.join(data_root, "obs.npy")), dtype=dtype) true_xp = xp.asarray(np.load(os.path.join(data_root, "true.npy")), dtype=dtype) ## set data for kalman filter skip = 1 # downsampling bg_value = 20 # background value Nf = int(N/skip) # Number of lines after skip obs = obs[:Tf, ::skip, ::skip].reshape(Tf, -1) true_xp = true_xp[:Tf, ::skip, ::skip].reshape(Tf, -1) + bg_value ## set parameters d = 1 # number of adjacency element advance = True sigma_initial = 0 # standard deviation of normal distribution for random making update_interval = 1 # update interval eta = 1.0 # learning rate cutoff = 1.0 # cutoff distance for update of transition matrix sigma = 0.2 # standard deviation of gaussian noise Q = sigma**2 * xp.eye(Nf*Nf) R = sigma**2 * xp.eye(Nf*Nf) # Nf x nlines ## record list mse_record = xp.zeros((3, Tf)) mae_record = xp.zeros((3, Tf)) time_record = xp.zeros(3) all_start_time = time.time() ### Execute F_initial = xp.eye(Nf*Nf) # identity A = xp.asarray(parametric_matrix_2d(Nf, Nf, d), dtype="int32") ## Kalman Filter filtered_value = xp.zeros((Tf, Nf*Nf)) kf = KalmanFilter(transition_matrix = F_initial, transition_covariance = Q, observation_covariance = R, initial_mean = obs[0], dtype = dtype) for t in range(Tf): filtered_value[t] = kf.forward_update(t, obs[t], return_on=True) xp.save(os.path.join(save_root, "kf_states.npy"), filtered_value) ## LLOCK save_dir = os.path.join(save_root, "llock") if not os.path.exists(save_dir): os.mkdir(save_dir) print("SLOCK : d={}, update_interval={}, eta={}, cutoff={}".format( d, update_interval, eta, cutoff)) slock = SpatiallyUniformLOCK(observation = obs, transition_matrix = F_initial, transition_covariance = Q, observation_covariance = R, initial_mean = obs[0], localization_matrix = A, dtype = dtype, update_interval = update_interval, eta = eta, cutoff = cutoff, save_dir = save_dir, advance_mode = advance, use_gpu = False) start_time = time.time() slock.forward() time_record[0] = time.time() - start_time time_record[1] = slock.times[3] time_record[2] = slock.times[3] / slock.times[4] print("SLOCK times : {}".format(time.time() - start_time)) # record error infromation for t in range(Tf): mse_record[0,t] = mean_squared_error( slock.get_filtered_value()[t], true_xp[t]) mae_record[0,t] = mean_absolute_error( slock.get_filtered_value()[t], true_xp[t]) mse_record[1,t] = mean_squared_error( filtered_value[t], true_xp[t]) mae_record[1,t] = mean_absolute_error( filtered_value[t], true_xp[t]) mse_record[2,t] = mean_squared_error( obs[t], true_xp[t]) mae_record[2,t] = mean_absolute_error( obs[t], true_xp[t]) ## save error-record if True: xp.save(os.path.join(save_root, "time_record.npy"), time_record) xp.save(os.path.join(save_root, "mse_record.npy"), mse_record) xp.save(os.path.join(save_root, "mae_record.npy"), mae_record) # mse_record = np.load(os.path.join(save_root, "mse_record.npy")) fig, ax = plt.subplots(1,1,figsize=(8,5)) for i, label in enumerate(["SLOCK", "KF", "observation"]): ax.plot(np.sqrt(mse_record[i]), label=label, lw=2) ax.set_xlabel("Timestep", fontsize=12) ax.set_ylabel("RMSE", fontsize=12) ax.legend(fontsize=15) fig.savefig(os.path.join(save_root, "rmse.png"), bbox_to_inches="tight") ## substantial mse def translation_matrix4(W=10, H=10, direction="right", cyclic=False): F = xp.zeros((W*H, W*H)) if direction=="right": F_block = xp.diag(xp.ones(W-1), -1) if cyclic: F_block[0, -1] = 1 for i in range(H): F[i*W:(i+1)*W, i*W:(i+1)*W] = F_block elif direction=="left": F_block = xp.diag(xp.ones(W-1), 1) if cyclic: F_block[-1, 0] = 1 for i in range(H): F[i*W:(i+1)*W, i*W:(i+1)*W] = F_block elif direction=="up": F_block = xp.eye(W) for i in range(H-1): F[i*W:(i+1)*W, (i+1)*W:(i+2)*W] = F_block if cyclic: F[(H-1)*W:H*W, 0:W] = F_block elif direction=="down": F_block = xp.eye(W) for i in range(H-1): F[(i+1)*W:(i+2)*W, i*W:(i+1)*W] = F_block if cyclic: F[0:W, (H-1)*W:H*W] = F_block return F def translation_matrix8(W=10, H=10, direction="right", cyclic=False): if direction in ["right", "left", "up", "down"]: F = translation_matrix4(W, H, direction, cyclic) elif direction in ["up-right", "up-left", "down-right", "down-left"]: direction1, direction2 = direction.split("-") F = translation_matrix4(W, H, direction1, cyclic) @ translation_matrix4(W, H, direction2, cyclic) return F direction_count = 0 directions = ["right", "up", "left", "down", "right", "up", "up-right","left", "down-right","up","down-left","up","down-right"] direction_changes = [5,10,20,30,35,40,45,55,65,75,85,95,1000] Ftrue = translation_matrix8(Nf, Nf, directions[0]) mean_error = np.zeros((2, Tf//update_interval-1)) for t in range(Tf//update_interval-1): fvalue = np.load(os.path.join(save_dir, "transition_matrix_{:03}.npy".format(t))) if t+1>=direction_changes[direction_count]: direction_count += 1 Ftrue = translation_matrix8(Nf, Nf, directions[direction_count], True) mean_error[0,t] = np.sqrt(np.power(np.absolute(fvalue - Ftrue)[A.astype(bool) & ~Ftrue.astype(bool)], 2).mean()) mean_error[1,t] = np.sqrt(np.power(np.absolute(fvalue - Ftrue)[A.astype(bool) & Ftrue.astype(bool)], 2).mean()) fig, ax = plt.subplots(1,1,figsize=(12,5)) lw = 2 for i in range(2): ax.plot(update_interval * np.array(range(Tf//update_interval-1)), mean_error[i], label="true={}".format(i), lw=lw) for mc in direction_changes[:-1]: ax.axvline(mc, ls="--", color="navy", lw=1) ax.set_xlabel("Timestep", fontsize=15) ax.set_ylabel("SRMSE", fontsize=15) ax.set_yscale("log") ax.legend(fontsize=12) ax.tick_params(labelsize=12) directions_for_print = ["right", " up", " left", " down", "right", " up", "up-right"," left", " down-right"," up"," down-left"," up"," down-right"] fig.text(0.09, 0, "Direction: ", fontsize=10) for kk, direction, mc in zip(range(len(directions)), directions_for_print, [0] + direction_changes[:-1]): fig.text(0.16 + mc*0.0071, 0, direction, fontsize=10) fig.savefig(os.path.join(save_root, "srmse.png"), bbox_inches="tight") all_execute_time = int(time.time() - all_start_time) print("all time (sec): {} sec".format(all_execute_time)) print("all time (min): {} min".format(int(all_execute_time//60))) print("all time (hour): {} hour + {} min".format(int(all_execute_time//3600), int((all_execute_time//60)%60)))
# [227, 255, 207, 255, 230, 255, 97, 1, 119, 253, 178, 2, 255, 255, 1, 0, 3, 0, 232, 3, 111, 4] print("fully calibrated!") carX = 0 carvX = 0 carY = 0 carvY = 0 caraX = 0 caraY = 0 carAngle = 0 carkvX = 0 carkX = 0 kalmanFilter = KalmanFilter(0.001, 0.5) input("are you ready?") WAIT = 0.01 calibrate(4) while True: heading, roll, pitch = bno.read_euler() x, y, z = bno.read_linear_acceleration() #print("\nheading: "+str(heading)+", roll: "+str(roll)+", pitch: "+str(pitch)) #print("X: "+str(x/100)+", Y: "+str(y/100)+", Z: "+str(z/100)+"") kalmanFilter.input_latest_noisy_measurement(x) kX = kalmanFilter.get_latest_estimated_measurement()