def __init__(self): self.volts = 0. self.wsize = 61 self.filter_order = 3 self.theta = rospy.get_param("~theta") self.off_y = rospy.get_param("~off_y") self.abcd = rospy.get_param("~abcd") self.maximum_time = rospy.get_param("~maximum_time") self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order) size = 2*self.wsize+1 self.volt_filt = 48000*np.ones(size) rospy.Subscriber("/power_board/voltage", Float64, self.callback) self.pub_power = rospy.Publisher('/power_state', PowerState) self.msg_power = PowerState()
def __init__(self): self.volts = 0. self.wsize = 61 self.filter_order = 3 self.theta = rospy.get_param("~theta") self.off_y = rospy.get_param("~off_y") self.abcd = rospy.get_param("~abcd") self.maximum_time = rospy.get_param("~maximum_time") self.sg = savitzky.savitzky_golay(window_size=self.wsize, order=self.filter_order) size = 2 * self.wsize + 1 self.volt_filt = 48000 * np.ones(size) rospy.Subscriber("/power_board/voltage", Float64, self.callback) self.pub_power = rospy.Publisher('/power_state', PowerState) self.msg_power = PowerState()
def main(argv): rospy.init_node('csv_proc') volt_values=[] time_values=[] sg = savitzky.savitzky_golay(window_size=901, order=3) #################### # Parameters for the Python script #################### filename = rosparam.get_param("/csv_proc/file_name") robot_name = rosparam.get_param("/csv_proc/robot_name") mode = rosparam.get_param("/csv_proc/mode") yaml_file = open("voltage_filter.yaml", "w") yl = {} if(mode=="update"): abcd = rosparam.get_param("/csv_proc/abcd") try: opts, args = getopt.getopt(argv,"hf:r:",["ifile=", "irobot="]) except getopt.GetoptError: print 'time_volt.py -f <filename> -r <robotname>' sys.exit(2) for opt, arg in opts: if opt == '-h': print 'time_volt.py -f <filename> -r <robotname>' sys.exit() elif opt in ("-f", "--ifile"): filename = arg elif opt in ("-r", "--irobot"): robot_name = arg #################### # Opening the csv file and getting the values for time and voltage #################### with open(filename, 'rb') as csvfile: csvreader = csv.reader(csvfile, delimiter=' ', quotechar='|') for row in csvreader: row = row[0].split(',') volt_v = (float)(row[1]) * 1000.0 if(volt_v < 48000 and volt_v > 44000): time_values.append((float)(row[0])) volt_values.append(volt_v) time_values[:] = [x - time_values[0] for x in time_values] time_values = time_values[::-1] #################### # Plotting graphics for the Voltage vs Time #################### pylab.figure(1) pylab.plot(volt_values, time_values) pylab.ylabel("Time elapsed(seconds)") pylab.xlabel("Voltage(mV)") pylab.title("Time x Volt,file="+ filename.replace('.csv','')) pylab.grid(True) secArray = np.asarray(time_values) voltArray = np.asarray(volt_values) # Polyfit for the voltage values z1 = np.polyfit(voltArray, secArray,1) z2 = np.polyfit(voltArray, secArray,2) z3 = np.polyfit(voltArray, secArray,3) # Linear space for better visualization xp = np.linspace(49000, 43000, 100) # Generating the polynomial function from the fit p1 = np.poly1d(z1) p2 = np.poly1d(z2) p3 = np.poly1d(z3) pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) pylab.savefig(filename.replace('.csv',''), format="pdf") #pylab.show() #################### # Residuals Analysis #################### pylab.figure(2) pylab.ylabel("Residuals(s)") pylab.xlabel("Voltage(mV)") pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) #Evaluating the polynomial through all the volt values z1_val = np.polyval(z1, volt_values) z2_val = np.polyval(z2, volt_values) z3_val = np.polyval(z3, volt_values) # Getting the residuals from the fit functions compared to the real values z1_res = time_values - z1_val z2_res = time_values - z2_val z3_res = time_values - z3_val pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) pylab.grid() pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) pylab.savefig(filename.replace('.csv','')+'_res', format="pdf") ################### # Savitzky Golay Filter Applied to the Battery Voltage ################### values_filt = sg.filter(voltArray) pylab.figure(3) pylab.subplot(211) pylab.plot(voltArray, time_values) pylab.grid(True) pylab.title('Comparison between real and filtered data') pylab.ylabel('Real Values(mV)') pylab.subplot(212) pylab.plot(values_filt, time_values) pylab.grid(True) pylab.ylabel('Filtered Values(mV)') pylab.xlabel('Time(s)') ##### ################### # Filtered fits ################### pylab.figure(4) pylab.plot(values_filt, time_values) pylab.ylabel("Time elapsed(seconds)") pylab.xlabel("Voltage(mV)") pylab.title("Time x Volt,file="+ filename.replace('.csv','')) pylab.grid(True) secArray = np.asarray(time_values) # Polyfit for the voltage values z1 = np.polyfit(values_filt, secArray,1) z2 = np.polyfit(values_filt, secArray,2) z3 = np.polyfit(values_filt, secArray,3) if (mode=="initial"): z3_l = [] for el in z3: z3_l.append((float)(el)) abcd = z3_l yl["abcd"] = z3_l yl["theta"] = 0. yl["off_y"] = 0. # Linear space for better visualization xp = np.linspace(49000, 43000, 100) # Generating the polynomial function from the fit p1 = np.poly1d(z1) p2 = np.poly1d(z2) p3 = np.poly1d(z3) pylab.plot(xp, p1(xp), 'r-', xp, p2(xp), 'g-', xp, p3(xp), 'm-') pylab.text(46000, 11900, 'p1=' + p1.__str__(), bbox=dict(facecolor='red', alpha=0.5)) pylab.text(46000, 11600, 'p2=' + p2.__str__(), bbox=dict(facecolor='green', alpha=0.5)) pylab.text(46000, 11200, 'p3=' + p3.__str__(), bbox=dict(facecolor='magenta', alpha=0.5)) #################### # Residuals Analysis for the filtered Signal #################### pylab.figure(5) pylab.ylabel("Residuals(s)") pylab.xlabel("Voltage(mV)") pylab.title("Residuals x Voltage,file="+ filename.replace('.csv','')) #Evaluating the polynomial through all the time values z1_val = np.polyval(z1, values_filt) z2_val = np.polyval(z2, values_filt) z3_val = np.polyval(z3, values_filt) # Getting the residuals from the fit functions compared to the real values z1_res = time_values - z1_val z2_res = time_values - z2_val z3_res = time_values - z3_val pylab.plot(time_values, z1_res, time_values, z2_res, time_values, z3_res) pylab.grid() pylab.legend(('Residuals 1st order', 'Residuals 2nd order', 'Residuals 3rd order')) #################### # Polynomial Evaluation for the filtered signal and the function from the non-moving case #################### if(mode == "update"): pylab.figure(6) poly_vals = np.polyval(abcd, values_filt) pylab.plot(values_filt, poly_vals, values_filt, time_values) pylab.legend(('Polynomial', 'Real')) pylab.grid() ##### pylab.figure(7) pylab.ylabel("Time available(seconds)") pylab.xlabel("Voltage(mV)") pylab.title("Time x Volt,file="+ filename.replace('.csv','')) pylab.grid(True) poly_vals = np.polyval(abcd, values_filt) ss = lambda y1, y2: ((y1-y2)**2).sum() #theta = -0.07 #new_x = values_filt*cos(theta) - poly_vals*sin(theta) #new_y = values_filt*sin(theta) + poly_vals*cos(theta) theta = -0.2 theta_values = [] cost_values = [] off_values = [] while theta < 0.2: theta +=0.01 new_x = values_filt*cos(theta) - poly_vals*sin(theta) new_y = values_filt*sin(theta) + poly_vals*cos(theta) off_y = -6000 cost_values_i =[] off_y_values_i=[] while off_y < 6000: off_y +=200 new_y_temp = new_y new_y_temp = new_y_temp + off_y ss1=ss(time_values,new_y_temp) print ss1, off_y cost_values_i.append(ss1) off_y_values_i.append(off_y) #ss1=ss(time_values,new_y) #print ss1, theta #cost_values.append(ss1) theta_values.append(theta) cost_min = min(cost_values_i) cost_min_index = cost_values_i.index(cost_min) cost_values.append(cost_values_i[cost_min_index]) off_values.append(off_y_values_i[cost_min_index]) cost_min = min(cost_values) cost_min_index = cost_values.index(cost_min) theta = theta_values[cost_min_index] off_y = off_values[cost_min_index] new_x = values_filt*cos(theta) - poly_vals*sin(theta) new_y = values_filt*sin(theta) + poly_vals*cos(theta) new_y = new_y + off_y yl["abcd"] = abcd yl["theta"] = theta yl["off_y"] = off_y yl["maximum_time"] = (float)(new_y[0]) pylab.plot(poly_vals, values_filt, time_values, values_filt, new_y, values_filt) pylab.legend(('Poly not moving', 'Real', 'Shifted Fit')) yaml.safe_dump(yl, yaml_file,default_flow_style=False) yaml_file.close() pylab.show()
def __init__(self, server, online, offFile, cTimeFile, dTimeFile,recTimeFile, refTimeFile, txTimeFile, kfFile, mavFile): self.server = server self.offFileName = offFile self.cTimeFileName = cTimeFile self.dTimeFileName = dTimeFile self.recTimeFileName = recTimeFile self.refTimeFileName = refTimeFile self.txTimeFileName = txTimeFile self.kfFileName = kfFile self.mavFileName = mavFile self.realTime = "realtime.txt" self.thread_update = None self.response = None self.ntpClient = ntplib.NTPClient() self.fileOffsets = open("on" + self.offFileName, "w") self.cTimeFile = open("on" + self.cTimeFileName, "w") self.dTimeFile = open("on" + self.dTimeFileName, "w") self.realTimeFile = open("on" + self.realTime, "w") self.refTimeFile = open("on" + self.refTimeFileName, "w") self.txTimeFile = open("on" + self.txTimeFileName, "w") self.recTimeFile = open("on" + self.recTimeFileName, "w") self.kfFile2 = open("on" + self.kfFileName, "w") self.kfFile = open(self.kfFileName, "w") self.mavFile = open("on" + self.mavFileName, "w") self.online = online self.file_off = open("off_kalman_levine.txt", "w") self.file_off2 = open("off_kalman2.txt", "w") self.kf = kalman_class.Kalman() self.kl = kalman_levine.kalman_levine() self.kf2 = kalman_2nd.Kalman2() self.sg = savitzky.savitzky_golay(window_size=39, order=5) self.estimateds = zeros(39) self.sg_index = 0 self.sgK = 1 # this variable controls if savitzky is applied to the Kalman Filter self.sgL = 1 # this variable controls if savitzky is applied to the Levines Kalman Filter self.sgK2 = 1 self.kf_off = mat([[0.],[0.]]) self.kf2_off = mat([[0.],[0.]]) self.kl_off = mat([[0.],[0.]]) self.init_kalman = 1 self.delta_t1 = 0. self.delta_t4 = 0. self.delta_t12 = 0. self.delta_t42 = 0. self.delta_t1av = 0. self.delta_t4av = 0. if(self.online): self.response = self.ntpClient.request(self.server, version=4) self.t1_kalman = (float)(self.response.orig_time) self.t4_kalman = (float)(self.response.dest_time) self.t1_kalman_average = (float)(self.response.orig_time) self.t4_kalman_average = (float)(self.response.dest_time) self.t1_av = (float)(self.response.orig_time) self.t4_av = (float)(self.response.dest_time) else: self.orig_time = open(self.cTimeFileName).readlines() self.dest_time = open(self.dTimeFileName).readlines() self.tx_time = open(self.txTimeFileName).readlines() self.recv_time = open(self.recTimeFileName).readlines() self.off_rec = open(self.offFileName).readlines() for a,b,c, d, e in itertools.izip(self.orig_time, self.dest_time, self.tx_time, self.recv_time, self.off_rec): self.orig_time[self.orig_time.index(a)] = (float)(a.strip()) self.dest_time[self.dest_time.index(b)] = (float)(b.strip()) self.tx_time[self.tx_time.index(c)] = (float)(c.strip()) self.recv_time[self.recv_time.index(d)] = (float)(d.strip()) self.off_rec[self.off_rec.index(e)] = (float)(e.strip()) self.off_recT = zeros(len(self.off_rec)) self.off_recT2 = zeros(len(self.off_rec)) self.off_recL = zeros(len(self.off_rec)) self.t1_kalman = (self.orig_time[0]) self.t4_kalman = (self.dest_time[0]) self.t1_kalman2 = (self.orig_time[0]) self.t4_kalman2 = (self.dest_time[0]) self.t1_kl = (self.orig_time[0]) self.t4_kl = (self.dest_time[0]) self.t1_kalman_average = (self.orig_time[0]) self.t4_kalman_average = (self.dest_time[0]) self.t1_av = (self.orig_time[0]) self.t4_av = (self.dest_time[0]) self.t1_old = self.t1_kalman self.t4_old = self.t4_kalman self.enable_kalman = 0 self.enable_ntpdate = 0 self.actual_calculated_offset = 0. logger.info("Init Funcion Concluded") self.past_offs = [0,0,0,0,0] self.offs_moving = [0,0,0,0,0] self.new_off = 0 self.start_time = time.time() self.update_once = 1