Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse code

design a new algorithm!

  • Loading branch information...
commit be15e96220c902b0fe85b519a45528f54c22e941 1 parent 0821e72
Marije Baalman authored

Showing 1 changed file with 91 additions and 70 deletions. Show diff stats Hide diff stats

  1. +91 70 tools/plotdata/plotloggvb.py
161 tools/plotdata/plotloggvb.py
@@ -65,15 +65,28 @@ class LinePlot(HasTraits):
65 65
66 66 def __init__(self, plot ):
67 67 super(LinePlot, self).__init__()
  68 +
68 69 self.plot = plot
69 70
70   -def makeplot( x,y,title ):
  71 +def makeplot( x,y,title, miny, maxy ):
71 72 plotdata = ArrayPlotData(x=x, y=y ) #, y2=y2, y3=y3 )
72 73 plot = Plot(plotdata)
73 74 plot.plot(("x", "y"), type="line", color="blue")
74 75 plot.title = title
  76 +
  77 + spec_range = plot.value_mapper.range
  78 + spec_range.low = miny
  79 + spec_range.high = maxy
  80 +
75 81 return plot
76 82
  83 +def makeplot2( x,y,title):
  84 + plotdata = ArrayPlotData(x=x, y=y ) #, y2=y2, y3=y3 )
  85 + plot = Plot(plotdata)
  86 + plot.plot(("x", "y"), type="line", color="blue")
  87 + plot.title = title
  88 +
  89 + return plot
77 90
78 91 def online_variance(data):
79 92 n = 0
@@ -103,48 +116,51 @@ def motion_state_machine( meanF, stdF, meanS, stdS, deltaT ):
103 116 global motion_state
104 117 global still_count
105 118 global motion_state_index
106   - if motion_state == 'still':
107   - if stdF > 0.3:
108   - motion_state = 'accelerating'
109   - motion_state_index = 2.
110   - elif motion_state == 'accelerating':
111   - if meanF < 0.5:
112   - motion_state = 'steady_motion'
113   - motion_state_index = 1.
114   - elif motion_state == 'steady_motion':
115   - if stdF > 0.25 and meanF < -0.5:
116   - motion_state = 'decelerating'
117   - motion_state_index = 3.
118   - #elif motion_state == 'decelerating':
119   - #if stdS < 0.1 and stdF < 0.1:
120   - #motion_state = 'still'
121   - #motion_state_index = 0.
122   - else:
123   - if stdS < 0.1 and stdF < 0.1:
124   - still_count = still_count + deltaT * 0.001
125   - if still_count > 3.:
126   - still_count = 0.
127   - motion_state = 'still'
128   - motion_state_index = 0.
  119 +
  120 + if stdF > 0.1 and stdS > 0.1:
  121 + motion_state = 'motion'
  122 + motion_state_index = 1.
  123 + if stdF < 0.04 and stdS < 0.04:
  124 + motion_state = 'still'
  125 + motion_state_index = 0.
  126 + if stdF > 0.2 and meanF > 0.1:
  127 + motion_state = 'accelerating'
  128 + motion_state_index = 2.
  129 + if stdF > 0.3 and meanF < -0.1:
  130 + motion_state = 'decelerating'
  131 + motion_state_index = 3.
  132 +
  133 + #still_count = still_count + deltaT * 0.001
  134 + #if still_count > 3.:
  135 + #still_count = 0.
  136 + #motion_state = 'still'
  137 + #motion_state_index = 0.
129 138
130   -def calculate_speed( meanF, deltaT ):
  139 +def calculate_speed( meanF, rawF, deltaT ):
131 140 global speed
  141 + global offset
  142 + global motion_state
  143 + #global motion_still_count
132 144 if motion_state == 'still':
133   - speed = 0.
  145 + speed = speed * 0.99
  146 + offset = 0.99*offset + 0.01*meanF
