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 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
