def __init__(self, name, X0, Dt, proc_cov, meas_cov, Hk=None, num=1000, L=100): """ INPUTS: name ~ string identifying filter X0 ~ initial state Dt ~ timestep proc_cov ~ process covariance meas_cov ~ measurement covariance Hk ~ matrix for measurement model num ~ number of particles L ~ number of timesteps """ self.name = name self.X0 = X0 self.num = num self.dim = len(X0) self.meas_dim = len(meas_cov) self.Dt = Dt self.proc_cov = proc_cov self.meas_cov = meas_cov self.est_cov = copy.deepcopy(proc_cov) if Hk != None: self.Hk = Hk self.L = L self.index = 0 self.t = 0 self.tvec = zeros((self.L+1, 1)) self.xfilt = zeros((self.L+1, self.dim)) self.xpred = zeros((self.L+1, self.dim)) self.covs = zeros((self.L+1, self.dim, self.dim)) self.xfilt[0] = X0 self.xpred[0] = X0 self.covs[0] = self.est_cov self.measurements = zeros((self.L+1, self.meas_dim)) self.measurements[0] = tools.matmult(self.Hk, self.X0) self.xkk = X0 self.Qk = self.proc_cov self.Rk = self.meas_cov self.Pkk = copy.deepcopy(self.est_cov)
def step_filter(self, zt, Winc=None, *args, **kwargs): if Winc == None: Winc = random.multivariate_normal(zeros(self.dim), self.proc_cov) self.index += 1 self.t += self.Dt # prediction xkkm1 = self.state_trans(Winc, *args, **kwargs) Fkm1 = self.proc_linearize(xkkm1) Pkkm1 = tools.matmult(Fkm1,self.Pkk,Fkm1.T) + self.Qk # update yk = zt - tools.matmult(self.Hk, xkkm1) Sk = tools.matmult(self.Hk, Pkkm1, self.Hk.T) + self.Rk Kk = tools.matmult(Pkkm1, self.Hk.T, linalg.inv(Sk)) self.xkk = xkkm1 + tools.matmult(Kk, yk) self.Pkk = tools.matmult((eye(self.dim) - tools.matmult(Kk,self.Hk)), Pkkm1) # store data self.xfilt[self.index] = self.xkk self.xpred[self.index] = xkkm1 self.covs[self.index] = self.Pkk self.tvec[self.index] = self.t self.measurements[self.index] = zt return
def fullcb(self, data): rospy.logdebug("Full controller callback triggered") op_cond = self.operating_condition if op_cond is OperatingCondition.CALIBRATE: self.send_initial_config() # self.first_flag = False return elif op_cond is OperatingCondition.EMERGENCY: # we need to stop robots!: self.stop_robots() return elif op_cond is not OperatingCondition.RUN: # we are not running, keep setting initializations self.first_flag = True self.callback_count = 0 self.ekf.index = 0 # clear trajectories: self.mass_ref_vec.clear() self.mass_filt_vec.clear() return if self.first_flag: rospy.loginfo("Beginning Trajectory") # publish start time: self.time_pub.publish(data.header.stamp) self.send_initial_config() self.tbase = data.header.stamp self.first_flag = False self.callback_count = 0 # reset filter: self.ekf.index = 0 self.ekf.xkk = self.ekf.X0 self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov) # send reference traj U and store: self.Uprev = self.X0[2:4] self.Ukey = self.Uopt[0] # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.X0) else: self.callback_count += 1 zk = tools.config_to_array(self.system, data) # get updated estimates self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev) # set index value: if self.callback_count > len(self.Uopt)-1: index = len(self.Uopt)-1 else: index = self.callback_count # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.Xref[index]) # calculate controls: self.Ukey = self.Uopt[index] + \ tools.matmult(self.Kstab[index], self.Xopt[index] - self.ekf.xkk) # send controls to robot: self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey) # add path information: self.add_to_path_vectors(data, self.Xref[index], self.ekf.xkk) # store data: self.Uprev = self.Ukey # check for the end of the trajectory: if self.callback_count > len(self.tvec)-1: rospy.loginfo("Trajectory complete!") try: self.op_change_client(OperatingCondition(OperatingCondition.STOP)) except rospy.ServiceException, e: rospy.loginfo("Service did not process request: %s"%str(e)) # now we can stop the robots self.stop_robots()
def lqrcb(self, data): rospy.logdebug("LQR control callback triggered") op_cond = self.operating_condition if op_cond is OperatingCondition.CALIBRATE: self.send_initial_config() return elif op_cond is OperatingCondition.EMERGENCY: # we need to stop robots!: self.stop_robots() return elif op_cond is not OperatingCondition.RUN: # we are not running, keep setting initializations self.first_flag = True self.wait_flag = True self.callback_count = 0 self.ekf.index = 0 # clear trajectories: self.mass_ref_vec.clear() self.mass_filt_vec.clear() # reset if self.interactive_bool: self.RM.reset_interps() return if self.first_flag: # let's enforce a few delays here to ensure that we build up enough # reference traj: if self.interactive_bool and self.wait_flag: if self.callback_count == 0: # publish start time: self.time_pub.publish(data.header.stamp) self.send_initial_config() self.tbase = data.header.stamp elif self.callback_count > self.n_win: self.wait_flag = False self.callback_count += 1 return rospy.loginfo("Beginning Trajectory") self.first_flag = False self.callback_count = 0 # reset filter: self.ekf.index = 0 self.ekf.xkk = self.ekf.X0 self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov) # get reference traj after initial dt: r = self.RM.calc_reference_traj(self.dsys, [0, self.dt, 2*self.dt]) if r: Xtmp,Utmp = r else: rospy.logwarn("waiting for more reference in first_flag of lqrcb") self.first_flag = True return # send reference traj U and store: self.convert_and_send_input(Xtmp[0][2:4], Xtmp[1][2:4])#self.Uprev, self.Ukey) self.Uprev = Utmp[0] self.Ukey = Utmp[1] # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.X0) else: self.callback_count += 1 zk = tools.config_to_array(self.system, data) # get updated estimate self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev) # get reference r = self.RM.calc_reference_traj(self.dsys, [self.callback_count*self.dt]) if r: xref,uref = r else: rospy.logwarn("reference trajectory returned None for lqr current state!") return # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, xref[0]) # calculate controls: self.Ukey = xref[0][2:4] + tools.matmult(self.Kstab, xref[0] - self.ekf.xkk) # send controls to robot: self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey) # add path information: self.add_to_path_vectors(data, xref[0], self.ekf.xkk) # store data: self.Uprev = self.Ukey # check for the end of the trajectory: if self.callback_count >= len(self.tvec): rospy.loginfo("Trajectory complete!") try: self.op_change_client(OperatingCondition(OperatingCondition.STOP)) except rospy.ServiceException, e: rospy.loginfo("Service did not process request: %s"%str(e)) # now we can stop the robots self.stop_robots()
def proc_linearize(self, xlin): return (eye(self.dim) + tools.matmult( self.Dt*self.A(xlin + 0.5*self.Dt*self.f(xlin)), eye(self.dim) + 0.5*self.Dt*self.A(xlin)))
def state_trans(self, Winc, *args, **kwargs): return self.xkk + self.Dt*tools.matmult(self.A, self.xkk)
def fullcb(self, data): rospy.logdebug("Full controller callback triggered") op_cond = self.operating_condition if op_cond is OperatingCondition.CALIBRATE: self.send_initial_config() # self.first_flag = False return elif op_cond is OperatingCondition.EMERGENCY: # we need to stop robots!: self.stop_robots() return elif op_cond is not OperatingCondition.RUN: # we are not running, keep setting initializations self.first_flag = True self.callback_count = 0 self.ekf.index = 0 # clear trajectories: self.mass_ref_vec.clear() self.mass_filt_vec.clear() return if self.first_flag: rospy.loginfo("Beginning Trajectory") # publish start time: self.time_pub.publish(data.header.stamp) self.send_initial_config() self.tbase = data.header.stamp self.first_flag = False self.callback_count = 0 # reset filter: self.ekf.index = 0 self.ekf.xkk = self.ekf.X0 self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov) # send reference traj U and store: self.Uprev = self.X0[2:4] self.Ukey = self.Uopt[0] # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.X0) else: self.callback_count += 1 zk = tools.config_to_array(self.system, data) # get updated estimates self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev) # set index value: if self.callback_count > len(self.Uopt) - 1: index = len(self.Uopt) - 1 else: index = self.callback_count # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.Xref[index]) # calculate controls: self.Ukey = self.Uopt[index] + \ tools.matmult(self.Kstab[index], self.Xopt[index] - self.ekf.xkk) # send controls to robot: self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey) # add path information: self.add_to_path_vectors(data, self.Xref[index], self.ekf.xkk) # store data: self.Uprev = self.Ukey # check for the end of the trajectory: if self.callback_count > len(self.tvec) - 1: rospy.loginfo("Trajectory complete!") try: self.op_change_client( OperatingCondition(OperatingCondition.STOP)) except rospy.ServiceException, e: rospy.loginfo("Service did not process request: %s" % str(e)) # now we can stop the robots self.stop_robots()
def lqrcb(self, data): rospy.logdebug("LQR control callback triggered") op_cond = self.operating_condition if op_cond is OperatingCondition.CALIBRATE: self.send_initial_config() return elif op_cond is OperatingCondition.EMERGENCY: # we need to stop robots!: self.stop_robots() return elif op_cond is not OperatingCondition.RUN: # we are not running, keep setting initializations self.first_flag = True self.wait_flag = True self.callback_count = 0 self.ekf.index = 0 # clear trajectories: self.mass_ref_vec.clear() self.mass_filt_vec.clear() # reset if self.interactive_bool: self.RM.reset_interps() return if self.first_flag: # let's enforce a few delays here to ensure that we build up enough # reference traj: if self.interactive_bool and self.wait_flag: if self.callback_count == 0: # publish start time: self.time_pub.publish(data.header.stamp) self.send_initial_config() self.tbase = data.header.stamp elif self.callback_count > self.n_win: self.wait_flag = False self.callback_count += 1 return rospy.loginfo("Beginning Trajectory") self.first_flag = False self.callback_count = 0 # reset filter: self.ekf.index = 0 self.ekf.xkk = self.ekf.X0 self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov) # get reference traj after initial dt: r = self.RM.calc_reference_traj(self.dsys, [0, self.dt, 2 * self.dt]) if r: Xtmp, Utmp = r else: rospy.logwarn( "waiting for more reference in first_flag of lqrcb") self.first_flag = True return # send reference traj U and store: self.convert_and_send_input(Xtmp[0][2:4], Xtmp[1][2:4]) #self.Uprev, self.Ukey) self.Uprev = Utmp[0] self.Ukey = Utmp[1] # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, self.X0) else: self.callback_count += 1 zk = tools.config_to_array(self.system, data) # get updated estimate self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev) # get reference r = self.RM.calc_reference_traj(self.dsys, [self.callback_count * self.dt]) if r: xref, uref = r else: rospy.logwarn( "reference trajectory returned None for lqr current state!" ) return # publish filtered and reference: self.publish_state_and_config(data, self.ekf.xkk, xref[0]) # calculate controls: self.Ukey = xref[0][2:4] + tools.matmult(self.Kstab, xref[0] - self.ekf.xkk) # send controls to robot: self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey) # add path information: self.add_to_path_vectors(data, xref[0], self.ekf.xkk) # store data: self.Uprev = self.Ukey # check for the end of the trajectory: if self.callback_count >= len(self.tvec): rospy.loginfo("Trajectory complete!") try: self.op_change_client( OperatingCondition(OperatingCondition.STOP)) except rospy.ServiceException, e: rospy.loginfo("Service did not process request: %s" % str(e)) # now we can stop the robots self.stop_robots()