Exemplo n.º 1
0
 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]]))
Exemplo n.º 2
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 ) )
Exemplo n.º 5
0
    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
Exemplo n.º 6
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)
Exemplo n.º 7
0
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]]
Exemplo n.º 8
0
    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
Exemplo n.º 9
0
    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)
Exemplo n.º 11
0
    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
Exemplo n.º 12
0
 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
Exemplo n.º 14
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()
Exemplo n.º 15
0
    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)
Exemplo n.º 16
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()
Exemplo n.º 17
0
 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)
Exemplo n.º 19
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
Exemplo n.º 20
0
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
Exemplo n.º 21
0
    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
Exemplo n.º 22
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)
Exemplo n.º 23
0
 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))
Exemplo n.º 24
0
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)
Exemplo n.º 25
0
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
Exemplo n.º 26
0
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()
Exemplo n.º 27
0
	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
Exemplo n.º 28
0
    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()
Exemplo n.º 29
0
 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
Exemplo n.º 30
0
    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
Exemplo n.º 31
0
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)
Exemplo n.º 32
0
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)
Exemplo n.º 33
0
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()
Exemplo n.º 34
0
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)