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 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 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 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
def test_UKF_converge():
dt = 0.1
F = np.identity(2)
F[0, 1] = dt
def f(x):
return np.dot(F, x)
R = np.identity(2)
def h(x):
return x
def subtraction_supported_residual(a, b):
return a - b
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,
f=f,
h=h,
residual_x=subtraction_supported_residual,
residual_z=subtraction_supported_residual)
for i in range(100):
x = np.dot(F, x)
kf.unscented_predict()
# we are testing filter covergence, not performance, so give it
# perfect measurements
kf.unscented_update(z=x, R=R)
x_confidence = np.sqrt(kf.P[0, 0]) * 5
x_dot_confidence = np.sqrt(kf.P[1, 1]) * 5
assert kf.x_hat[0, 0] - x_confidence <= kf.x_hat[
0, 0] < kf.x_hat[0, 0] + x_confidence
assert kf.x_hat[1, 0] - x_dot_confidence <= kf.x_hat[
1, 0] < kf.x_hat[1, 0] + x_dot_confidence
# test to see that UKF reduces to linear filter when state-transition and
# observation functions are linear
linear_kf = run_filter_convergence()
assert np.allclose(kf.x_hat, linear_kf.x_hat)
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)