Esempio n. 1
0
  def update_OptFlowRad(self, optflow):
    ts = optflow.header.stamp
    r = optflow.distance

    print 'opt flow distance is ', r
    # small measurements may be invalid
    if r < 0.32:
      return

    # sonar may give wonky measurements when uav isn't level
    if not model.vehicle_is_level(self.x):
      return

    # handle timing issues
    if self.prop_time is not None:
      dt = (ts - self.prop_time).to_sec()
      if dt < -0.1:
        rospy.logwarn('lidarlite message %f seconds old, skipping', -dt)
        return
      elif dt > 0.1:
        rospy.logwarn('lidarlite message %f seconds in future, skipping', dt)
        return
      elif dt > 0:
        (x, Sigma) = self.process(self.x, self.Sigma, self.u, dt, self.disturb_mode)
      else:
        (x, Sigma) = (self.x, self.Sigma)

    # Now do correction
    z = np.array([r])

    (h,Hx,Q) = model.observation_alt_px4flow(x, self.disturb_mode, z)
    (x_c, Sigma_c) = self.update(x, Sigma, z, h, Hx, Q)
    (self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)

    return
Esempio n. 2
0
  def update_IMU(self, imu):

    ts = imu.header.stamp
    # use UNCORRECTED rates as control input
    gyro_u = np.array([imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z])
    acc = np.array([imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z])

    if self.flight_state == model.FLIGHT_STATE_FLIGHT:
      # check for bump/disturbance
      if model.accel_detect_bump(self.x, acc, 15.0):
        self.disturb_mode = model.DISTURB_ACTIVE
        self.bump_time = ts
        rospy.loginfo('Bump detected')
      else:
        # if we're not nominal, but it's been a while since last bump, return to nominal
        if self.disturb_mode != model.DISTURB_NOMINAL and (ts - self.bump_time).to_sec() > 0.25:
          self.disturb_mode = model.DISTURB_NOMINAL
          rospy.loginfo('Return to nominal')

      # propogate if neccessary
      self.propogate_from_imu(ts, gyro_u) 

      # and do correction based on accelerometer
      (h, Hx, Q) = model.observation_acc_flight(self.x, self.disturb_mode)
      (x_c, Sigma_c) = self.update(self.x, self.Sigma, acc, h, Hx, Q)
      (self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)

      # detect landing
      if self.disturb_mode == model.DISTURB_NOMINAL and model.vehicle_is_level(self.x) and model.vehicle_is_stationary(self.x, gyro_u, acc):
        if self.potential_landing_ts is None:
          self.potential_landing_ts == ts
          self.potential_landing_nonstationary_count = 0
        
        elif (ts - self.potential_landing_ts) > 0.25:
          rospy.loginfo('Landing detected. Entering ground mode.')
          self.potential_landing_ts = None
          self.flight_state = model.FLIGHT_STATE_GROUNDED
          
      else:
        # we aren't stationary, can't be landed
        self.potential_landing_ts = None


    elif self.flight_state == model.FLIGHT_STATE_TAKEOFF:
      # just propogate imu
      self.propogate_from_imu(ts, gyro_u)

      # transition to flight mode after short period
      if (ts - self.takeoff_ts).to_sec() > 1.0:
        self.flight_state = model.FLIGHT_STATE_FLIGHT
        rospy.loginfo('Entering flight mode.')

    else:   # we're grounded
      # check for takeoff
      if model.accel_detect_takeoff(self.x, acc):
        rospy.loginfo('Takeoff detected.')
        self.flight_state = model.FLIGHT_STATE_TAKEOFF
        self.takeoff_ts = ts
        # assume we start in disturbed state
        self.disturb_mode = model.DISTURB_ACTIVE
        self.bump_time = ts
        print self.Sigma

      else: # use grounded data
        # if there is very little motion, we can zupt
        if not model.accel_detect_bump(self.x, acc, 0.10):
          rospy.loginfo('zupting')
          (h, Hx, Q) = model.observation_zupt(self.x)
          (x_c, Sigma_c) = self.update(self.x, self.Sigma, gyro_u, h, Hx, Q)
          (self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
          self.prop_time = ts

        # otherwise propogate grounded model
        # and use grounded accelerometer model
        else:
          self.propogate_from_imu(ts, gyro_u)
          
          (h, Hx, Q) = model.observation_acc_ground(self.x, self.disturb_mode)
          (x_c, Sigma_c) = self.update(self.x, self.Sigma, acc, h, Hx, Q)
          (self.x, self.Sigma) = model.enforce_bounds(x_c, Sigma_c)
          self.prop_time = ts
    return