class KalmanTrack: """ This class represents the internal state of individual tracked objects observed as bounding boxes. """ def __init__(self, initial_state): """ Initialises a tracked object according to initial bounding box. :param initial_state: (array) single detection in form of bounding box [X_min, Y_min, X_max, Y_max] """ self.kf = KalmanFilter(dim_x=7, dim_z=4) # Transition matrix self.kf.F = 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]]) # Transformation matrix (Observation to State) self.kf.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.R[2:, 2:] *= 10. # observation error covariance self.kf.P[4:, 4:] *= 1000. # initial velocity error covariance self.kf.P *= 10. # initial location error covariance self.kf.Q[-1, -1] *= 0.01 # process noise self.kf.Q[4:, 4:] *= 0.01 # process noise self.kf.x[:4] = xxyy_to_xysr(initial_state) # initialize KalmanFilter state def project(self): """ :return: (ndarray) The KalmanFilter estimated object state in [x1,x2,y1,y2] format """ return xysr_to_xxyy(self.kf.x) def update(self, new_detection): """ Updates track with new observation and returns itself after update :param new_detection: (np.ndarray) new observation in format [x1,x2,y1,y2] :return: KalmanTrack object class (itself after update) """ self.kf.update(xxyy_to_xysr(new_detection)) return self def predict(self): """ :return: ndarray) The KalmanFilter estimated new object state in [x1,x2,y1,y2] format """ if self.kf.x[6] + self.kf.x[2] <= 0: self.kf.x[6] *= 0.0 self.kf.predict() return self.project()
def kalman_angle(): global kAngle_g T = 0.01 lamC = 1.0 #variance for compass lamGPS = 1.0 #variance for gps bearing lamGyro = 1.0 #variance for gyroscope sig = 1.0 #variance for model sig2 = sig**2 lamC2 = lamC**2 lamGPS2 = lamGPS**2 lamGyro2 = lamGyro**2 (A, H, Q, R) = create_model_parameters(T, sig2, lamC2) #initialize evolution, measurement, model error, and measurement error # initial state x = np.array([0, 0.1]) #starting velocity and position P = 0 * np.eye(2) #starting probability 100% kalman_filter = KalmanFilter(A, H, Q, R, x, P) #create the weights filter lastTime = time.time() startTime = lastTime lastOmega = 0 lastAngle = 0 thetaCoord_l = 0 #local variable to prevent race conditions lastBearing = 0 gpsTrust = 1.0 while True: while (omega_g == lastOmega or thetaCoord_g == lastAngle): #check if there were updates to the compass time.sleep(0.01) #if no updates then wait a bit thetaCoord_l = thetaCoord_g lastAngle = thetaCoord_l if (kalman_filter._x[0] - thetaCoord_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (thetaCoord_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 kalman_filter.predict() #evolve the state and the error kalman_filter.update((thetaCoord_l,omega_g),(lamC2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that. # while (omega_g == lastOmega or gpsTheta_g == lastBearing): #check if there were updates to the gps bearing # time.sleep(0.01) #if no updates then wait a bit gpsTheta_l = gpsTheta_g lastBearing = gpsTheta_l if (kalman_filter._x[0] - gpsTheta_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (gpsTheta_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 lamGPS2 = gpsTrust/gpsDistance_g kalman_filter.predict() #evolve the state and the error kalman_filter.update((gpsTheta_l,omega_g),(lamGPS2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that.
class PoseEstimater: def __init__(self, N): self.N = N self.state_estimater = KalmanFilter(6) self.sub = rospy.Subscriber("/kinect_head/rgb/ObjectDetection", ObjectDetection, self.cb_object_detection, queue_size=10) self.pub = rospy.Publisher('fridge_pose', PoseStamped, queue_size=10) def cb_object_detection(self, msg): header = copy.deepcopy(msg.header) def convert_pose2tf(pose): pos = pose.position rot = pose.orientation trans = [pos.x, pos.y, pos.z] rot = [rot.x, rot.y, rot.z, rot.w] return (trans, rot) header = msg.header assert len(msg.objects) == 1 obj = msg.objects[0] tf_handle_to_kinect = convert_pose2tf(obj.pose) tf_kinect_to_map = listener.lookupTransform( '/map', '/head_mount_kinect_rgb_optical_frame', rospy.Time(0)) rpy = tf.transformations.euler_from_quaternion(tf_handle_to_kinect[1]) dist_to_kinect = np.linalg.norm(tf_handle_to_kinect[0]) print("dist to kinnect is {0}".format(dist_to_kinect)) tf_handle_to_map = convert(tf_handle_to_kinect, tf_kinect_to_map) trans = tf_handle_to_map[0] rpy = tf.transformations.euler_from_quaternion(tf_handle_to_map[1]) state = np.array(list(trans) + list(rpy)) std_trans = dist_to_kinect * 0.5 std_rot = dist_to_kinect * 0.1 cov = np.diag([std_trans**2] * 3 + [std_rot**2] * 3) if self.state_estimater.isInitialized: self.state_estimater.update(state, cov) else: cov_init = cov * 10 self.state_estimater.initialize(state, cov_init) x_est, cov = self.state_estimater.get_current_est(withCov=None) print(x_est) pose_msg = create_posemsg_from_pose3d(x_est) header.frame_id = "/map" posestamped_msg = PoseStamped(header=header, pose=pose_msg) self.pub.publish(posestamped_msg)
class Tracking(object): def __init__(self): self.tracking_list = [] self.data_association = DataAssociation() self.tracking_filter = KalmanFilter() pass def update(self,object_list,timeframe): if not self.tracking_list: for object in object_list: self.init_track(object) else: self.tracking_filter.predict(self.tracking_list,timeframe) asso = self.data_association.nearest_neighbor(object_list,self.tracking_list) self.tracking_filter.update(object_list,self.tracking_list,asso) def init_track(self,object): tr = Track(object) self.tracking_list.append(tr) def number_of_tracks(self): return len(self.tracking_list)
def filter_data(data, x0, P, Q, R): filter1 = KalmanFilter(x0, P, 1, 0, 1, Q, R) x_out = np.zeros(data.size) P_out = np.zeros(data.size) for k in np.arange(1, data.size): x_out[k], P_out[k] = filter1.update(0, data[k]) return x_out, P_out
class TestKalmanFilter(unittest.TestCase): def setUp(self): A = np.eye(2) H = np.eye(2) Q = np.eye(2) R = np.eye(2) x_0 = np.array([1, 1]) P_0 = np.eye(2) self.kalman_filter = KalmanFilter(A, H, Q, R, x_0, P_0) def test_predict(self): self.kalman_filter.predict() (x, P) = self.kalman_filter.get_state() self.assertTrue((x == np.array([1, 1])).all()) self.assertTrue((P == 2 * np.eye(2)).all()) def test_update(self): self.kalman_filter.update(np.array([0, 0])) (x, P) = self.kalman_filter.get_state() self.assertTrue((x == np.array([0.5, 0.5])).all()) self.assertTrue((P == 0.5 * np.eye(2)).all())
class fans_kalman(object): def __init__(self, initial_x0, sigma_w, sigma_v): self.sigma_w = sigma_w self.sigma_v = sigma_v self.define_kalman_parameters(initial_x0) self.define_kalman_filter() def define_kalman_parameters(self, initial_x0): 'initialize the parameters of kalman filter here' dt = 1 self.F = np.array([[1, dt], [0, 1]]) self.H = np.array([1, 0]).reshape(1, 2) self.Q = np.array([[self.sigma_w, 0.0], [0.0, self.sigma_w]]) self.R = np.array([self.sigma_v]).reshape(1, 1) self.P = np.array([[10., 0], [0, 10.]]) 'Notice that, the inital_x0 here actually is a state, which is a 2-d array' self.initial_x0 = np.array([[initial_x0], [0]]) def define_kalman_filter(self): self.kf = KalmanFilter(F=self.F, H=self.H, Q=self.Q, R=self.R, P=self.P, x0=self.initial_x0) def predict_interface(self, new_measurement): 'predict the value, and update the kalman filter' if new_measurement is not None: self.kf.update(new_measurement) 'Otherwise, skip the measurement-update step...just perform the time-update' res = np.dot(self.H, self.kf.predict())[0] 'res is a scalar here' return res
class EM: def __init__(self): self.kf = KalmanFilter() def find_Q(self, data=None): log_likelihoods = [] for i in range(0, 20): x_posteriors, x_predictions, cov = self.run(data=data) log_likelihoods.append(self.calculate_log_likelihood(x_posteriors, cov, data)) self.m_step(x_predictions, x_posteriors, data) return log_likelihoods def run(self, data=None): x_posteriors, x_predictions, cov = [], [], [] for z in data: self.kf.predict() x_predictions.append(self.kf.x) self.kf.update(z) x_posteriors.append(self.kf.x) cov.append(self.kf.P) x_posteriors, x_predictions, cov = np.array(x_posteriors), np.array(x_predictions), np.array(cov) return x_posteriors, x_predictions, cov def calculate_log_likelihood(self, x_posteriors, cov, measurements): log_likelihood = 0 for i in range(0, len(cov)): S = np.dot(np.dot(self.kf.H, cov[i]), self.kf.H.T) + self.kf.R state_posterior_in_meas_space = np.dot(self.kf.H, x_posteriors[i]).squeeze() distribution = multivariate_normal(mean=state_posterior_in_meas_space, cov=S) log_likelihood += np.log(distribution.pdf(measurements[i])) return log_likelihood def m_step(self, x_predictions, x_posteriors, measurements): self.kf = KalmanFilter() self.kf.Q = np.cov((x_posteriors - x_predictions).squeeze().T, bias=True) self.kf.R = np.cov((measurements.T - np.dot(self.kf.H, x_posteriors.squeeze().T)), bias=True)
def run(filename): arrays = [] filtered_arrays = [] sums = [] current = None with open(filename, "rU") as csvfile: spamreader = csv.reader(csvfile, delimiter=",", quotechar="|") readings = list(spamreader) for index, row in enumerate(readings): if "#" in row[0]: arrays.append([]) filtered_arrays.append([]) sums.append((0, 0, 0)) current = arrays[-1] current_filtered = filtered_arrays[-1] initial_values = list((float(i) for i in readings[index + 1])) x_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[0]) y_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[1]) z_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[2]) else: x, y, z = tuple(float(i) for i in row) current.append(tuple((x, y, z))) x_filt = x_Kalman.update(x) y_filt = y_Kalman.update(y) z_filt = z_Kalman.update(z) current_filtered.append(tuple((x_filt, y_filt, z_filt))) x += sums[-1][0] + x_filt y += sums[-1][1] + y_filt z += sums[-1][2] + z_filt sums[-1] = (x, y, z) display_graphs(arrays, filtered_arrays, sums)
def run(filename): current_filtered = [] sums = [] current = [] x_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0) x_Kalmanq=KalmanFilter(p=1000, r=100, q=1, k=0) x_Kalmanr=KalmanFilter(p=1000, r=5000, q=1, k=0) y_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0) y_Kalmanq = KalmanFilter(p=1000, r=100, q=10, k=0) y_Kalmanr = KalmanFilter(p=1000, r=500, q=1, k=0) z_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0) z_Kalmanq = KalmanFilter(p=1000, r=100, q=1, k=0) z_Kalmanr = KalmanFilter(p=1000, r=500, q=10, k=0) with open(filename, "rU") as csvfile: spamreader = csv.reader(csvfile, delimiter=',', quotechar='|') readings = list(spamreader) x_Kalman.x,y_Kalman.x,z_Kalman.z=[float(i) for i in readings[0]] x_Kalman.x=x_Kalmanr.x=x_Kalman.x y_Kalman.x=y_Kalmanr.x=y_Kalman.x z_Kalman.x=z_Kalmanr.x=z_Kalman.x for index, row in enumerate(readings): if(index >= START and index <= END): x, y, z = tuple(float(i) for i in row) current.append(tuple((x, y, z))) x_filt = x_Kalman.update(x) x_filtq = x_Kalmanq.update(x) x_filtr = x_Kalmanq.update(x) y_filt = y_Kalman.update(y) z_filt = z_Kalman.update(z) y_filtq = y_Kalmanq.update(y) y_filtr = y_Kalmanq.update(y) z_filtq = z_Kalmanq.update(z) z_filtr = z_Kalmanq.update(z) current_filtered.append(tuple(((x_filt,x_filtq,x_filtr), (y_filt,y_filtq,y_filtr), (z_filt,z_filtq,z_filtr)))) display_graphs(current, current_filtered)
x_estimated = [x_init] cov_estimated = [Sigma_init] x_real_list = [np.array([[0, 0, 5, 10]]).transpose()] x_real = x_real_list[0] z_list = [] for i in xrange(100): kf.predict(u) # simulate noisy observation x_real = A.dot(x_real)+u z = x_real[0:2, :] + np.random.multivariate_normal(np.zeros((2,)), R).reshape((2,1)) kf.update(z) x_estimated.append(kf.x) cov_estimated.append(kf.Sigma) x_real_list.append(x_real) z_list.append(z) plt.figure() px = np.array([ pv[0, 0] for pv in x_real_list]) py = np.array([ pv[1, 0] for pv in x_real_list]) plt.plot(px, py, 'r', label="True position") gx = np.array([ pv[0, 0] for pv in x_estimated]) gy = np.array([ pv[1, 0] for pv in x_estimated]) plt.plot(gx, gy, 'b', label="KF position estimate")
class KalmanBoxTracker(object): """ This class represents the internel state of individual tracked objects observed as bbox. """ count1 = 0 count2 = 0 def __init__(self, bbox, channel, reset_id): """ Initialises a tracker using initial bounding box. """ self.channel = channel self.F = 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]]) self.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(F=self.F, H=self.H) self.kf.R[2:, 2:] *= 10. self.kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1, -1] *= 0.01 self.kf.Q[4:, 4:] *= 0.01 self.kf.x[:4], self.score, self.class_type = convert_bbox_to_z(bbox) self.time_since_update = 0 if channel == 1: if reset_id: # print('channel1 is reset to 0') KalmanBoxTracker.count1 = 0 self.id = KalmanBoxTracker.count1 KalmanBoxTracker.count1 += 1 elif channel == 2: if reset_id: # print('channel2 is reset to 0') KalmanBoxTracker.count2 = 0 self.id = KalmanBoxTracker.count2 KalmanBoxTracker.count2 += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 def update(self, bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)[0]) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if (self.kf.x[6] + self.kf.x[2]) <= 0: self.kf.x[6] *= 0.0 self.kf.predict() self.age += 1 if self.time_since_update > 0: self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x) def get_area(self): if self.class_type == 0: return 1.2 elif self.class_type == 1: return 4.48 elif self.class_type == 2: return 5.36 elif self.class_type == 3: return 24.74 def get_velocity(self): velocity_x = self.kf.x[4] velocity_y = self.kf.x[5] magnitude = int(np.sqrt(velocity_x ** 2 + velocity_y ** 2)) if (velocity_x != 0) & (velocity_y != 0): direction = int(np.arctan(velocity_y / velocity_x) * (180 / np.pi)) return magnitude, direction else: return 0, 0
class FusionEKF: def __init__(self): self.kalman_filter = KalmanFilter() self.is_initialized = False self.previous_timestamp = 0 self.noise_ax = 1 self.noise_ay = 1 self.noise_az = 1 self.noise_vector = np.diag( [self.noise_ax, self.noise_ay, self.noise_az]) self.kalman_filter.P = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) self.kalman_filter.x = np.zeros((6, 1)) # self.uwb_std = 23.5 / 1000 self.uwb_std = 23.5 / 10 self.R_UWB = np.array([[self.uwb_std**2]]) self.kalman_filter.R = self.R_UWB def process_measurement(self, anchor_pose, anchor_distance): if not self.is_initialized: self.is_initialized = True self.previous_timestamp = time.time() return current_time = time.time() dt = current_time - self.previous_timestamp self.previous_timestamp = current_time self.calulate_F_matrix(dt) self.calculate_Q_matrix(dt) self.kalman_filter.predict() self.kalman_filter.update(anchor_pose, anchor_distance) # print("X:", self.kalman_filter.x) # print("P:", self.kalman_filter.P) # print(self.kalman_filter.Q) def calulate_F_matrix(self, dt): self.kalman_filter.F = np.array([[1, 0, 0, dt, 0, 0], [0, 1, 0, 0, dt, 0], [0, 0, 1, 0, 0, dt], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) def calculate_Q_matrix(self, dt): a = [[(dt**4) / 4, (dt**3) / 2], [(dt**3) / 2, dt**2]] self.kalman_filter.Q = np.kron(a, self.noise_vector)
log.new_log('covariance') log.new_log('moving average') # Moving average for measurements moving_avg = MovingAverage(15) # Number of iterations to perform iterations = 100 for i in range(0, iterations): # Generate a random acceleration w = matrix(random.multivariate_normal([0.0, 0.0], Q)).T # Predict (X, P) = kf.predict(X, P, w) # Update (X, P) = kf.update(X, P, Z) # Update the actual position A = F * A + w # Synthesise a new noisy measurement distributed around the real position Z = matrix([[random.normal(A[0, 0], sigma_z)], [0.0]]) # Update the moving average with the latest measured position moving_avg.update(Z[0, 0]) # Update the log for plotting later log.log('measurement', Z[0, 0]) log.log('estimate', X[0, 0]) log.log('actual', A[0, 0]) log.log('time', i * dt) log.log('covariance', P[0, 0]) log.log('moving average', moving_avg.getAvg()) # Plot the system behaviour
delta_t = 0.5 # predictions are going to be made for 0.5 seconds in the future A = np.array([[1, delta_t], [0, 1]]) B = np.zeros((2, 2)) G = np.identity(2) H = np.array([[1, 0]]) sigma_w = 0.1 # 10cm = standard deviation of position prediction noise for one time step of dt seconds Q = (sigma_w**2) * np.identity(2) sigma_n = 0.2 # 20cm = standard deviation of position measurement noise R = (sigma_n**2) * np.identity(1) x_init = np.array([[0, 5]]).transpose() sigma_p = 1 # 10cm = uncertainty about initial position sigma_v = 20 # 20m/s = uncertainty about initial velocity Sigma_init = np.array([[sigma_p**2, 0], [0, sigma_v**2]]) kf = KalmanFilter(A, B, G, H, Q, R, x_init, Sigma_init) plot_mean_and_covariance(kf.x, kf.Sigma) kf.predict() plot_mean_and_covariance(kf.x, kf.Sigma) kf.update(z=5) plot_mean_and_covariance(kf.x, kf.Sigma)
class SensorFusion: X_INDEX = 0 Y_INDEX = 1 VX_INDEX = 2 VY_INDEX = 3 AX_INDEX = 4 AY_INDEX = 5 def __init__(self): self.resetup([True] * 6) self._kf = KalmanFilter() def resetup(self, maintain_state_flag): self._maintain_state_flag = copy.copy(maintain_state_flag) self._maintain_state_size = self._maintain_state_flag.count(True) print(self._maintain_state_size) print(self._maintain_state_flag) # lidar H will not change even if we change the state we maintain self._lidar_H = np.zeros(self._maintain_state_size * 2).reshape(2, -1) self._lidar_H[0, 0], self._lidar_H[1, 1] = 1.0, 1.0 '''setup F matrix, if we maintain acceleration, it should be reflected in F matrix''' def setup_F(self, dt): self._F = np.identity(self._maintain_state_size) self._F[SensorFusion.X_INDEX, SensorFusion.VX_INDEX] = dt self._F[SensorFusion.Y_INDEX, SensorFusion.VY_INDEX] = dt if self._maintain_state_flag[SensorFusion.AX_INDEX]: self._F[SensorFusion.VX_INDEX, SensorFusion.AX_INDEX] = dt if self._maintain_state_flag[SensorFusion.AY_INDEX]: self._F[SensorFusion.VY_INDEX, SensorFusion.AY_INDEX] = dt def setup_lidar_H(self): self._kf.setH(self._lidar_H) def update_with_lidar_obs(self, lidar_obs): z = np.array([lidar_obs.get_local_px(), lidar_obs.get_local_py()]).reshape(-1, 1) self._kf.setMeasurement(z) self._kf.setH(self._lidar_H) print("lidar_H ", self._lidar_H) self._kf.update() return self._kf.getState(), self._kf.getP() def predict(self, dt, warp): rotation = warp[:2, :2] translation = warp[:2, 2] self._kf.warp(rotation, translation) self.setup_F(dt) self._kf.setF(self._F) self._kf.predict() def update_with_radar_obs(self, radar_obs, use_lat_v=False): z = radar_obs.get_radar_measurement().reshape(-1, 1) if not use_lat_v: z = z[:3, :].reshape(-1, 1) print("radar z", z) self._kf.setMeasurement(z) self.setup_radar_H(use_lat_v) print("radar H", self._kf.getH()) self._kf.update() return self._kf.getState(), self._kf.getP() def setup_radar_H(self, use_lat_v=False): if use_lat_v: self.setup_radar_H_w_lat(self.get_state()) else: self.setup_radar_H_wo_lat(self.get_state()) def cal_radar_observe_wo_lat(self, state): state = copy.copy(state) x, y, vx, vy = state[0], state[1], state[2], state[3] R = np.sqrt(x**2 + y**2) theta = np.arctan2(y, x) dRdt = vx * np.cos(theta) + vy * np.sin(theta) return np.array([R, theta, dRdt]).reshape(-1, 1) def setup_radar_H_wo_lat(self, state): state = copy.copy(state) state.reshape(-1, 1) x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] obs = self.cal_radar_observe_wo_lat(state) R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0] dR_dx = x / R dR_dy = y / R dR_dvx = 0.0 dR_dvy = 0.0 dtheta_dx = -y / (R**2) dtheta_dy = x / (R**2) dtheta_dvx = 0.0 dtheta_dvy = 0.0 dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \ vy * np.cos(theta) * dtheta_dx dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \ vy * np.cos(theta) * dtheta_dy dRdt_dvx = np.sin(theta) dRdt_dvy = np.cos(theta) H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy], [dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy], [dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy]]) self._kf.setH(H) def cal_radar_observe_w_lat(self, state_): state = copy.copy(state_) x, y, vx, vy = state[0], state[1], state[2], state[3] R = np.sqrt(x**2 + y**2) theta = np.arctan2(y, x) dRdt = vx * np.cos(theta) + vy * np.sin(theta) dLdt = -vx * np.sin(theta) + vy * np.cos(theta) return np.array([R, theta, dRdt, dLdt], dtype=float).reshape(-1, 1) def setup_radar_H_w_lat(self, state): state = copy.copy(state) state.reshape(-1, 1) x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] obs = self.cal_radar_observe_w_lat(state) R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0] dR_dx = x / R dR_dy = y / R dR_dvx = 0.0 dR_dvy = 0.0 dtheta_dx = -y / (R**2) dtheta_dy = x / (R**2) dtheta_dvx = 0.0 dtheta_dvy = 0.0 dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \ vy * np.cos(theta) * dtheta_dx dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \ vy * np.cos(theta) * dtheta_dy dRdt_dvx = np.sin(theta) dRdt_dvy = np.cos(theta) dLdt_dx = -vx * np.cos(theta) * dtheta_dx - \ vy * np.sin(theta) * dtheta_dx dLdt_dy = -vx * np.cos(theta) * dtheta_dy - \ vy * np.sin(theta) * dtheta_dy dLdt_dvx = -np.sin(theta) dLdt_dvy = np.cos(theta) H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy], [dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy], [dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy], [dLdt_dx, dLdt_dy, dLdt_dvx, dLdt_dvy]]) self._kf.setH(H) def set_maintain_state(self, state_flags): self.resetup(state_flags) def set_P(self, P): self._kf.setP( P[:self._maintain_state_size, :self._maintain_state_size]) def get_P(self): return self._kf.getP() def set_Q(self, Q): self._kf.setQ( Q[:self._maintain_state_size, :self._maintain_state_size]) def get_Q(self): return self._kf.getQ() def set_R(self, R): self._kf.setR(R) def get_R(self): return self._kf.getR() def set_state(self, state): print("state:", state) if len(state) != self._maintain_state_size: print(" STATE dimension not equals to maintain state size") s = [] for i in range(len(self._maintain_state_flag)): if self._maintain_state_flag[i]: s.append(state[i]) s = np.array(s).reshape(-1, 1) self._kf.setState(s) def get_state(self): return self._kf.getState() def get_state_in_lidar_format(self, Twl): state = self.get_state() px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] if len(state) == 6: ax, ay = state[4, 0], state[5, 0] else: ax, ay = 0, 0 return LidarObservation(px, py, vx, vy, Twl=Twl, local_ax=ax, local_ay=ay) def get_state_in_radar_format(self, Twr): state = self.get_state() px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] R = np.sqrt(px**2 + py**2) theta = np.arctan2(py, px) range_v = vx * np.cos(theta) + vy * np.sin(theta) lat_v = -vx * np.sin(theta) + vy * np.cos(theta) return RadarObservation(R, theta, range_v, lat_v, Twr)
class MainWindow(QtGui.QMainWindow): # Signals: connect_signal = QtCore.pyqtSignal(str, int) disconnect_signal = QtCore.pyqtSignal() def __init__(self, parent=None): super().__init__(parent) self._initialise_gui_widgets() self._setup_gui() self.connected = False self.time_zero = time.time() def _initialise_gui_widgets(self): """ Create all the widgets for the window. """ # # Main widget self.main_widget = QtGui.QWidget() # # Address label self.address_label = QtGui.QLabel("Server:") self.address_label.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter) # # Address bar self.address_bar = QtGui.QLineEdit() # Create a regex validator for the IP address ip_values = "(?:[0-1]?[0-9]?[0-9]|2[0-4][0-9]|25[0-5])" ip_regex = QtCore.QRegExp("^" + ip_values + "\\." + ip_values + "\\." + ip_values + "\\." + ip_values + "$") ip_validator = QtGui.QRegExpValidator(ip_regex, self) self.address_bar.setValidator(ip_validator) self.address_bar.setText("192.168.42.1") # # Port bar self.port_bar = QtGui.QLineEdit() self.port_bar.setValidator(QtGui.QIntValidator()) self.port_bar.setText("12345") # # Connect button self.connect_button = QtGui.QPushButton("&Connect") self.connect_button.clicked.connect(self._toggle_connect) # # Plot widgets self.plot_widgets = { 'accelerometer': plt.UpdatingPlotWidget(title='Accelerometer'), 'magnetometer': plt.UpdatingPlotWidget(title='Magnetometer'), 'gyroscope': plt.UpdatingPlotWidget(title='Gyroscope'), 'barometer': plt.UpdatingPlotWidget(title='Barometer'), 'gps-pos': plt.UpdatingPlotWidget(title='GPS Position') } for name, widget in self.plot_widgets.items(): if name == 'barometer': widget.add_item('altitude', pen='k') widget.add_item('filtered', pen='r') widget.add_item('altitude_gps', pen='b') self.filter1 = KalmanFilter(x_prior=0, P_prior=2, A=1, B=0, H=1, Q=0.005, R=1.02958) elif name == 'gps-pos': widget.add_item('position', symbol='o') else: widget.add_item('x', pen='r') widget.add_item('y', pen='g') widget.add_item('z', pen='b') def _setup_gui(self): """ Add all the widgets to the window. Layout: 0 1 2 3 4 5 |-----------------------------------------------------| 0 | | | Label | Address | Port | Connect | |-----------------------------------------------------| 1 | Accelerometer | Gyroscope | |-----------------------------------------------------| 2 | Magnetometer | Barometer | |-----------------------------------------------------| 3 | GPS XY | | | | | | |-----------------------------------------------------| """ # # Window setup self.setWindowTitle('Remote Display') self.resize(450, 800) # # Main widget setup self.main_widget = QtGui.QWidget() self.setCentralWidget(self.main_widget) # # Layout setup self.grid = QtGui.QGridLayout() self.main_widget.setLayout(self.grid) self.grid.setRowMinimumHeight(0, 50) self.grid.setRowMinimumHeight(1, 200) self.grid.setRowMinimumHeight(2, 200) self.grid.setRowMinimumHeight(3, 200) self.grid.setColumnMinimumWidth(0, 200) self.grid.setColumnMinimumWidth(1, 200) self.grid.setColumnMinimumWidth(2, 200) self.grid.setColumnMinimumWidth(3, 200) self.grid.setColumnMinimumWidth(4, 200) self.grid.setColumnMinimumWidth(5, 200) self.grid.setSpacing(10) self.main_widget.setLayout(self.grid) # # Add widgets to layout self.grid.addWidget(self.address_label, 0, 2) self.grid.addWidget(self.address_bar, 0, 3) self.grid.addWidget(self.port_bar, 0, 4) self.grid.addWidget(self.connect_button, 0, 5) for name, widget in self.plot_widgets.items(): if name == 'accelerometer': self.grid.addWidget(widget, 1, 0, 1, 3) elif name == 'gyroscope': self.grid.addWidget(widget, 1, 3, 1, 3) elif name == 'magnetometer': self.grid.addWidget(widget, 2, 0, 1, 3) elif name == 'barometer': self.grid.addWidget(widget, 2, 3, 1, 3) elif name == 'gps-pos': self.grid.addWidget(widget, 3, 0, 1, 1) @QtCore.pyqtSlot() def _toggle_connect(self): """ Slot for when connect button is clicked """ if self.connected: self.disconnect_signal.emit() self.connect_button.setText("&Connect") self.address_bar.setEnabled(True) self.port_bar.setEnabled(True) self.connected = False else: address = self.address_bar.text() port = int(self.port_bar.text()) self.connect_signal.emit(address, port) self.connect_button.setText("Dis&connect") self.address_bar.setEnabled(False) self.port_bar.setEnabled(False) self.connected = True @QtCore.pyqtSlot(list) def data_received(self, received_data): """ Slot for when data is received - updates the plots """ data_source = received_data[0] values = received_data[1] t = time.time() - self.time_zero if data_source == com.BAROMETER_UNFILTERED_ID: self.plot_widgets['barometer'].get_item('altitude').update_data( t, values[0]) self.plot_widgets['barometer'].get_item('filtered').update_data( t, self.filter1.update(u_input=0, z_measurement=values[0])[0]) elif data_source == com.ACCELEROMETER_ID: if len(values) != 3: print( "Error: Expected 3 values for accelerometer data, got {}". format(len(values))) else: self.plot_widgets['accelerometer'].get_item('x').update_data( t, values[0]) self.plot_widgets['accelerometer'].get_item('y').update_data( t, values[1]) self.plot_widgets['accelerometer'].get_item('z').update_data( t, values[2]) elif data_source == com.MAGNETOMETER_ID: if len(values) != 3: print("Error: Expected 3 values for magnetometer data, got {}". format(len(values))) else: self.plot_widgets['magnetometer'].get_item('x').update_data( t, values[0]) self.plot_widgets['magnetometer'].get_item('y').update_data( t, values[1]) self.plot_widgets['magnetometer'].get_item('z').update_data( t, values[2]) elif data_source == com.GYROSCOPE_ID: if len(values) != 3: print("Error: Expected 3 values for gyroscope data, got {}". format(len(values))) else: self.plot_widgets['gyroscope'].get_item('x').update_data( t, values[0]) self.plot_widgets['gyroscope'].get_item('y').update_data( t, values[1]) self.plot_widgets['gyroscope'].get_item('z').update_data( t, values[2]) elif data_source == com.GPS_POS_ID: if len(values) != 2: print( "Error: Expected 2 values for GPS position, got {}".format( len(values))) else: self.plot_widgets['gps-pos'].get_item('position').update_data( values[0], values[1]) elif data_source == com.GPS_ALT_ID: if len(values) != 1: print( "Error: Expected 1 value for GPS altitude, got {}".format( len(values))) else: self.plot_widgets['barometer'].get_item( 'altitude_gps').update_data(t, values[0])
np.random.seed(21) (A, H, Q, R) = create_model_parameters() K = 20 # initial state x = np.array([0, 0.5, 0, 0.5]) P = 0 * np.eye(4) (state, meas) = simulate_system(K, x) kalman_filter = KalmanFilter(A, H, Q, R, x, P) est_state = np.zeros((K, 4)) est_cov = np.zeros((K, 4, 4)) for k in range(K): kalman_filter.predict() kalman_filter.update(meas[k, :]) (x, P) = kalman_filter.get_state() est_state[k, :] = x est_cov[k, ...] = P plt.figure(figsize=(7, 5)) plt.plot(state[:, 0], state[:, 2], '-bo') plt.plot(est_state[:, 0], est_state[:, 2], '-ko') plt.plot(meas[:, 0], meas[:, 1], ':rs') plt.xlabel('x [m]') plt.ylabel('y [m]') plt.legend(['true state', 'inferred state', 'observed measurement']) plt.axis('square') plt.tight_layout(pad=0) plt.show()
sift = Sift(gray_frame) while is_frame_good: cv2.imshow('video 0', frame) prev_f = frame is_frame_good, frame = video.read() # Redraw frame with new bounding box cv2.rectangle(frame, (bounding_box[0][0][0], bounding_box[0][1][0]), (bounding_box[0][2][0], bounding_box[0][3][0]), (255, 0, 0), 2) """ Correct filter with new bounding box on previous frame Predict bounding box in new frame """ bounding_box[0] = kf.update(bounding_box[0]) """ Extract features using sift in previous image Match features extracted in previous image with current image (updates bounding box too) """ pdb.set_trace() features = sift.extract_features(bounding_box[0]) gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) matches, bounding_box[0] = sift.match_features(gray_frame) # Update previous frame cv2.waitKey(25) # delays next video frams (ms) video.release() cv2.destroyAllWindows()
class Track: def __init__(self, bb, id_, max_miss_): self.track = [bb] self.alive = True self.kf = KalmanFilter() self.id_track = id_ self.mean, self.cov = self.kf.initiate(bb.asp_ratio()) self.num_miss = 0 self.num_max_miss = max_miss_ self.project_mean = 0 self.color = (int(random.random() * 256), int(random.random() * 256), int(random.random() * 256)) def last(self): return self.track[-1] def get_last_mean(self): return self.mean[0:4] def append(self, bb): self.kalman_steps(bb.asp_ratio()) aproxBB = BoudingBox(coord=_restoreStandardValue(self.get_last_mean()), frame=bb.getIDFrame()) self.track.append(aproxBB) def kalman_steps(self, dets): self.mean, self.cov = self.kf.predict(self.mean, self.cov) self.mean, self.cov = self.kf.update(self.mean, self.cov, dets) def getTrack(self): return self.track def getBBFrame(self, frame_id): for bb in self.track: if bb.getIDFrame() == frame_id: return bb def __len__(self): return len(self.track) def is_alive(self): return self.alive def ressurect(self): self.alive = True def project_position(self): self.append( BoudingBox(_restoreStandardValue(self.mean[:4]), self.last().getIDFrame() + 1)) def missed(self): self.num_miss += 1 self.project_position() if self.num_miss > self.num_max_miss: self.kill() def kill(self): self.alive = False def getID(self): return self.id_track def getcolor(self): return self.color
tx= range(0,len(samples),25) for i in tx: tens.append(samples[i:(i+25)]) m=max(samples) print (m,samples.index(m)) m=min(samples) print (m,samples.index(m)) t=[mean(i) for i in tens ] s=[pstdev(i) for i in tens ] v=[pvariance(i) for i in tens] print(s) print(max(s),sum(s)) print(v) print(max(v),sum(v)) print(pstdev(samples)) kf = KalmanFilter(p=1000, r=100, q=0.5, k=0, initial_value=samples[0]) kf2 = KalmanFilter(p=300, r=100, q=0.1, k=0, initial_value=samples[0]) kf3 = KalmanFilter(p=1000, r=100, q=0.1, k=0, initial_value=samples[0]) f = [kf.update(i) for i in samples] f2 = [kf2.update(i) for i in samples] f3 = [kf3.update(i) for i in samples] plt.plot(x,samples,x,f,x,f2,tx,t,x,f3) plt.show()
class EKF: def __init__(self): self.is_initialized = False self.previous_timestamp = 0 # Initialize measurement covariance matrix - laser self.R_laser = np.array([[0.0225, 0.0], [0.0, 0.0225]], dtype=np.float32) # Initialize measurement covariance matrix - radar self.R_radar = np.array( [[0.09, 0.0, 0.0], [0.0, 0.0009, 0.0], [0.0, 0.0, 0.09]], dtype=np.float32) # Initialize state variable x_init = np.zeros(4, dtype=np.float32) # Initialize state covariance matrix P_init = np.array( [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float32) # System model - dt not yet taken into account F_init = np.array( [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=np.float32) # Transformation from state variable to measurement H_init = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float32) # Covariance matrix of process noise - dt not yet taken into account Q_init = np.array([ [1, 0, 1, 0], [0, 1, 0, 1], [1, 0, 1, 0], [0, 1, 0, 1], ], dtype=np.float32) # Initialize our Kalman filter self.ekf = KalmanFilter(x_init, P_init, F_init, H_init, self.R_laser, Q_init) # Set the process noise constants self.noise_ax = 9.0 self.noise_ay = 9.0 def process_measurement(self, m): if not self.is_initialized: # First measurement if m['sensor_type'] == 'R': # Convert radar data in polar to cartesian coordinates # (Note that rho_dot from the very first measurement is # not used)) px = m['rho'] * np.cos(m['phi']) py = m['rho'] * np.sin(m['phi']) self.ekf.x = np.array([px, py, 0.0, 0.0]) self.previous_timestamp = m['timestamp'] elif m['sensor_type'] == 'L': # Initialize state variable px, py = m['x'], m['y'] self.ekf.x = np.array([px, py, 0, 0]) self.previous_timestamp = m['timestamp'] self.is_initialized = True return # Prediction # Update state transition matrix (time measured in seconds) dt = (m['timestamp'] - self.previous_timestamp) / 1000000.0 self.previous_timestamp = m['timestamp'] self.ekf.F[0][2] = dt self.ekf.F[1][3] = dt # Set the process noise covariance dt2 = dt * dt dt3 = dt2 * dt dt4 = dt3 * dt self.ekf.Q = np.array( [[dt4 * self.noise_ax / 4.0, 0.0, dt3 * self.noise_ax / 2.0, 0.0], [0.0, dt4 * self.noise_ay / 4.0, 0.0, dt3 * self.noise_ay / 2.0], [dt3 * self.noise_ax / 2.0, 0.0, dt2 * self.noise_ax, 0.0], [0.0, dt3 * self.noise_ay / 2.0, 0.0, dt2 * self.noise_ay]], dtype=np.float32) self.ekf.predict() # Measurement update if m['sensor_type'] == 'R': # Radar updates z = np.array([m['rho'], m['phi'], m['rho_dot']], dtype=np.float32) self.ekf.R = self.R_radar self.ekf.update_ekf(z) elif m['sensor_type'] == 'L': # Laser updates z = np.array([m['x'], m['y']], dtype=np.float32) self.ekf.R = self.R_laser self.ekf.update(z)
def AMM_predict(self, interpolated_track_dict, x_in=np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose()): # ========== parameter setting ========== # x_in = np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose() # print('x_in',x_in) P_in = np.mat([[100.0, 0, 0, 0], [0, 100.0, 0, 0], [0, 0, 100.0, 0], [0, 0, 0, 100.0]]) H_in = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]]) R_in = np.mat([[0.1, 0], [0, 0.1]]) ## tuning ? t = 0.5 t_2 = t * t t_3 = t_2 * t t_4 = t_3 * t alpha_ax_2 = 2.25 * 2.25 alpha_ay_2 = 2.25 * 2.25 Q_in = np.mat([[t_4 / 4 * alpha_ax_2, t_3 / 2 * alpha_ax_2, 0, 0], [t_3 / 2 * alpha_ax_2, t_2 * alpha_ax_2, 0, 0], [0, 0, t_4 / 4 * alpha_ay_2, t_3 / 2 * alpha_ay_2], [0, 0, t_3 / 2 * alpha_ay_2, t_2 * alpha_ay_2]]) # ========== parameter setting ========== # # print('interpolated_track_dict', interpolated_track_dict) for track_id in interpolated_track_dict.keys(): track_ids = [] # print('track_id', track_id) track_ids.append(track_id) history_trajectory_x = interpolated_track_dict[track_id][0] # print('history_trajectory_x',history_trajectory_x) history_trajectory_y = interpolated_track_dict[track_id][1] # print('history_trajectory_y', history_trajectory_y) #x_in = np.mat([[history_trajectory_x[0], history_trajectory_y[0], 0.0, 0.0]]).transpose() kf0 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf1 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf2 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf3 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf4 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf5 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf6 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf7 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) kf8 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in) for i in range(len(history_trajectory_x) - 1, -1, -1): #===================== estimation done =================== # pose_xy = np.mat( [history_trajectory_x[i], history_trajectory_y[i]]) # print('pose_xy', pose_xy, i) kf0.predict('CV', t) kf0.update(pose_xy.transpose()) #need to transpose? gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf0.S_) L_cv = gaussain_distribution.pdf(kf0.y_.transpose()) L_cv = max(1e-30, L_cv) kf1.predict('CTA_a1', t) kf1.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf1.S_) L_cta_a1 = gaussain_distribution.pdf(kf1.y_.transpose()) L_cta_a1 = max(1e-30, L_cta_a1) kf2.predict('CTA_a2', t) kf2.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf2.S_) L_cta_a2 = gaussain_distribution.pdf(kf2.y_.transpose()) L_cta_a2 = max(1e-30, L_cta_a2) kf3.predict('CTA_d1', t) kf3.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf3.S_) L_cta_d1 = gaussain_distribution.pdf(kf3.y_.transpose()) L_cta_d1 = max(1e-30, L_cta_d1) kf4.predict('CTA_d2', t) kf4.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf4.S_) L_cta_d2 = gaussain_distribution.pdf(kf4.y_.transpose()) L_cta_d2 = max(1e-30, L_cta_d2) kf5.predict('CT_l1', t) kf5.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf5.S_) L_ct_l1 = gaussain_distribution.pdf(kf5.y_.transpose()) L_ct_l1 = max(1e-30, L_ct_l1) kf6.predict('CT_l2', t) kf6.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf6.S_) L_ct_l2 = gaussain_distribution.pdf(kf6.y_.transpose()) L_ct_l2 = max(1e-30, L_ct_l2) kf7.predict('CT_r1', t) kf7.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf7.S_) L_ct_r1 = gaussain_distribution.pdf(kf7.y_.transpose()) L_ct_r1 = max(1e-30, L_ct_r1) kf8.predict('CT_r2', t) kf8.update(pose_xy.transpose()) gaussain_distribution = scipy.stats.multivariate_normal([0, 0], kf8.S_) L_ct_r2 = gaussain_distribution.pdf(kf8.y_.transpose()) L_ct_r2 = max(1e-30, L_ct_r2) prob_sum = self.AMM_prob_dict['CV'] * L_cv + \ self.AMM_prob_dict['CTA_a1'] * L_cta_a1 + self.AMM_prob_dict['CTA_a2'] * L_cta_a2 + \ self.AMM_prob_dict['CTA_d1'] * L_cta_d1 + self.AMM_prob_dict['CTA_d2'] * L_cta_d2 + \ self.AMM_prob_dict['CT_l1'] * L_ct_l1 + self.AMM_prob_dict['CT_l2'] * L_ct_l2 + \ self.AMM_prob_dict['CT_r1'] * L_ct_r1 + self.AMM_prob_dict['CT_r2'] * L_ct_r2 #print("+++++++++++++++++++++") #print(self.AMM_prob_dict) self.AMM_prob_dict[ 'CV'] = self.AMM_prob_dict['CV'] * L_cv / prob_sum self.AMM_prob_dict['CTA_a1'] = self.AMM_prob_dict[ 'CTA_a1'] * L_cta_a1 / prob_sum self.AMM_prob_dict['CTA_a2'] = self.AMM_prob_dict[ 'CTA_a2'] * L_cta_a2 / prob_sum self.AMM_prob_dict['CTA_d1'] = self.AMM_prob_dict[ 'CTA_d1'] * L_cta_d1 / prob_sum self.AMM_prob_dict['CTA_d2'] = self.AMM_prob_dict[ 'CTA_d2'] * L_cta_d2 / prob_sum self.AMM_prob_dict[ 'CT_l1'] = self.AMM_prob_dict['CT_l1'] * L_ct_l1 / prob_sum self.AMM_prob_dict[ 'CT_l2'] = self.AMM_prob_dict['CT_l2'] * L_ct_l2 / prob_sum self.AMM_prob_dict[ 'CT_r1'] = self.AMM_prob_dict['CT_r1'] * L_ct_r1 / prob_sum self.AMM_prob_dict[ 'CT_r2'] = self.AMM_prob_dict['CT_r2'] * L_ct_r2 / prob_sum current_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \ self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \ self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \ self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \ self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_ # print('current_x_estimation', current_x_estimation) current_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_x_estimation - kf0.x_) * (current_x_estimation - kf0.x_).transpose()) + \ self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \ self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \ self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\ self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \ self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \ self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \ self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \ self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose()) #===================== estimation done =================== # # now lets predict: current_pose_xy = np.mat( [history_trajectory_x[0], history_trajectory_y[0]]) # print('current_pose_xy',current_pose_xy) kf0.predict('CV', self.period_of_predict) kf1.predict('CTA_a1', self.period_of_predict) kf2.predict('CTA_a2', self.period_of_predict) kf3.predict('CTA_d1', self.period_of_predict) kf4.predict('CTA_d2', self.period_of_predict) kf5.predict('CT_l1', self.period_of_predict) kf6.predict('CT_l2', self.period_of_predict) kf7.predict('CT_r1', self.period_of_predict) kf8.predict('CT_r2', self.period_of_predict) prob_sum_p = self.AMM_prob_dict['CV'] + \ self.AMM_prob_dict['CTA_a1'] + self.AMM_prob_dict['CTA_a2'] + \ self.AMM_prob_dict['CTA_d1'] + self.AMM_prob_dict['CTA_d2'] + \ self.AMM_prob_dict['CT_l1'] + self.AMM_prob_dict['CT_l2'] + \ self.AMM_prob_dict['CT_r1'] + self.AMM_prob_dict['CT_r2'] + 0.00000000000001 self.AMM_prob_dict['CV'] = self.AMM_prob_dict['CV'] / prob_sum_p self.AMM_prob_dict[ 'CTA_a1'] = self.AMM_prob_dict['CTA_a1'] / prob_sum_p self.AMM_prob_dict[ 'CTA_a2'] = self.AMM_prob_dict['CTA_a2'] / prob_sum_p self.AMM_prob_dict[ 'CTA_d1'] = self.AMM_prob_dict['CTA_d1'] / prob_sum_p self.AMM_prob_dict[ 'CTA_d2'] = self.AMM_prob_dict['CTA_d2'] / prob_sum_p self.AMM_prob_dict[ 'CT_l1'] = self.AMM_prob_dict['CT_l1'] / prob_sum_p self.AMM_prob_dict[ 'CT_l2'] = self.AMM_prob_dict['CT_l2'] / prob_sum_p self.AMM_prob_dict[ 'CT_r1'] = self.AMM_prob_dict['CT_r1'] / prob_sum_p self.AMM_prob_dict[ 'CT_r2'] = self.AMM_prob_dict['CT_r2'] / prob_sum_p predict_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \ self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \ self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \ self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \ self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_ predict_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_pose_xy - kf0.x_) * (current_pose_xy - kf0.x_).transpose()) + \ self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \ self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \ self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\ self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \ self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \ self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \ self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \ self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose()) self.predict_dict[track_id] = [ predict_x_estimation, predict_P_estimation ]
def main(): R_values = [0.001, 0.1, 1, 10, 1000] Q_values = [0.001, 0.1, 1, 10, 1000] parser = argparse.ArgumentParser( description="Run SSD on input folder and show result in popup window") parser.add_argument("object_detector", choices=['ssd', 'yolo_full', 'yolo_tiny'], help="Specify which object detector network should be used") parser.add_argument("test", choices=['Q', 'R'], help="Which noise matrix should be tested") args = parser.parse_args() if args.object_detector == 'ssd': fd = SSDObjectDetection(frozen, pbtxt) elif args.object_detector == 'yolo_full': fd = YOLOObjectDetection('full') elif args.object_detector == 'yolo_tiny': fd = YOLOObjectDetection('tiny') # Config should_plot = False # Get images list from dataset dataset = "data/TinyTLP/" for path, directories, files in os.walk(dataset): all_directories = directories break results = {} if args.test == 'R': k = 2 Q_value = 1 for l, R_value in enumerate(R_values): for dir in all_directories: results[dir] = [] ground_truth_file = dataset + dir + "/groundtruth_rect.txt" images_wildcard = dataset + dir + "/img/*.jpg" images_filelist = glob(images_wildcard) # Sort them in ascending order # images_filelist = sorted(images_filelist, key=lambda xx: int( # xx.split('/')[-1].split('.')[0])) # Extract all ground truths ground_truth = list(csv.reader(open(ground_truth_file))) gt_measurements = [] for row in ground_truth: gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)])) initial_position = ground_truth[0] frame_id, x, y, w, h, is_lost = initial_position x = int(x) y = int(y) w = int(w) h = int(h) kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value) # Iterate of every image features = {} t = tqdm(images_filelist[1:], desc="Processing") da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1) plt.ion() for i, im in enumerate(t): img = plt.imread(images_filelist[i]) height, width, _ = img.shape # Compute features features[i] = np.array(fd.compute_features(img)) # Do prediction mu_bar, Sigma_bar = kf.predict() # Do data association da.update_prediction(mu_bar, Sigma_bar) m = da.associate(features[i]) kf.update(m) gt = list(map(int, ground_truth[i])) kf_x = kf.get_x() if should_plot: x, y = np.mgrid[0:width, 0:height] pos = np.empty(x.shape + (2,)) pos[:, :, 0] = x pos[:, :, 1] = y rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]]) f = rv.pdf(pos) f[f < 1e-5] = np.nan plt.gca() plt.cla() plt.imshow(img) plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5) if m is not None: plt.plot(m[0], m[1], marker='o', color='yellow') plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red') plt.pause(0.0001) print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}") results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])) print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}") with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp: pickle.dump(results, fp) elif args.test == 'Q': l = 2 R_value = 1 for k, Q_value in enumerate(Q_values): for dir in all_directories: results[dir] = [] ground_truth_file = dataset + dir + "/groundtruth_rect.txt" images_wildcard = dataset + dir + "/img/*.jpg" images_filelist = glob(images_wildcard) # Sort them in ascending order # images_filelist = sorted(images_filelist, key=lambda xx: int( # xx.split('/')[-1].split('.')[0])) # Extract all ground truths ground_truth = list(csv.reader(open(ground_truth_file))) gt_measurements = [] for row in ground_truth: gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)])) initial_position = ground_truth[0] frame_id, x, y, w, h, is_lost = initial_position x = int(x) y = int(y) w = int(w) h = int(h) kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value) # Iterate of every image features = {} t = tqdm(images_filelist[1:], desc="Processing") da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1) plt.ion() for i, im in enumerate(t): img = plt.imread(images_filelist[i]) height, width, _ = img.shape # Compute features features[i] = np.array(fd.compute_features(img)) # Do prediction mu_bar, Sigma_bar = kf.predict() # Do data association da.update_prediction(mu_bar, Sigma_bar) m = da.associate(features[i]) kf.update(m) gt = list(map(int, ground_truth[i])) kf_x = kf.get_x() if should_plot: x, y = np.mgrid[0:width, 0:height] pos = np.empty(x.shape + (2,)) pos[:, :, 0] = x pos[:, :, 1] = y rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]]) f = rv.pdf(pos) f[f < 1e-5] = np.nan plt.gca() plt.cla() plt.imshow(img) plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5) if m is not None: plt.plot(m[0], m[1], marker='o', color='yellow') plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red') plt.pause(0.0001) print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}") results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])) print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}") with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp: pickle.dump(results, fp)
# While testing, don't run on too many frames iteration = 0 while is_frame_good and iteration <= 200: # Read a frame is_frame_good, frame = video.read() if not is_frame_good: break # To generate static image directory for use with StaticCapture # cv2.imwrite(f'outimgs/out{iteration}.jpg', frame) # iteration += 1 # continue # Compute Sift predicted bounding_box = sift.ProcessImg(frame, bounding_box) # Apply Kalman Filter bounding_box = kf.update(bounding_box) # Convert to int bounding_box = bounding_box.astype(np.int64) # Draw bounding box frame = DrawRectangle(frame, bounding_box) # Show image cv2.imshow('Frame', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break iteration += 1
yg += d_yg * 1000 if img_id < 2: angle = np.arctan2(xg, yg) if np.rad2deg(angle) < 0: sign = -1 else: sign = 1 continue prev_lat = lat prev_lon = lon xg_rot, yg_rot = rotate(xg, yg, -(np.pi / 2 - angle)) kalman_y.update(xg_rot) kalman_x.update(yg_rot * sign) trajectory.append([ kalman_x.x, kalman_y.x, -model.yc, model.xc, yg_rot * sign, xg_rot, x, z, xg, yg ]) draw_xg, draw_yg = int(xg_rot + 90), int(yg_rot * sign + 290) draw_x, draw_y = int(x) + 290, int(z) + 90 draw_xb, draw_yb = -int(model.yc) + 290, int(model.xc) + 90 draw_kalman_x, draw_kalman_y = int(kalman_x.x) + 290, int(kalman_y.x) + 90 #print(kalman_y.P, kalman_x.P) print(sign) #cv2.circle(traj, (draw_x, draw_y), 1, (255, 0, 0), 1)
# init x = np.array([0, 0]) P = np.array([[0, 0], [0, 0]]) kf = KalmanFilter(F, H, Q, R) cart = Cart() vs = [0] # filterd v ws = [0] # true v xs = [0] # filterd x ys = [0] # true x zs = [0] # observed x for _ in range(100): y, z, w = cart() x, P = kf.predict(x, P) x, P = kf.update(x, P, z) vs.append(x[1]) ws.append(w) xs.append(x[0]) ys.append(y) zs.append(z) # plot graph figs, axes = plt.subplots(1, 2) axes[1].plot(vs, label='filterd v', marker='.') axes[1].plot(ws, label='true v', marker='x') axes[0].plot(xs, label='filterd x', marker='.') axes[0].plot(ys, label='true x', marker='x') axes[0].plot(zs, label='observed x', marker='+') axes[0].set_ylabel('x')
# plt.draw() # plt.pause(0.1) # input() kf = KalmanFilter(dim=3, x=mu, P=sig, R=measurement_sig, Q=motion_sig) ## TODO: Loop through all measurements/motions # this code assumes measurements and motions have the same length # so their updates can be performed in pairs for n in range(measurements.shape[0]): # motion update, with uncertainty mu, sig = kf.predict(Q=motion_sig) print('Predict: {} \n{}\n\n'.format(mu, sig)) # measurement update, with uncertainty mu, sig = kf.update(y=measurements[n], R=measurement_sig) print('Update: {} \n{}\n\n'.format(mu, sig)) # print (mu) measurement_sig -= np.eye( 3 ) * 0.25 # Assumes noise reduces; keep this constant by commenting out this line if needed. if PLOT: plt.cla() ax.axvline(c='grey', lw=1) ax.axhline(c='grey', lw=1) # g = [] # for x in x_axis: # g.append(f(mu, sig, x).flatten()) # ax.plot(np.linspace(-1,1,10),np.linspace(-1,1,10))
for labeled_object in tracked_objects[track_id]: z = labeled_object.bbox print("z:") print(z) top_left_x = float(z[0]) top_left_y = float(z[1]) bottom_right_x = float(z[2]) bottom_right_y = float(z[3]) centre_x = (top_left_x + bottom_right_x) / 2 centre_y = (top_left_y + bottom_right_y) / 2 measurements = np.zeros((2, 1)) measurements[0] = centre_x measurements[1] = centre_y print("measurements:") print(z) x, P = filter.predict() gt_points_x.append(measurements[0]) gt_points_y.append(measurements[1]) kalman_points_x.append(x[0]) kalman_points_y.append(x[1]) #print("x:") print(labeled_object.bbox) filter.update(measurements) plt.scatter(gt_points_x, gt_points_y) plt.scatter(kalman_points_x, kalman_points_y) plt.show() break