def __init__(self, listen_to_vicon=False, vicon_channel=None, publish_to_lcm=False, use_rpydot=False, use_ekf=False, use_ukf=False, delay_comp=False): self._tvlqr_counting = False self._last_time_update = time.time() self._current_dt = 0.0 Thread(target=self._estimator_watchdog).start() self.q = [1.0, 0.0, 0.0, 0.0] # quaternion of sensor frame relative to auxiliary frame self.integralFB = [0.0, 0.0, 0.0] # integral error terms scaled by Ki self._last_rpy = [0.0, 0.0, 0.0] self._last_gyro = [0.0, 0.0, 0.0] self._last_acc = [0.0, 0.0, 0.0] self._last_imu_update = time.time() self._last_xyz = [0.0, 0.0, 0.0] self._last_xyz_raw = [0.0, 0.0, 0.0] self._last_dxyz = [0.0, 0.0, 0.0] self._last_vicon_rpy = [0.0, 0.0, 0.0] self._last_vicon_update = time.time() self._valid_vicon = False self._vicon_alpha_pos = .8 self._vicon_alpha_vel = .7 self._vicon_yaw = 0.0 self._use_rpydot = use_rpydot self._publish_to_lcm = publish_to_lcm if publish_to_lcm: self._xhat_lc = lcm.LCM() self._listen_to_vicon = listen_to_vicon self._vicon_channel = vicon_channel if listen_to_vicon: Thread(target=self._vicon_listener).start() self._input_log = list() self._last_input = [0.0, 0.0, 0.0, 0.0] self._last_input_time = time.time() self._delay_comp = delay_comp if delay_comp: self._cf_model = Crazyflie2() self._delay = 0.028 # delay in the control loop in seconds self._use_ekf = use_ekf self._use_ukf = use_ukf self._use_kalman = use_ekf or use_ukf if self._use_kalman: # states: x y z dx dy dz accxbias accybias acczbias # inputs: roll pitch yaw accx accy accz # measurements: x y z self._plant = DoubleIntegrator() self._last_kalman_update = time.time() if use_ekf: self._kalman = ExtendedKalmanFilter(dim_x=9, dim_z=3, plant=self._plant) elif use_ukf: self._kalman = UnscentedKalmanFilter(dim_x=9, dim_z=3, plant=self._plant)
def read_traceXY(self, file, type, verb, plot, psum, init_x, init_y):
with open(file) as f:
op = None
s = f.readlines()
i = 0
read_num = False
read_locs = False
read_uncertainties = 0
num = 0
init_xy = [[0, 0], [0, 0]]
static_xy = [[0, 0], [0, 0]]
m_var = [0, 0]
a_var = [0, 0]
d_var = [0, 0]
h_var = [0, 0]
self.r0_est_pts_x = []
self.r0_est_pts_y = []
self.r0_gt_pts_x = []
self.r0_gt_pts_y = []
self.r1_est_pts_x = []
self.r1_est_pts_y = []
self.r1_gt_pts_x = []
self.r1_gt_pts_y = []
self.r0_sim_x = []
self.r0_sim_y = []
self.r1_sim_x = []
self.r1_sim_y = []
self.err0 = 0
self.err0_count = 0
self.err0_final = 0
self.err1 = 0
self.err1_count = 0
self.err1_final = 0
while (i < len(s)):
if (len(s[i]) <= 1 or s[i][0] == '#'):
i = i + 1
continue
if (not (read_num)):
num = int(s[i].split()[0])
read_num = True
i = i + 1
continue
if (not (read_locs)):
x = s[i].split()
x0 = float(x[0])
y0 = float(x[1])
x = s[i + 1].split()
x1 = float(x[0])
y1 = float(x[1])
init_xy = [[x0, y0], [init_x, init_y]]
static_xy = [[x0, y0], [init_x, init_y]]
self.r0_gt_pts_x.extend([x0])
self.r0_gt_pts_y.extend([y0])
self.r0_est_pts_x.extend([x0])
self.r0_est_pts_y.extend([y0])
self.r1_gt_pts_x.extend([x1])
self.r1_gt_pts_y.extend([y1])
self.r1_est_pts_x.extend([init_x])
self.r1_est_pts_y.extend([init_y])
self.r0_sim_x.extend([[], x0])
self.r0_sim_y.extend([[], y0])
self.r1_sim_x.extend([[], x1])
self.r1_sim_y.extend([[], y1])
read_locs = True
i = i + 2
continue
if (read_uncertainties < self.NUM_UNCERTAINTIES):
x = s[i].split()
if (x[0] == 'O'):
m_var = [float(x[1]), float(x[2])]
elif (x[0] == 'A'):
a_var = [
math.radians(math.sqrt(float(x[1])))**2,
math.radians(math.sqrt(float(x[2])))**2
]
elif (x[0] == 'D'):
d_var = [float(x[1]), float(x[2])]
elif (x[0] == 'H'):
h_var = [
math.radians(math.sqrt(float(x[1])))**2,
math.radians(math.sqrt(float(x[2])))**2
]
read_uncertainties = read_uncertainties + 1
i = i + 1
continue
if (read_num and read_uncertainties == self.NUM_UNCERTAINTIES
and read_locs):
break
if (type == 0):
op = OdometryLocalize(init_xy, m_var, a_var, d_var, h_var)
elif (type == 1):
op = ExtendedKalmanFilter(init_xy, m_var, a_var, d_var, h_var)
#elif(type == 2):
# op = GridLocalize(init_xy, m_var, a_var, d_var, h_var)
elif (type == 3):
op = UnscentedKalmanFilter(init_xy, m_var, a_var, d_var, h_var)
elif (type == 4):
op = ParticleLocalize(init_xy, m_var, a_var, d_var, h_var)
# elif(type == 5):
# op = ExtendedKalmanFilterWPH(init_xy, m_var, a_var, d_var, h_var)
else:
print "Unknown filter type"
sys.exit()
counter = 0
while (i < len(s)):
line = s[i]
if (len(line) <= 1 or line[0] == '#'):
i = i + 1
continue
x = line.split()
if x[0] == 'M':
d0 = float(x[1])
h0 = math.radians(float(x[2]))
d1 = float(x[3])
h1 = math.radians(float(x[4]))
op.measure_movement([d0, d1], [h0, h1])
(x0, y0) = op.get_possible_pose_estimates(0)
(x1, y1) = op.get_possible_pose_estimates(1)
self.r0_sim_x.extend([x0])
self.r0_sim_y.extend([y0])
self.r1_sim_x.extend([x1])
self.r1_sim_y.extend([y1])
elif x[0] == 'D':
d0 = float(x[1])
d1 = float(x[2])
ph0 = math.radians(float(x[3]))
ph1 = math.radians(float(x[4]))
op.measure_distance([d0, d1], [ph0, ph1])
(x0, y0) = op.get_pose_estimate(0)
(x1, y1) = op.get_pose_estimate(1)
self.r0_sim_x.extend([x0])
self.r0_sim_y.extend([y0])
self.r1_sim_x.extend([x1])
self.r1_sim_y.extend([y1])
elif x[0] == 'C':
(x0, y0) = (float(x[1]), float(x[2]))
(x1, y1) = (float(x[3]), float(x[4]))
(pred_x0, pred_y0) = op.get_pose_estimate(0)
(pred_x1, pred_y1) = op.get_pose_estimate(1)
if verb or (i >= len(s) - 7 and psum):
print "Rover 0 abs. pose %d: (%f, %f)" % (counter, x0,
y0)
print "Rover 0 est. pose %d: (%f, %f)" % (
counter, pred_x0, pred_y0)
pd0 = math.sqrt((pred_x0 - x0)**2 + (pred_y0 - y0)**2)
d0 = math.sqrt((x0 - static_xy[0][0])**2 +
(y0 - static_xy[0][1])**2)
self.err0_count += 1
self.err0_final = (100.0 * (abs(pd0) / d0))
self.err0 += self.err0_final
if verb or (i >= len(s) - 7 and psum):
print "Rover 0 error: %f%%" % (100.0 * (abs(pd0) / d0))
self.r0_gt_pts_x.extend([x0])
self.r0_gt_pts_y.extend([y0])
self.r0_est_pts_x.extend([pred_x0])
self.r0_est_pts_y.extend([pred_y0])
if verb or (i >= len(s) - 7 and psum):
print "Rover 1 abs. pose %d: (%f, %f)" % (counter, x1,
y1)
print "Rover 1 est. pose %d: (%f, %f)" % (
counter, pred_x1, pred_y1)
pd1 = math.sqrt((pred_x1 - x1)**2 + (pred_y1 - y1)**2)
d1 = math.sqrt((x1 - static_xy[1][0])**2 +
(y1 - static_xy[1][1])**2)
self.err1_count += 1
self.err1_final = (100.0 * (abs(pd1) / d1))
self.err1 += self.err1_final
if verb or (i >= len(s) - 7 and psum):
print "Rover 1 error: %f%%" % (100.0 * (abs(pd1) / d1))
print "\n"
self.r1_gt_pts_x.extend([x1])
self.r1_gt_pts_y.extend([y1])
self.r1_est_pts_x.extend([pred_x1])
self.r1_est_pts_y.extend([pred_y1])
counter = counter + 1
else:
print "Invalid trace file"
i = i + 1
if verb:
print self.r0_gt_pts_x
print self.r0_gt_pts_y
if plot:
# self.plot_cont(len(self.r0_sim_x) / 2, 500.0, self.r0_sim_x, self.r0_sim_y, self.r0_gt_pts_x, self.r0_gt_pts_y, self.r1_sim_x, self.r1_sim_y, self.r1_gt_pts_x, self.r1_gt_pts_y)
self.plot_points(self.r0_gt_pts_x, self.r0_gt_pts_y,
self.r0_est_pts_x, self.r0_est_pts_y,
self.r1_gt_pts_x, self.r1_gt_pts_y,
self.r1_est_pts_x, self.r1_est_pts_y)
