def run_filter_convergence(): dt = 0.1 F = np.identity(2) F[0, 1] = dt R = np.identity(2) H = np.identity(2) x = np.array([[1, 1]]).T x_hat_init = np.array([[0, 0]]).T # 1 on the diagonals is reasonable starting covariance P_init = np.identity(2) G = np.array([[dt**2 / 2, dt]]).T Q = np.dot(G, G.T) kf = Kalman(x_hat_init, P_init, Q) for i in range(100): x = np.dot(F, x) B = np.zeros(shape=(2, 1)) u = 0 kf.predict(F, u, B) # we are testing filter covergence, not performance, so give it # perfect measurements kf.update(x, H, R) return kf
class RangeFilter: chassis = Chassis range_finder = RangeFinder bno055 = BNO055 vision = Vision state_vector_size = 1 # the range sensor noise range_variance = 0.0005 # the encoder noise odometry_variance = 1e-3 loop_dt = 1 / 50 reset_thresh = 0.5 vision_based_range_variance = 0.1 def __init__(self): pass def setup(self): self.reset() self.last_vision_mode = self.vision.enabled def reset(self): r = self.range_finder.getDistance() if math.isinf(r): r = 0 self.is_reset = True r = self.range_finder.getDistance() if math.isinf(r): r = 40 # starting state x_hat = np.array([r]).reshape(-1, 1) P = np.zeros(shape=(self.state_vector_size, self.state_vector_size)) P[0][0] = self.range_variance Q = np.array([self.odometry_variance]).reshape(self.state_vector_size, self.state_vector_size) # error vision and error rate of change of vision are correlated R = np.array([self.range_variance]).reshape(self.state_vector_size, self.state_vector_size) self.filter = Kalman(x_hat, P, Q, R) self.range_deque = deque(maxlen=50) self.odometry_deque = deque(maxlen=50) self.update_deques() self.last_vision_time = self.vision.time def update_deques(self): self.odometry_deque.append( np.array(self.chassis.get_raw_wheel_distances())) r = self.range_finder.getDistance() self.range_deque.append(40 if math.isinf(r) else r) def predict(self, timesteps=1): F = np.identity(self.state_vector_size) B = np.identity(self.state_vector_size) robot_center_movement = -np.mean(self.odometry_deque[-timesteps] - self.odometry_deque[-timesteps - 1]) u = np.array([robot_center_movement]).reshape(-1, 1) self.filter.predict(F, u, B) self.last_odometry = np.array(self.chassis.get_raw_wheel_distances()) def range_update(self, timesteps=1): r = self.range_deque[-timesteps] if not abs(r - self.range) < math.sqrt(self.filter.P[0][0]) * 5: return elif abs(r - self.range) > self.reset_thresh: self.reset() return z = np.array([r]).reshape(-1, 1) H = np.identity(self.state_vector_size) self.filter.update(z, H) def vision_update(self): z = np.array([self.vision_predicted_range()]).reshape(-1, 1) H = np.identity(self.state_vector_size) R = np.array([self.vision_based_range_variance]).reshape(-1, 1) self.filter.update(z, H, R) def execute(self): if not self.vision.enabled: if self.last_vision_mode: self.reset() self.last_vision_mode = self.vision.enabled return self.last_vision_mode = self.vision.enabled self.update_deques() if self.range < 0.1 or self.range >= 5: r = self.range_deque[-1] if math.isinf(r): print("reset") self.reset() timesteps_since_vision = int((time.time() - self.vision.time) / 50) self.predict() self.range_update() if self.vision.time != self.last_vision_time and self.vision_predicted_range( ) != 0: if timesteps_since_vision > 25: self.reset() else: to_roll_back = min(timesteps_since_vision, len(self.filter.history), len(self.odometry_deque) - 1) self.filter.roll_back(to_roll_back) self.vision_update() for i in range(to_roll_back): self.predict(timesteps=to_roll_back - i) self.range_update(timesteps=to_roll_back - i) self.last_vision_time = self.vision.time @property def range(self): return self.filter.x_hat[0][0] def vision_predicted_range(self): """ Predict what our range finder reading should be based off of the distance between the vision targets. Used in order to gate the ranges that are used to update our kalman filter. """ # if self.vision.target_sep == 0: # return 0 # target_angle = 0 # if self.bno055.getHeading() > math.pi/6: # # we think that we are looking at the right hand target # target_angle = math.pi/3 # elif self.bno055.getHeading() < -math.pi/6: # # we think that we are looking at the left hand target # target_angle = -math.pi/3 # perpendicular_to_heading = target_angle - self.bno055.getHeading() # theta_alpha = math.pi/2 - perpendicular_to_heading - self.vision.derive_vision_angle() # predicted_range = self.vision.derive_target_range() * math.sin(perpendicular_to_heading * math.pi/2) / math.sin(theta_alpha) # return predicted_range return self.vision.derive_target_range()
class PositionFilter: imu: NavX chassis: SwerveChassis vision: Vision # initial covariance of the system P_init = np.identity(2, dtype=float) * 1e-4 # initial covariance per timestep (process noise) Q = np.identity(2, dtype=float) * 1e-4 / 50 # vision measurement covariance R = np.identity(2, dtype=float) * 2e-6 CUBE_HEIGHT = 0.3 def on_enable(self): self.reset() def reset(self): self.kalman = Kalman(x_hat=self.chassis.position, P=self.P_init, residual_z=angle_residual) self.use_vision = False self.cube = np.zeros((2, 1)) self.last_position = self.chassis.position self.odometry_deque = deque([], maxlen=50) self.imu_deque = deque([], maxlen=50) self.last_vision_tm = self.vision.time def predict(self, timesteps_ago=None): if timesteps_ago is None: self.odometry_deque.append(self.chassis.position) # theta_imu = (self.imu.getAngle() + self.chassis.odometry_z_vel+(1/50)/2) theta_imu = self.imu.getAngle() self.imu_deque.append(theta_imu) timesteps_ago = 0 pos = self.odometry_deque[-timesteps_ago - 1] last_pos = pos if len(self.odometry_deque) >= 2: last_pos = self.odometry_deque[-timesteps_ago - 2] position_delta = pos - last_pos self.kalman.predict(np.identity(2), position_delta, np.identity(2), Q=self.Q) def update(self, steps_since_vision, vision_data): # print("update") if not self.imu_deque: return imu_rollback = min(steps_since_vision, len(self.imu_deque) - 1) theta_imu = self.imu_deque[-imu_rollback - 1] def observation(state): cube_from_robot = self.cube - state field_azimuth = math.atan2(cube_from_robot[1, 0], cube_from_robot[0, 0]) azimuth = constrain_angle(field_azimuth - theta_imu) zenith = math.atan2( np.linalg.norm(cube_from_robot), -self.vision.CAMERA_HEIGHT + self.CUBE_HEIGHT / 2) # print("state obs az %s zen %s" % (azimuth, zenith)) return np.array([[azimuth], [zenith]], dtype=float) measurement = np.array([[vision_data[0]], [vision_data[1]]], dtype=float) self.kalman.unscented_update(measurement, z_dim=2, R=self.R, h=observation) def execute(self): self.chassis.update_odometry() self.predict() vision_tm = self.vision.time vision_data = self.vision.data if (not self.last_vision_tm == vision_tm and len(vision_data) > 1): since_vision_recieved = time.monotonic() - vision_tm since_vision = since_vision_recieved + vision_data[-1] steps_since_vision = int(since_vision * 50) if steps_since_vision > len(self.odometry_deque) - 1: return # print("steps since %s" % since_vision) self.kalman.roll_back(steps_since_vision) self.update(steps_since_vision, vision_data) for i in range(steps_since_vision, 0, -1): self.predict(timesteps_ago=steps_since_vision - 1) self.last_vision_tm = vision_tm # print("Chassis odom %s" % self.chassis.position) # print("Filter %s" % self.position) @property def position(self): return np.reshape(self.kalman.x_hat, 2)
class VisionFilter: vision = Vision bno055 = BNO055 state_vector_size = 2 # the initial uncertainty in the vision rate init_dx_variance = 0.01 # the vision sensor noise vision_x_variance = 0.0005 init_x_variance = 0.01 # the variance in the unknown acceleration impulse acceleration_variance = 0.4 loop_dt = 1 / 50 reset_thresh = 0.2 def __init__(self): pass def setup(self): self.reset() def reset(self): timesteps_since_vision = int((time.time() - self.vision.time) / 50) start_x = 0 if timesteps_since_vision < 10: start_x = 0 # starting state x_hat = np.array([start_x, 0]).reshape(-1, 1) P = np.zeros(shape=(self.state_vector_size, self.state_vector_size)) P[0][0] = VisionFilter.init_x_variance P[1][1] = VisionFilter.init_dx_variance Q = (np.array([[self.loop_dt**4 / 4, self.loop_dt**3 / 3], [self.loop_dt**3 / 2, self.loop_dt**2]]).reshape( self.state_vector_size, self.state_vector_size) * self.acceleration_variance) self.last_vision = self.vision.x self.last_vision_time = self.vision.time # error vision and error rate of change of vision are correlated R = np.array( [[VisionFilter.vision_x_variance, VisionFilter.vision_x_variance], [ VisionFilter.vision_x_variance, VisionFilter.vision_x_variance * 6 ]]).reshape(self.state_vector_size, self.state_vector_size) self.filter = Kalman(x_hat, P, Q, R) self.imu_deque = deque(maxlen=50, iterable=[self.get_heading_state()]) def get_heading_state(self): return np.array( [self.bno055.getHeading(), self.bno055.getHeadingRate()]).reshape(-1, 1) def on_enable(self): self.reset() def predict(self, timestep=1): """Predict what the measurement should be in the next timestep. :param timestep: the number of timesteps in the past that we are predicting forward *from*""" F = np.identity(self.state_vector_size) F[0][1] = self.loop_dt B = np.identity(self.state_vector_size) self.imu_deque.append(self.get_heading_state()) u = Vision.rad_to_vision_units(self.imu_deque[-timestep] - self.imu_deque[-timestep - 1]) self.filter.predict(F, u, B) def update(self): x = self.vision.x dx = (self.vision.x - self.last_vision) / (self.vision.time - self.last_vision_time) H = np.identity(self.state_vector_size) z = np.array([x, dx]).reshape(-1, 1) self.filter.update(z, H) self.last_vision = self.vision.x self.last_vision_time = self.vision.time def execute(self): if self.vision.time == 0: return timesteps_since_vision = int((time.time() - self.vision.time) / 50) if timesteps_since_vision > 10: return elif abs(self.vision.x - self.filter.x_hat[0][0]) > self.reset_thresh: self.reset() self.predict() if self.vision.time != self.last_vision_time: to_roll_back = min(timesteps_since_vision, len(self.filter.history)) self.filter.roll_back(to_roll_back) self.update() for i in range(timesteps_since_vision): self.predict(timestep=timesteps_since_vision - i) @property def x(self): return self.filter.x_hat[0][0] @property def dx(self): return self.filter.x_hat[1][0] @property def angle(self): return -(self.x * self.horizontal_fov / 2)