def initialize_kalman_filter(self, cnt): from kalman_filter import KalmanFilter x, y, w, h = cv2.boundingRect(cnt) self.xKalmanFilter = KalmanFilter(np.array([[x], [0]])) self.yKalmanFilter = KalmanFilter(np.array([[y], [0]])) self.wKalmanFilter = KalmanFilter(np.array([[w], [0]])) self.hKalmanFilter = KalmanFilter(np.array([[h], [0]]))
class LQGController: controlForce = 0.0 def __init__(self, inertia, damping, stiffness, timeStep): self.dt = timeStep self.observer = KalmanFilter(3, 3, 1) self.observer.SetMeasurement(0, 0, 1.0) self.observer.SetMeasurement(1, 1, 1.0) self.observer.SetMeasurement(2, 2, 1.0) self.observer.SetStatePredictionFactor(0, 1, self.dt) self.observer.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2) self.observer.SetStatePredictionFactor(1, 2, self.dt) self.observer.SetStatePredictionFactor(2, 2, 0.0) self.SetSystem(inertia, damping, stiffness) def SetSystem(self, inertia, damping, stiffness): A = [[1, self.dt, 0.5 * self.dt**2], [0, 1, self.dt], [-stiffness / inertia, -damping / inertia, 0]] B = [[0], [0], [1 / inertia]] C = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] self.feedbackGain = GetLQRController(A, B, C, 0.0001) self.observer.SetStatePredictionFactor(2, 0, -stiffness / inertia) self.observer.SetStatePredictionFactor(2, 1, -damping / inertia) self.observer.SetInputPredictionFactor(2, 0, 1 / inertia) def Process(self, setpoint, measurement, externalForce): reference = [ measurement[0] - setpoint[0], measurement[1] - setpoint[1], measurement[2] - setpoint[2] ] state, error = self.observer.Process( reference, [self.controlForce + externalForce]) self.controlForce = -self.feedbackGain.dot(state)[0] return self.controlForce
def __init__( self, impedance, timeStep ): self.waveController = WaveController( impedance[ 0 ] + impedance[ 1 ] + impedance[ 2 ], timeStep ) self.dt = timeStep self.waveObserver = KalmanFilter( 2, 2 ) self.waveObserver.SetMeasurement( 0, 0, 4.0 ) self.waveObserver.SetMeasurement( 1, 1, 4.0 ) self.waveObserver.SetStatePredictionFactor( 0, 1, self.dt )
class WavePredTeleoperator: remotePosition = 0.0 outputWaveIntegral = 0.0 def __init__( self, impedance, timeStep ): self.waveController = WaveController( impedance[ 0 ] + impedance[ 1 ] + impedance[ 2 ], timeStep ) self.dt = timeStep self.waveObserver = KalmanFilter( 2, 2 ) self.waveObserver.SetMeasurement( 0, 0, 4.0 ) self.waveObserver.SetMeasurement( 1, 1, 4.0 ) self.waveObserver.SetStatePredictionFactor( 0, 1, self.dt ) def SetSystem( self, impedance ): waveImpedance = ( max( impedance[ 0 ], 0.0 ), max( impedance[ 1 ], 1.0 ), max( impedance[ 2 ], 0.0 ) ) self.waveController.SetImpedance( waveImpedance[ 0 ] + waveImpedance[ 1 ] + waveImpedance[ 2 ] ) def Process( self, localState, localForce, remotePacket, timeDelay ): localPosition, localVelocity, localAcceleration = localState inputWave, inputWaveIntegral, inputEnergy, remoteImpedance = remotePacket remoteState, predictedWave = self.waveObserver.Process( [ inputWaveIntegral + inputWave * timeDelay, inputWave ] ) remoteVelocity = self.waveController.PreProcess( predictedWave[ 1 ], inputEnergy, remoteImpedance ) feedbackForce, outputWave, outputEnergy, localImpedance = self.waveController.PostProcess( localVelocity, localForce ) self.remotePosition += remoteVelocity * self.dt self.outputWaveIntegral += outputWave * self.dt return ( feedbackForce, ( self.remotePosition, remoteVelocity, 0.0 ), ( outputWave, self.outputWaveIntegral, outputEnergy, localImpedance ) )
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 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)
class KalmanPredictor: predictions = [] def __init__(self, timeStep): self.dt = timeStep self.remoteObserver = KalmanFilter(3, 3) self.remoteObserver.SetMeasurement(0, 0, 1.0) self.remoteObserver.SetMeasurement(1, 1, 1.0) self.remoteObserver.SetMeasurement(2, 2, 1.0) self.remoteObserver.SetPredictionNoise(0, 2.0) self.remoteObserver.SetPredictionNoise(1, 2.0) self.remoteObserver.SetPredictionNoise(2, 2.0) def Process(self, inputPacket, remoteTime, timeDelay): setpoint, setpointVelocity, setpointAcceleration = inputPacket measurement = [setpoint, setpointVelocity, setpointAcceleration] timeDelay = int(timeDelay / self.dt) * self.dt self.remoteObserver.SetStatePredictionFactor(0, 1, timeDelay) self.remoteObserver.SetStatePredictionFactor(0, 2, 0.5 * timeDelay**2) self.remoteObserver.SetStatePredictionFactor(1, 2, timeDelay) state, prediction = self.remoteObserver.Predict(measurement) if len(self.predictions) > 0: if remoteTime >= self.predictions[0][1]: estimatedMeasurement = self.predictions.pop(0)[0] self.remoteObserver.Update(measurement, estimatedMeasurement) self.remoteObserver.Correct() self.predictions.append((prediction, remoteTime + timeDelay)) return [prediction[0], prediction[1], prediction[2]]
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 _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 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): 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 __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 __init__(self, detection, trackId): super(Tracks, self).__init__() self.KF = KalmanFilter() self.KF.predict_state_vector() self.KF.correct_state_vector(np.matrix(detection).reshape(2, 1)) self.trace = deque(maxlen=20) self.prediction = detection.reshape(1, 2) self.trackId = trackId self.skipped_frames = 0
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 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 __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 __init__(self, timeStep): self.dt = timeStep self.remoteObserver = KalmanFilter(3, 3) self.remoteObserver.SetMeasurement(0, 0, 1.0) self.remoteObserver.SetMeasurement(1, 1, 1.0) self.remoteObserver.SetMeasurement(2, 2, 1.0) self.remoteObserver.SetPredictionNoise(0, 2.0) self.remoteObserver.SetPredictionNoise(1, 2.0) self.remoteObserver.SetPredictionNoise(2, 2.0)
def __init__(self, inertia, damping, stiffness, timeStep): self.dt = timeStep self.localController = LQGController(inertia, damping, stiffness, timeStep) self.remoteStateObserver = KalmanFilter(3, 3) self.remoteStateObserver.SetMeasurement(0, 0, 4.0) self.remoteStateObserver.SetMeasurement(1, 1, 4.0) self.remoteStateObserver.SetMeasurement(2, 2, 4.0) self.remoteInputObserver = KalmanFilter(3, 1) self.remoteInputObserver.SetMeasurement(0, 0, 1.0)
def calculateKalman(measurements): kf = KalmanFilter(dt=1, zdims=2, r=1000, q=0.0000001, xvals=3) results = [] print "Calculating..." for measurement in measurements: measArray = np.asarray([[measurement[0]], [measurement[1]]]) x = kf.run(measArray) results.append((x[:2]).tolist()) return results
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
def __init__(self, metric, max_iou_distance=0.7, max_age=70, n_init=3): self.metric = metric self.max_iou_distance = max_iou_distance self.max_age = max_age self.n_init = n_init self.kf = KalmanFilter() self.tracks = [] self._next_id = 1 self.last_confirm_id = 1
def __init__(self, inertia, damping, stiffness, timeStep): self.dt = timeStep self.observer = KalmanFilter(3, 3, 1) self.observer.SetMeasurement(0, 0, 1.0) self.observer.SetMeasurement(1, 1, 1.0) self.observer.SetMeasurement(2, 2, 1.0) self.observer.SetStatePredictionFactor(0, 1, self.dt) self.observer.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2) self.observer.SetStatePredictionFactor(1, 2, self.dt) self.observer.SetStatePredictionFactor(2, 2, 0.0) self.SetSystem(inertia, damping, stiffness)
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))
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)
def compute_track(video): track = [] proposals = [] object_detected = False kalman_filter = None for t, im in enumerate(video): scores, bboxes = im_detect(sess, net, im) bboxes, scores = filter_proposals(scores, bboxes, nms_thresh=NMS_THRESH, conf_thresh=CONF_THRESH) affinities = np.zeros(scores.shape) if not object_detected: if bboxes.shape[0] == 0: track.append(np.zeros(6)) proposals.append(np.zeros((1, 6))) continue else: object_detected = True i = np.argmax(scores) xhat_0 = bboxes[i] kalman_filter = KalmanFilter(xhat_0) track.append( np.concatenate((bboxes[i], scores[i], affinities[i]))) proposals.append( np.concatenate([bboxes, scores, affinities], axis=1)) continue xhat, P = kalman_filter.update_time() score, affinity = np.zeros(1), np.zeros(1) if bboxes.shape[0] > 0: track_im = crop_and_resize(im, track[-1][:4]) bbox_ims = [crop_and_resize(im, bbox) for bbox in bboxes] affinities = [ get_affinity(track_im, bbox_im) for bbox_im in bbox_ims ] affinities = np.reshape(affinities, scores.shape) i, aff = np.argmax(affinities), np.max(affinities) if aff > AFF_THRESH: xhat, P = kalman_filter.update_measurement(bboxes[i][:4]) score, affinity = scores[i], affinities[i] track.append(np.concatenate((xhat, score, affinity))) proposals.append(np.concatenate((bboxes, scores, affinities), axis=1)) return track, proposals
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()
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 __init__(self, opt, frame_rate=30): self.opt = opt if opt.gpus[0] >= 0: opt.device = torch.device('cuda') else: opt.device = torch.device('cpu') print('Creating model...') self.model = create_model(opt.arch, opt.heads, opt.head_conv) self.model = load_model(self.model, opt.load_model) self.model = self.model.to(opt.device) self.model.eval() self.tracked_stracks = [] # type: list[STrack] self.lost_stracks = [] # type: list[STrack] self.removed_stracks = [] # type: list[STrack] self.frame_id = 0 self.det_thresh = opt.conf_thres self.buffer_size = int(frame_rate / 30.0 * opt.track_buffer) self.max_time_lost = self.buffer_size self.max_per_image = 128 self.mean = np.array(opt.mean, dtype=np.float32).reshape(1, 1, 3) self.std = np.array(opt.std, dtype=np.float32).reshape(1, 1, 3) self.kalman_filter = KalmanFilter()
def __init__(self, prediction, id): self.id = id # identification of each track object self.KF = KalmanFilter() # KF instance to track this object self.prediction = np.asarray(prediction) # predicted centroids (x,y) self.lostDetectionTime = 0 # number of frames skipped undetected self.detectionTime = 0 self.trace = [] # trace path
def __init__(self, prediction, trackIdCount): self.track_id = trackIdCount # identification of each track object self.KF = KalmanFilter() # KF instance to track this object self.prediction = np.asarray(prediction) # predicted centroids (x,y) self.skipped_frames = 0 # number of frames skipped undetected self.trace = [] # trace path
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)
P = diag((0.0, 0.0)) # Acceleration model G = matrix([[(dt ** 2) / 2], [dt]]) # State transition model F = matrix([[1, dt], [0, 1]]) # Observation vector Z = matrix([[0.0], [0.0]]) # Observation model H = matrix([[1, 0], [0, 0]]) # Observation covariance R = matrix([[sigma_z ** 2, 0], [0, 1]]) # Process noise covariance matrix Q = G * (G.T) * sigma_a ** 2 # Initialise the filter kf = KalmanFilter(X, P, F, Q, Z, H, R) # Set the actual position equal to the starting position A = X # Create log for generating plots log = Logger() log.new_log('measurement') log.new_log('estimate') 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)
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()
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)