134 147 else: #moving
135   - speed += meanF * deltaT * 0.001 * 3.6
  148 + #speed += ( (rawF+meanF)/2. - offset ) * deltaT * 0.001 * 3.6
  149 + speed += (0.35*(rawF- offset) + 0.65*(meanF- offset) ) * deltaT * 0.001 * 3.6
136 150 #speed = speed * 3.6 # km/h
137 151
138   -def substract_offset( value, offset ):
139   - return value - offset
  152 +#def substract_offset( value, offset ):
  153 + #return value - offset
140 154
141 155 def openFile( filename ):
142 156 global motion_state
143 157 global motion_state_index
144 158 global speed
  159 + global offset
145 160 global still_count
146 161 still_count = 0.
147 162 speed = 0.
  163 + offset = 0.
148 164 motion_state_index = 0.
149 165 motion_state = 'still'
150 166 csvfile = open(filename, "rb")
@@ -160,24 +176,10 @@ def openFile( filename ):
160 176 if len( row ) >= rowlength:
161 177 rowlength = len( row )
162 178 rows.append( list(map(float, row) ) )
163   - #diffrows.append( list(map(float, [ row[1], row[4] ]) ) )
164   -
165   - #print rows
166   - #print diffrows
167   - #print len(rows), len(diffrows)
168 179
169 180 marray = numpy.asarray( rows )
170 181 #diffarray = numpy.asarray( diffrows )
171 182
172   - #marray1 = marray[ marray[:,1] == 1]
173   - #marray2 = marray[ marray[:,1] == 2]
174   -
175   - #diffarray1 = diffarray[ marray[:,1] == 1]
176   - #diffarray2 = diffarray[ marray[:,1] == 2]
177   -
178   - #hasNode1 = False
179   - #hasNode2 = False
180   -
181 183 mplot = marray.transpose(1,0)
182 184
183 185 #plotX = makeplot( mplot[0], mplot[1], "X" )
@@ -213,18 +215,19 @@ def openFile( filename ):
213 215
214 216 ##print meanDT.size
215 217 ### calculate motion
216   - #speeds = []
217   - #motions = []
218   - #for i in range( 1, meanDT.size ):
  218 + speeds = []
  219 + motions = []
  220 + for i in range( 1, mplot[4].size ):
219 221 ##print i
220   - #motion_state_machine( means[1][i], stds[1][i], means[0][i], stds[0][i], meanDT[i] )
221   - #calculate_speed( means[1][i], meanDT[i] )
  222 + motion_state_machine( mplot[4][i], mplot[5][i], mplot[6][i], mplot[7][i], 5 )
  223 + #calculate_speed( mplot[4][i], 5 )
  224 + calculate_speed( mplot[4][i], mplot[14][i], 5 )
222 225 ##calculate_speed( means[1][i], deltatimes[i] )
223   - #speeds.append( speed )
224   - #motions.append( motion_state_index )
  226 + speeds.append( speed )
  227 + motions.append( motion_state_index )
225 228 ##print speed
226   - #mspeeds = numpy.asarray( speeds )
227   - #mmotion = numpy.asarray( motions )
  229 + mspeeds = numpy.asarray( speeds )
  230 + mmotion = numpy.asarray( motions )
