def test(x, y, z, qcov, rcov): # Initializations dt = 1 # Measurement interval (s) I = lambda x: identity(x) A = zeros((9, 9)) # Transition matrix A[0:3, 0:3] = I(3) A[0:3, 3:6] = I(3) * dt A[0:3, 6:9] = I(3) * 0.5 * dt * dt A[3:6, 3:6] = I(3) A[3:6, 6:9] = I(3) * dt A[6:9, 6:9] = I(3) H = zeros((3, 9)) # Measurement matrix H[0:3, 0:3] = I(3) Q = identity(9) * qcov # Transition covariance R = identity(3) * rcov # Noise covariance B = identity(9) # Control matrix kf = KalmanFilter(A, H, Q, R, B) # Run through the dataset n = len(x) xkf, ykf, zkf = zeros(n), zeros(n), zeros(n) for i in xrange(n): kf.correct(array([x[i], y[i], z[i]])) kf.predict(array([])) Skf = kf.getCurrentState() xkf[i], ykf[i], zkf[i] = Skf[0], Skf[1], Skf[2] return xkf, ykf, zkf
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 UltraSoundFilter(object): def __init__(self, us): self.ultra_sound = us self.kalman_filter = None self.last_update_time = None self.calibration_time = 3.0 self.calibration_range = None self.calibration_std = None def calibrate(self): """ Run initialization procedure on empty scene. Determine mean and std of dists within calibration_time. Can block for a while.""" ranges = [] start_time = time.time() while time.time() - start_time <= self.calibration_time: dist = self.ultra_sound.get_distance() time.sleep(0.05) if dist is not None: ranges.append(dist) if len(ranges) <= 1: raise RuntimeError("No valid ranges during calibration.") return self.calibration_range = sum(ranges) / len(ranges) sqr_error = [(dist - self.calibration_range)**2 for dist in ranges] self.calibration_std = math.sqrt(sum(sqr_error) / (len(ranges) - 1)) logger.info( f"Calibrated range as {self.calibration_range:.2f}m +- {self.calibration_std:.3f}m" ) self.kalman_filter = KalmanFilter(self.calibration_range, self.calibration_std) def update(self): """ Update the internal feature based on sensor data. """ cur_dist = self.ultra_sound.get_distance() now = time.time() if cur_dist is None: return if self.last_update_time is None: delta_t = 0 else: delta_t = now - self.last_update_time self.kalman_filter.predict(delta_t) self.kalman_filter.correct(cur_dist) self.last_update_time = now def get_distance(self): if self.kalman_filter is None: return None return self.kalman_filter.distance()
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)
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 Tracklet: STATUS_BORN = 0 STATUS_TRACKING = 1 STATUS_LOST = 2 def __init__(self, global_id, x, y, confidence, timestamp): self.id = global_id self.last_update = timestamp self.estimator = KalmanFilter(x, y, 1 - confidence) self.status = Tracklet.STATUS_BORN def get_tracklet_info(self): return { 'id': self.id, 'x': self.estimator.mu[0], 'y': self.estimator.mu[2], 'v': sqrt(self.estimator.mu[1]**2 + self.estimator.mu[3]**2), 'phi': atan2(self.estimator.mu[1], self.estimator.mu[3]), 'sigma_x': self.estimator.sigma[0][0], 'sigma_y': self.estimator.sigma[2][2], 'status': self.status } def estimate_position_at(self, timestamp): estimate = self.estimator.estimate_at(timestamp - self.last_update)[0] return estimate[0], estimate[2] def predict(self, timestamp): self.estimator.predict(math.fabs(self.last_update - timestamp)) self.last_update = timestamp def update(self, detection, confidence): self.estimator.correct(detection, 1 - confidence) def similarity(self, coordinates, timestamp): return euclidean(self.estimate_position_at(timestamp), coordinates) def confidence(self): return self.estimator.sigma[0][0] * self.estimator.sigma[2][2]
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)
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 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 KalmanFilterNode(object): """ ROS node implementation of Kalman filter. This node subscribes to a list of all existing Sphero's positions broadcast from OptiTrack system, associates one of them to the Sphero in the same namespace and uses Kalman filter to output steady position and velocity data for other nodes. """ def sensor_callback(self, data): """Process received positions data and return Kalman estimation. :type data: Odometry """ if self.initial_run: # Initialize Kalman filter and estimation. initial_pos = data.pose.pose self.filter = KalmanFilter(1.0 / self.sub_frequency, initial_pos) self.X_est = Odometry() self.X_est.pose.pose = initial_pos self.initial_run = False else: X_measured = data.pose.pose time = data.header.stamp # If measurement data is not available, use only prediction step. # Else, use prediction and update step. if X_measured is None: self.X_est = self.filter.predict() else: self.X_est = self.filter.predict_update(X_measured) self.X_est.header.stamp = time if self.debug_enabled: self.debug_pub.publish(self.X_est) def __init__(self): """Initialize agent instance, create subscribers and publishers.""" # Initialize class variables. self.sub_frequency = rospy.get_param('/data_stream_freq') self.pub_frequency = rospy.get_param('/ctrl_loop_freq') self.debug_enabled = rospy.get_param('/debug_kalman') self.X_est = None # Create publishers. pub = rospy.Publisher('odom_est', Odometry, queue_size=self.pub_frequency) if self.debug_enabled: self.debug_pub = rospy.Publisher('debug_est', Odometry, queue_size=self.sub_frequency) # Create subscribers. self.initial_run = True rospy.Subscriber('odom', Odometry, self.sensor_callback, queue_size=self.sub_frequency) # Don't try to publish anything before variable is set. while self.X_est is None: rospy.sleep(0.1) # Main while loop. rate = rospy.Rate(self.pub_frequency) while not rospy.is_shutdown(): pub.publish(self.X_est) rate.sleep()
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)
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)
log.new_log('actual') log.new_log('time') 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())
[0, 0, 0, sigma_v**2]]) x_init = np.array([[0, 2, 2, 5]]).transpose() kf = KalmanFilter(A, B, G, H, Q, R, x_init, Sigma_init) u = np.array([[0, -0.5*g*delta_t**2, 0, -g*delta_t]]).transpose() 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])
# 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='+')
lastYTilt = evDevValue elif evDevCode == WACOM_EVCODE_DISTANCE: lastDistance = evDevValue elif evDevCode == WACOM_EVCODE_PRESSURE: if not ONLY_DEBUG: if not mouseButtonPressed and evDevValue > SCREEN_DRAW_BUTTON_PRESSURE: mouse.press(Button.left) mouseButtonPressed = True elif mouseButtonPressed and evDevValue <= SCREEN_DRAW_BUTTON_PRESSURE: mouse.release(Button.left) mouseButtonPressed = False lastPressure = evDevValue if ONLY_DEBUG: print('XPos: %5d | YPos: %5d | XTilt: %5d | YTilt: %5d | Distance: %3d | Pressure: %4d' % (lastXPos, lastYPos, lastXTilt, lastYTilt, lastDistance, lastPressure)) else: screenY = SCREEN_DRAW_AREA_FROM_X + (WACOM_WIDTH - lastXPos) * ratioX # (X doesn't need to invert) screenX = SCREEN_DRAW_AREA_FROM_Y + (WACOM_HEIGHT - lastYPos) * ratioY # (Y has to be inverted) currentMeasurement = np.array([[np.float32(screenX)], [np.float32(screenY)]]) currentPrediction = kalman.predict() cmx, cmy = currentMeasurement[0], currentMeasurement[1] cpx, cpy = currentPrediction[0], currentPrediction[1] kalman.correct(currentMeasurement) mouseMoveAbs(int(np.round(cpx)), int(np.round(cpy)))
class FilterNode(): def __init__(self, ): rp.init_node("filter") # Get rate of state publisher self.rate = rp.get_param("rate", 100) # Get VRPN client topic self.vrpn_topic = rp.get_param("vrpn_topic") # Number of states self.n = 12 # System state self.X = np.matrix(np.zeros((self.n, 1))) # Initial State Transition Matrix self.F = np.asmatrix(np.eye(self.n)) # Initial Process Matrix self.P = np.asmatrix(1.0e3 * np.eye(self.n)) # Process Noise Level self.N = 1.0e-3 # Initialize H and R matrices for optitrack pose self.Hopti = np.matrix(np.zeros((6, self.n))) self.Hopti[0:3, 0:3] = np.matrix(np.eye(3)) self.Hopti[3:6, 6:9] = np.matrix(np.eye(3)) self.Ropti = np.matrix(np.zeros((6, 6))) self.Ropti[0:3, 0:3] = 1.0e-3 * np.matrix(np.eye(3)) self.Ropti[3:6, 3:6] = 1.0e-5 * np.matrix(np.eye(3)) # Initialize Kalman Filter self.kalmanF = KalmanFilter() self.isInit = False self.lastTime = None # Set up Subscriber self.vrpn_sub = rp.Subscriber(self.vrpn_topic, PoseStamped, self.state_callback, queue_size=1) # Set up Publisher self.state_pub = rp.Publisher("opti_state/pose", PoseStamped, queue_size=1) self.state_rate_pub = rp.Publisher("opti_state/rates", TwistStamped, queue_size=1) # Create thread for publisher t = threading.Thread(target=self.statePublisher) t.start() rp.spin() def state_callback(self, msg): attiQ = Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w) rpy = quat2rpy(attiQ) pose = np.matrix([[msg.pose.position.x], [msg.pose.position.y], [msg.pose.position.z], [rpy[0]], [rpy[1]], [rpy[2]]]) if self.isInit: timeNow = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs dt = timeNow - self.lastTime # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Correction self.kalmanF.correct(pose, self.Hopti, self.Ropti) # Update state self.X = self.kalmanF.getState() self.lastTime = timeNow else: # Initialize state self.X[0] = pose[0] self.X[1] = pose[1] self.X[2] = pose[2] self.X[6] = pose[3] self.X[7] = pose[4] self.X[8] = pose[5] # Initialize filter self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.lastTime = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs self.isInit = True def statePublisher(self): r = rp.Rate(self.rate) while not rp.is_shutdown(): if self.isInit: timeNow = rp.Time.now() stateMsg = PoseStamped() stateMsg.header.stamp = timeNow stateMsg.header.frame_id = 'optitrack' stateMsg.pose.position.x = self.X.item(0) stateMsg.pose.position.y = self.X.item(1) stateMsg.pose.position.z = self.X.item(2) q = rpy2quat(self.X.item(6), self.X.item(7), self.X.item(8)) stateMsg.pose.orientation.x = q.x stateMsg.pose.orientation.y = q.y stateMsg.pose.orientation.z = q.z stateMsg.pose.orientation.w = q.w twistMsg = TwistStamped() twistMsg.header.stamp = timeNow twistMsg.header.frame_id = 'optitrack' twistMsg.twist.linear.x = self.X.item(3) twistMsg.twist.linear.y = self.X.item(4) twistMsg.twist.linear.z = self.X.item(5) twistMsg.twist.angular.x = self.X.item(9) twistMsg.twist.angular.y = self.X.item(10) twistMsg.twist.angular.z = self.X.item(11) self.state_pub.publish(stateMsg) self.state_rate_pub.publish(twistMsg) r.sleep()
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 ]
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)
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)
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 FilterNode(): def __init__(self): # Number of states self.n = 12 # System state self.X = np.matrix(np.zeros((self.n, 1))) # Initial State Transition Matrix self.F = np.asmatrix(np.eye(self.n)) self.F[3:6, 3:6] = 0.975 * np.matrix(np.eye(3)) self.F[9:12, 9:12] = 0.975 * np.matrix(np.eye(3)) # Initial Process Matrix self.P = np.asmatrix(1.0e3 * np.eye(self.n)) # Process Noise Level self.N = 1.0e-3 # Initialize H and R matrices for optitrack pose self.Hopti = np.matrix(np.zeros((6, self.n))) self.Hopti[0:3, 0:3] = np.matrix(np.eye(3)) self.Hopti[3:6, 6:9] = np.matrix(np.eye(3)) self.Ropti = np.matrix(np.zeros((6, 6))) self.Ropti[0:3, 0:3] = 1.0 * 1.0e-3 * np.matrix(np.eye(3)) self.Ropti[3:6, 3:6] = 1.0 * 1.0e-2 * np.matrix(np.eye(3)) # Initialize Kalman Filter self.kalmanF = KalmanFilter() self.isInit = False self.lastTime = None def state_update(self, T, R, mTime): attiQ = Quaternion(R[0], R[1], R[2], R[3]) rpy = quat2rpy(attiQ) pose = np.matrix([[T[0]], [T[1]], [T[2]], [rpy[0]], [rpy[1]], [rpy[2]]]) if self.isInit: dt = mTime - self.lastTime # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Correction self.kalmanF.correct(pose, self.Hopti, self.Ropti) # Update state self.X = self.kalmanF.getState() self.lastTime = mTime else: # Initialize state self.X[0] = pose[0] self.X[1] = pose[1] self.X[2] = pose[2] self.X[6] = pose[3] self.X[7] = pose[4] self.X[8] = pose[5] # Initialize filter self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.lastTime = mTime self.isInit = True def get_state(self): if self.isInit: timeNow = time.time() dt = timeNow - self.lastTime self.lastTime = timeNow # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Update state self.X = self.kalmanF.getState() return self.X else: return None
class FusionEKF: """ Gets inputs from multiple sources and uses a Kalman Filter to estimate the state of the system The state of the system is: X = {pD, dotpD, attD, dotattD, pJ, dotpJ, headJ, GSPoffsetJ, compassOffsetJ} The inputs are: - Drone's position in local ENU - Drone's velocity in local ENU - Drone's attitude - Drone's angular velocities - Jetyak's position in local ENU - Jetyak's heading - Jetyak's position in the Drone's body frame """ def __init__(self, F, P, N, rate): self.kalmanF = KalmanFilter() self.F = F self.P = P self.N = N self.n = np.shape(F)[0] self.X = np.matrix(np.zeros((self.n, 1))) self.dt = 1 / rate self.isInit = False self.X[0:3] = np.nan # Drone's position self.X[14:17] = np.nan # Jetyak's position self.X[19:24] = np.nan # Jetyak's GPS and heading offset def initialize(self, dataPoint): if dataPoint.getID() == 'dgps': self.X[0] = dataPoint.getZ().item(0) self.X[1] = dataPoint.getZ().item(1) self.X[2] = dataPoint.getZ().item(2) elif dataPoint.getID() == 'jgps': self.X[6] = dataPoint.getZ().item(0) self.X[7] = dataPoint.getZ().item(1) self.X[8] = dataPoint.getZ().item(2) elif dataPoint.getID() == 'tag': if (not (np.isnan(self.X[0]) or np.isnan(self.X[6]))): self.X[11] = self.X[6] - self.X[0] - dataPoint.getZ().item(0) self.X[12] = self.X[7] - self.X[1] - dataPoint.getZ().item(1) self.X[13] = self.X[8] - self.X[2] - dataPoint.getZ().item(2) self.X[14] = dataPoint.getZ().item(3) self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.kalmanF.updateF(self.dt) self.kalmanF.updateQ() self.kalmanF.predict() self.isInit = True print 'Colocalization filter initialized' def process(self, dataPoint): if not self.isInit: self.initialize(dataPoint) return None, None else: r = None P = None # KF Correction Step r, P = self.kalmanF.correct(dataPoint.getZ(), dataPoint.getH(), dataPoint.getR(), dataPoint.getChi()) if r is None: print "A ", dataPoint.getID(), " measurement was rejected" self.X = self.kalmanF.getState() return r, P def getState(self): if self.isInit: self.kalmanF.predict() return self.X else: return None def resetFilter(self): self.isInit = False self.X = np.matrix(np.zeros((self.n, 1))) self.X[0:3] = np.nan # Drone's position self.X[14:17] = np.nan # Jetyak's position self.X[19:24] = np.nan # Jetyak's GPS and heading offset
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)
red_rgb = (255, 0, 0) # light_blue_rgb = (173,216,230) draw_rectangle(rgb_frame, x1, y1, x2, y2, green_rgb, 1) cx, cy = get_center_rect(x1, y1, x2, y2) tracked_points.append([cx, cy]) if len(tracked_points) % nth_point_fade == 1 and len( tracked_points) > nth_point_fade: tracked_points.popleft() for _point in tracked_points: current_state_measurement = np.array([[_point[0]], [_point[1]]]) tracker_point = kf.predict() _ = kf.correct(current_state_measurement) # draw_circle(rgb_frame, _point[0], _point[1], 3, red_rgb) draw_circle(rgb_frame, tracker_point[0], tracker_point[1], 3, red_rgb) bgr_frame = cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR) cv2.imshow('frame', bgr_frame) if cv2.waitKey(28) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()
N_STEPS = 300 counter = 0 trajectory = [] for img_id in range(len(df['filename'].values)): img_path = os.path.join(base_path, df['filename'].values[img_id]) img = cv2.imread(img_path, 0) v, delta, current_time = df[['speed', 'angle', 'timestamp']].values[img_id] #acc_idx = abs(ins['timestamp'].values - current_time).argmin() #acc_x = ins['ax'].values[acc_idx] #acc_y = -ins['ay'].values[acc_idx] #print(v, delta) dy_b, dx_b = model.step(v, delta, current_time) kalman_y.predict(dy_b) kalman_x.predict(-dx_b) # Visual Odometery update vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): x, y, z = cur_t[0], cur_t[1], cur_t[2] else: x, y, z = 0., 0., 0. vo.x, vo.y, vo.z = x, y, z lat, lon = df[['lat', 'long']].values[img_id] d_xg, d_yg = latlon_to_xy_grid(lat, lon, prev_lat, prev_lon)
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
fig = plt.figure(figsize=plt.figaspect(1)) # Square figure ax = fig.add_subplot(111, projection='3d') # x_axis = np.arange(-20, 20, 0.1) # 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:
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