示例#1
0
	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)
示例#2
0
class StateEstimator():

	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 add_imu_reading(self, imu_reading):
		(gx, gy, gz, ax, ay, az, dt_imu) = imu_reading

		dt = time.time() - self._last_imu_update
		new_quat = MahonyAHRS.MahonyAHRSupdateIMU(gx,gy,gz,ax,ay,az,dt,
												  self.q[0],self.q[1],self.q[2],self.q[3],
												  self.integralFB[0],self.integralFB[0],self.integralFB[0])
		self._last_imu_update = time.time()

		self.q = new_quat[0:4]
		self.integralFB = new_quat[4:]
		try:
			self._last_rpy = quat2rpy(self.q)
			if self._listen_to_vicon:
				self._last_rpy[2] = self._vicon_yaw
		except ValueError:
			pass
		self._last_gyro = [gx,gy,gz]
		self._last_acc = [ax,ay,az]

	def add_input(self, input_sent):
		last_dt = time.time()-self._last_input_time
		self._input_log.append([self._last_input,last_dt])		
		if len(self._input_log)>20:
			self._input_log = self._input_log[10:]
		self._last_input = input_sent
		self._last_input_time = time.time()

	def get_last_inputs(self, tspan):
		control_inputs = list()
		log = list(self._input_log)
		dt = 0.0
		for entry in log:
			dt += entry[1]
			if dt>tspan:
				break
			control_inputs.insert(0,entry)
		if len(control_inputs)<1:
			control_inputs = [[[0.0, 0.0, 0.0, 0.0],tspan]]
		return control_inputs

	def _vicon_listener(self):
		_vicon_listener_lc = lcm.LCM()
		_vicon_listener_lc.subscribe(self._vicon_channel,self._add_vicon_reading)
		while True:
			_vicon_listener_lc.handle()

	def _add_vicon_reading(self, channel, data):
		msg = vicon_pos_t.decode(data)
		
		if msg.q[0] < -999:
			self._valid_vicon = False
			#self._last_dxyz = [0.0, 0.0, 0.0]
			return
		
		# put any filtering on yaw here if needed
		self._vicon_yaw = self._vicon_alpha_pos*msg.q[5]+(1-self._vicon_alpha_pos)*self._vicon_yaw

		xyz = list(msg.q)[0:3]
		dxyz = [0.0, 0.0, 0.0]
		if self._valid_vicon:
			dt = 1.0/120.0
			dt_measured = (msg.timestamp-self._last_vicon_update)/1000.0
			if (dt_measured>1.5*dt):
				dt = dt_measured
			dxyz[0] = (1.0/dt)*(xyz[0]-self._last_xyz_raw[0])
			dxyz[1] = (1.0/dt)*(xyz[1]-self._last_xyz_raw[1])
			dxyz[2] = (1.0/dt)*(xyz[2]-self._last_xyz_raw[2])
			self._last_xyz_raw = list(xyz)
		
		self._last_xyz[0] = self._vicon_alpha_pos*xyz[0]+(1-self._vicon_alpha_pos)*self._last_xyz[0]
		self._last_xyz[1] = self._vicon_alpha_pos*xyz[1]+(1-self._vicon_alpha_pos)*self._last_xyz[1]
		self._last_xyz[2] = self._vicon_alpha_pos*xyz[2]+(1-self._vicon_alpha_pos)*self._last_xyz[2]
		self._last_dxyz[0] = self._vicon_alpha_vel*dxyz[0]+(1-self._vicon_alpha_vel)*self._last_dxyz[0]
		self._last_dxyz[1] = self._vicon_alpha_vel*dxyz[1]+(1-self._vicon_alpha_vel)*self._last_dxyz[1]
		self._last_dxyz[2] = self._vicon_alpha_vel*dxyz[2]+(1-self._vicon_alpha_vel)*self._last_dxyz[2]
		self._last_vicon_update = msg.timestamp
		self._valid_vicon = True

	def get_xhat(self):
		if self._use_kalman:
			dt = time.time() - self._last_kalman_update
			self._kalman.predict(np.array(self._last_rpy + self._last_acc), dt)
			if self._valid_vicon:
				self._kalman.update(np.array(self._last_xyz))
			self._last_kalman_update = time.time()
			kalman_xhat = self._kalman.x.reshape(9).tolist()
			xhat = [kalman_xhat[0],kalman_xhat[1],kalman_xhat[2],
					self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
					kalman_xhat[3],kalman_xhat[4],kalman_xhat[5],
					self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]

			# msg = dxyz_compare_t()
			# msg.dxyzraw = self._last_dxyz
			# msg.dxyzfiltered = kalman_xhat[3:6]
			# self._xhat_lc.publish('dxyz_compare', msg.encode())

			msg = kalman_args_t()
			msg.input_rpy = self._last_rpy
			msg.input_acc = self._last_acc
			msg.input_dt = dt
			msg.valid_vicon = self._valid_vicon
			msg.meas_xyz = self._last_xyz
			msg.smooth_xyz = self._last_xyz
			msg.smooth_dxyz =self._last_dxyz
			self._xhat_lc.publish('kalman_args', msg.encode())

		else:
			xhat = [self._last_xyz[0],self._last_xyz[1],self._last_xyz[2],
					self._last_rpy[0],self._last_rpy[1],self._last_rpy[2],
					self._last_dxyz[0],self._last_dxyz[1],self._last_dxyz[2],
					self._last_gyro[0],self._last_gyro[1],self._last_gyro[2]]

		if self._delay_comp:
			control_inputs = self.get_last_inputs(self._delay)
			for ci in control_inputs:
				xhat = self._cf_model.simulate(xhat,ci[0],ci[1])

		if self._use_rpydot:
			try:
				xhat[9:12] = angularvel2rpydot(self._last_rpy, body2world(self._last_rpy, self._last_gyro))
			except ValueError:
				xhat[9:12] = [0.0, 0.0, 0.0]

		if self._publish_to_lcm:
			msg = crazyflie_state_estimate_t()
			msg.xhat = xhat
			msg.t = self.get_time()
			self._xhat_lc.publish("crazyflie_state_estimate", msg.encode())

		# uncomment the following if you want to trigger hover at a predefined state
	#	if xhat[2] <= 0.15:
	#		print('This is how low')
	#		msg = crazyflie_controller_commands_t()
	#		msg.is_running = False
	#		self._xhat_lc.publish('crazyflie_controller_commands', msg.encode())
		# 	msg = crazyflie_hover_commands_t()
		# 	msg.hover = True
		# 	self._xhat_lc.publish('crazyflie_hover_commands',msg.encode())

		return xhat

	def get_time(self):
		if self._tvlqr_counting:
			self._current_dt += (time.time()-self._last_time_update)
		self._last_time_update = time.time()

		# uncomment the following if you want to trigger hover at a predefined time
		# if 1.1-xhat[0] <= 0.01:
		# 	msg = crazyflie_hover_commands_t()
		# 	msg.hover = True
		# 	self._xhat_lc.publish('crazyflie_hover_commands',msg.encode())

		return self._current_dt

	def _estimator_watchdog(self):
		_watchdog_lc = lcm.LCM()
		_watchdog_lc.subscribe('crazyflie_state_estimator_commands',self._estimator_watchdog_update)
		while True:
			_watchdog_lc.handle()

	def _estimator_watchdog_update(self, channel, data):
		msg = crazyflie_state_estimator_commands_t.decode(data)
		self._tvlqr_counting = msg.tvlqr_counting
示例#3
0
    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)