228 231
229 232
230 233
@@ -234,25 +237,43 @@ def openFile( filename ):
234 237 #plotZm = makeplot( mplot[0], means[2], "Z mean" )
235 238 #ContainerHor(plotDT,plotDTm,plotDTs).configure_traits()
236 239
237   - timeaxis = mplot[0] * 0.001 / 60
238   -
239   -
240   - plot1 = makeplot( timeaxis, mplot[1], "motion" )
241   - plot2 = makeplot( timeaxis, mplot[2], "speed" )
242   - plot3 = makeplot( timeaxis, mplot[4], "mean" )
243   - plot4 = makeplot( timeaxis, mplot[5], "std" )
244   - plot5 = makeplot( timeaxis, mplot[6], "side mean" )
245   - plot6 = makeplot( timeaxis, mplot[7], "side std" )
246   - plot7 = makeplot( timeaxis, mplot[8], "grav mean" )
247   - plot8 = makeplot( timeaxis, mplot[9], "grav std" )
248   - plot9 = makeplot( timeaxis, mplot[10], "offset" )
249   - plot10 = makeplot( timeaxis, mplot[11], "stilltime" )
250   - ContainerHor4(plot1,plot2,plot3,plot4).configure_traits()
251   - ContainerHor4(plot5,plot6,plot7,plot8).configure_traits()
252   - ContainerHor4(plot2,plot3,plot9,plot10).configure_traits()
  240 + timeaxis = mplot[0] * 0.001
  241 +
  242 + plotC1 = makeplot( timeaxis, mmotion, "motion calc", -0.5, 3.5 )
  243 + plotC2 = makeplot( timeaxis, mspeeds, "speed calc", -20, 40 )
  244 + plotC3 = makeplot2( timeaxis, mspeeds, "speed calc" )
  245 +
  246 + plot1 = makeplot( timeaxis, mplot[1], "motion", -0.5, 3.5 )
  247 + plot2 = makeplot( timeaxis, mplot[2], "speed", -20, 20 )
  248 + plot3 = makeplot( timeaxis, mplot[4], "mean", -2.5, 2.5 )
  249 + plot4 = makeplot( timeaxis, mplot[5], "std", -0.05, 0.6 )
  250 + plot5 = makeplot( timeaxis, mplot[6], "side mean", -2.5, 2.5 )
  251 + plot6 = makeplot( timeaxis, mplot[7], "side std", -0.05, 0.6 )
  252 + plot7 = makeplot( timeaxis, mplot[8], "grav mean", -2.5, 2.5 )
  253 + plot8 = makeplot( timeaxis, mplot[9], "grav std", -0.05, 0.6 )
  254 + plot9 = makeplot( timeaxis, mplot[10], "offset", -0.5, 0.5 )
  255 + plot10 = makeplot( timeaxis, mplot[13], "stilltime", 0, 5 )
  256 + plot11 = makeplot( timeaxis, mplot[14], "acc forward", -3.5, 3.5 )
  257 + plot12 = makeplot( timeaxis, mplot[15], "acc side", -3.5, 3.5 )
  258 + plot13 = makeplot( timeaxis, mplot[16], "acc grav", 6.5, 13.5 )
  259 +
  260 + ContainerHor4(plotC1,plotC2,plot1,plot2).configure_traits()
  261 + ContainerHor4(plotC1,plotC2,plot2,plot3).configure_traits()
  262 + ContainerHor4(plotC1,plotC2,plot4,plot6).configure_traits()
  263 +
  264 + #ContainerHor4(plot1,plot2,plot3,plot4).configure_traits()
  265 + #ContainerHor4(plot5,plot6,plot7,plot8).configure_traits()
  266 + #ContainerHor4(plot2,plot3,plot9,plot10).configure_traits()
  267 + #ContainerHor4(plot11,plot12,plot13,plot2).configure_traits()
253 268 #ContainerHor4(plotYm,plotYs,plotXm,plotXs).configure_traits()
254   - LinePlot( plot4 ).configure_traits();
255   - LinePlot( plot6 ).configure_traits();
  269 + #ContainerHor4(plot3,plot4,plot6,plot8).configure_traits()
  270 + #ContainerHor4(plot11,plot3,plot4,plot2).configure_traits()
  271 + #LinePlot( plot4 ).configure_traits();
  272 + #LinePlot( plot6 ).configure_traits();
  273 + #LinePlot( plot8 ).configure_traits();
  274 + #LinePlot( plot11 ).configure_traits();
  275 + #LinePlot( plot13 ).configure_traits();
  276 + LinePlot( plotC3 ).configure_traits();
256 277
257 278 #ContainerGrid(plotXm,plotYm,plotZm,plotXs,plotYs,plotZs).configure_traits()
258 279

0 comments on commit be15e96

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