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
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