Skip to content
Browse files

design a new algorithm!

  • Loading branch information...
1 parent 0821e72 commit be15e96220c902b0fe85b519a45528f54c22e941 Marije Baalman committed Feb 16, 2012
Showing with 91 additions and 70 deletions.
  1. +91 −70 tools/plotdata/plotloggvb.py
View
161 tools/plotdata/plotloggvb.py
@@ -65,15 +65,28 @@ class LinePlot(HasTraits):
def __init__(self, plot ):
super(LinePlot, self).__init__()
+
self.plot = plot
-def makeplot( x,y,title ):
+def makeplot( x,y,title, miny, maxy ):
plotdata = ArrayPlotData(x=x, y=y ) #, y2=y2, y3=y3 )
plot = Plot(plotdata)
plot.plot(("x", "y"), type="line", color="blue")
plot.title = title
+
+ spec_range = plot.value_mapper.range
+ spec_range.low = miny
+ spec_range.high = maxy
+
return plot
+def makeplot2( x,y,title):
+ plotdata = ArrayPlotData(x=x, y=y ) #, y2=y2, y3=y3 )
+ plot = Plot(plotdata)
+ plot.plot(("x", "y"), type="line", color="blue")
+ plot.title = title
+
+ return plot
def online_variance(data):
n = 0
@@ -103,48 +116,51 @@ def motion_state_machine( meanF, stdF, meanS, stdS, deltaT ):
global motion_state
global still_count
global motion_state_index
- if motion_state == 'still':
- if stdF > 0.3:
- motion_state = 'accelerating'
- motion_state_index = 2.
- elif motion_state == 'accelerating':
- if meanF < 0.5:
- motion_state = 'steady_motion'
- motion_state_index = 1.
- elif motion_state == 'steady_motion':
- if stdF > 0.25 and meanF < -0.5:
- motion_state = 'decelerating'
- motion_state_index = 3.
- #elif motion_state == 'decelerating':
- #if stdS < 0.1 and stdF < 0.1:
- #motion_state = 'still'
- #motion_state_index = 0.
- else:
- if stdS < 0.1 and stdF < 0.1:
- still_count = still_count + deltaT * 0.001
- if still_count > 3.:
- still_count = 0.
- motion_state = 'still'
- motion_state_index = 0.
+
+ if stdF > 0.1 and stdS > 0.1:
+ motion_state = 'motion'
+ motion_state_index = 1.
+ if stdF < 0.04 and stdS < 0.04:
+ motion_state = 'still'
+ motion_state_index = 0.
+ if stdF > 0.2 and meanF > 0.1:
+ motion_state = 'accelerating'
+ motion_state_index = 2.
+ if stdF > 0.3 and meanF < -0.1:
+ motion_state = 'decelerating'
+ motion_state_index = 3.
+
+ #still_count = still_count + deltaT * 0.001
+ #if still_count > 3.:
+ #still_count = 0.
+ #motion_state = 'still'
+ #motion_state_index = 0.
-def calculate_speed( meanF, deltaT ):
+def calculate_speed( meanF, rawF, deltaT ):
global speed
+ global offset
+ global motion_state
+ #global motion_still_count
if motion_state == 'still':
- speed = 0.
+ speed = speed * 0.99
+ offset = 0.99*offset + 0.01*meanF
else: #moving
- speed += meanF * deltaT * 0.001 * 3.6
+ #speed += ( (rawF+meanF)/2. - offset ) * deltaT * 0.001 * 3.6
+ speed += (0.35*(rawF- offset) + 0.65*(meanF- offset) ) * deltaT * 0.001 * 3.6
#speed = speed * 3.6 # km/h
-def substract_offset( value, offset ):
- return value - offset
+#def substract_offset( value, offset ):
+ #return value - offset
def openFile( filename ):
global motion_state
global motion_state_index
global speed
+ global offset
global still_count
still_count = 0.
speed = 0.
+ offset = 0.
motion_state_index = 0.
motion_state = 'still'
csvfile = open(filename, "rb")
@@ -160,24 +176,10 @@ def openFile( filename ):
if len( row ) >= rowlength:
rowlength = len( row )
rows.append( list(map(float, row) ) )
- #diffrows.append( list(map(float, [ row[1], row[4] ]) ) )
-
- #print rows
- #print diffrows
- #print len(rows), len(diffrows)
marray = numpy.asarray( rows )
#diffarray = numpy.asarray( diffrows )
- #marray1 = marray[ marray[:,1] == 1]
- #marray2 = marray[ marray[:,1] == 2]
-
- #diffarray1 = diffarray[ marray[:,1] == 1]
- #diffarray2 = diffarray[ marray[:,1] == 2]
-
- #hasNode1 = False
- #hasNode2 = False
-
mplot = marray.transpose(1,0)
#plotX = makeplot( mplot[0], mplot[1], "X" )
@@ -213,18 +215,19 @@ def openFile( filename ):
##print meanDT.size
### calculate motion
- #speeds = []
- #motions = []
- #for i in range( 1, meanDT.size ):
+ speeds = []
+ motions = []
+ for i in range( 1, mplot[4].size ):
##print i
- #motion_state_machine( means[1][i], stds[1][i], means[0][i], stds[0][i], meanDT[i] )
- #calculate_speed( means[1][i], meanDT[i] )
+ motion_state_machine( mplot[4][i], mplot[5][i], mplot[6][i], mplot[7][i], 5 )
+ #calculate_speed( mplot[4][i], 5 )
+ calculate_speed( mplot[4][i], mplot[14][i], 5 )
##calculate_speed( means[1][i], deltatimes[i] )
- #speeds.append( speed )
- #motions.append( motion_state_index )
+ speeds.append( speed )
+ motions.append( motion_state_index )
##print speed
- #mspeeds = numpy.asarray( speeds )
- #mmotion = numpy.asarray( motions )
+ mspeeds = numpy.asarray( speeds )
+ mmotion = numpy.asarray( motions )
@@ -234,25 +237,43 @@ def openFile( filename ):
#plotZm = makeplot( mplot[0], means[2], "Z mean" )
#ContainerHor(plotDT,plotDTm,plotDTs).configure_traits()
- timeaxis = mplot[0] * 0.001 / 60
-
-
- plot1 = makeplot( timeaxis, mplot[1], "motion" )
- plot2 = makeplot( timeaxis, mplot[2], "speed" )
- plot3 = makeplot( timeaxis, mplot[4], "mean" )
- plot4 = makeplot( timeaxis, mplot[5], "std" )
- plot5 = makeplot( timeaxis, mplot[6], "side mean" )
- plot6 = makeplot( timeaxis, mplot[7], "side std" )
- plot7 = makeplot( timeaxis, mplot[8], "grav mean" )
- plot8 = makeplot( timeaxis, mplot[9], "grav std" )
- plot9 = makeplot( timeaxis, mplot[10], "offset" )
- plot10 = makeplot( timeaxis, mplot[11], "stilltime" )
- ContainerHor4(plot1,plot2,plot3,plot4).configure_traits()
- ContainerHor4(plot5,plot6,plot7,plot8).configure_traits()
- ContainerHor4(plot2,plot3,plot9,plot10).configure_traits()
+ timeaxis = mplot[0] * 0.001
+
+ plotC1 = makeplot( timeaxis, mmotion, "motion calc", -0.5, 3.5 )
+ plotC2 = makeplot( timeaxis, mspeeds, "speed calc", -20, 40 )
+ plotC3 = makeplot2( timeaxis, mspeeds, "speed calc" )
+
+ plot1 = makeplot( timeaxis, mplot[1], "motion", -0.5, 3.5 )
+ plot2 = makeplot( timeaxis, mplot[2], "speed", -20, 20 )
+ plot3 = makeplot( timeaxis, mplot[4], "mean", -2.5, 2.5 )
+ plot4 = makeplot( timeaxis, mplot[5], "std", -0.05, 0.6 )
+ plot5 = makeplot( timeaxis, mplot[6], "side mean", -2.5, 2.5 )
+ plot6 = makeplot( timeaxis, mplot[7], "side std", -0.05, 0.6 )
+ plot7 = makeplot( timeaxis, mplot[8], "grav mean", -2.5, 2.5 )
+ plot8 = makeplot( timeaxis, mplot[9], "grav std", -0.05, 0.6 )
+ plot9 = makeplot( timeaxis, mplot[10], "offset", -0.5, 0.5 )
+ plot10 = makeplot( timeaxis, mplot[13], "stilltime", 0, 5 )
+ plot11 = makeplot( timeaxis, mplot[14], "acc forward", -3.5, 3.5 )
+ plot12 = makeplot( timeaxis, mplot[15], "acc side", -3.5, 3.5 )
+ plot13 = makeplot( timeaxis, mplot[16], "acc grav", 6.5, 13.5 )
+
+ ContainerHor4(plotC1,plotC2,plot1,plot2).configure_traits()
+ ContainerHor4(plotC1,plotC2,plot2,plot3).configure_traits()
+ ContainerHor4(plotC1,plotC2,plot4,plot6).configure_traits()
+
+ #ContainerHor4(plot1,plot2,plot3,plot4).configure_traits()
+ #ContainerHor4(plot5,plot6,plot7,plot8).configure_traits()
+ #ContainerHor4(plot2,plot3,plot9,plot10).configure_traits()
+ #ContainerHor4(plot11,plot12,plot13,plot2).configure_traits()
#ContainerHor4(plotYm,plotYs,plotXm,plotXs).configure_traits()
- LinePlot( plot4 ).configure_traits();
- LinePlot( plot6 ).configure_traits();
+ #ContainerHor4(plot3,plot4,plot6,plot8).configure_traits()
+ #ContainerHor4(plot11,plot3,plot4,plot2).configure_traits()
+ #LinePlot( plot4 ).configure_traits();
+ #LinePlot( plot6 ).configure_traits();
+ #LinePlot( plot8 ).configure_traits();
+ #LinePlot( plot11 ).configure_traits();
+ #LinePlot( plot13 ).configure_traits();
+ LinePlot( plotC3 ).configure_traits();
#ContainerGrid(plotXm,plotYm,plotZm,plotXs,plotYs,plotZs).configure_traits()

0 comments on commit be15e96

Please sign in to comment.
Something went wrong with that request. Please try again.