def filterData(reconData, logDir, fileEnding, samplingInterval, coefficients): # Filter the reconstructed data #print "Filtering the data..." filteredData = [] time, x, y, z = splitData(reconData) xFiltered = scipy.signal.convolve(coefficients, x) yFiltered = scipy.signal.convolve(coefficients, y) zFiltered = scipy.signal.convolve(coefficients, z) numberToDrop = len(coefficients) - 1 time = time[numberToDrop:] xFiltered = xFiltered[numberToDrop:] yFiltered = yFiltered[numberToDrop:] zFiltered = zFiltered[numberToDrop:] for i, v in enumerate(time): sample = Sample() sample.time = time[i] sample.position.x = xFiltered[i] sample.position.y = yFiltered[i] sample.position.z = zFiltered[i] filteredData.append(sample) filteredData = filteredData[:-200] exportData(logDir + "FilteredData_" + fileEnding + ".txt", filteredData) return [filteredData]
def executeAlgorithm(self):
self.reconstructedSignal = []
nextTimeToRecord = 0
timeDiff = self.rawSignal[0].sample.time % self.samplingInterval
if timeDiff == 0:
self.reconstructedSignal.append( deepcopy(self.rawSignal[0].sample) )
nextTimeToRecord = self.rawSignal[0].sample.time + \
self.samplingInterval
else:
change = self.samplingInterval - timeDiff
newSample = Sample()
newSample.time = self.rawSignal[0].sample.time + change
newSample.position = deepcopy(self.rawSignal[0].sample.position)
self.reconstructedSignal.append(newSample)
nextTimeToRecord = newSample.time + self.samplingInterval
for index, predictionSample in enumerate(self.rawSignal):
if predictionSample.sample.time == nextTimeToRecord:
self.reconstructedSignal.append( deepcopy(predictionSample.sample) )
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
elif predictionSample.sample.time > nextTimeToRecord:
while predictionSample.sample.time > nextTimeToRecord:
deltaTime = nextTimeToRecord - self.rawSignal[index-1].sample.time
deltaTimeVector = Vector(deltaTime, deltaTime, deltaTime)
deltaPosition = self.rawSignal[index-1].velocity * deltaTimeVector
newPosition = self.rawSignal[index-1].sample.position + deltaPosition
newSample = Sample(nextTimeToRecord, newPosition)
self.reconstructedSignal.append( newSample )
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
if predictionSample.sample.time == nextTimeToRecord:
self.reconstructedSignal.append( deepcopy(predictionSample.sample) )
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
def filterData(reconData, logDir, fileEnding, samplingInterval, coefficients):
# Filter the reconstructed data
#print "Filtering the data..."
filteredData = []
time, x, y, z = splitData(reconData)
xFiltered = scipy.signal.convolve(coefficients, x)
yFiltered = scipy.signal.convolve(coefficients, y)
zFiltered = scipy.signal.convolve(coefficients, z)
numberToDrop = len(coefficients) - 1
time = time[numberToDrop:]
xFiltered = xFiltered[numberToDrop:]
yFiltered = yFiltered[numberToDrop:]
zFiltered = zFiltered[numberToDrop:]
for i, v in enumerate(time):
sample = Sample()
sample.time = time[i]
sample.position.x = xFiltered[i]
sample.position.y = yFiltered[i]
sample.position.z = zFiltered[i]
filteredData.append(sample)
filteredData = filteredData[:-200]
exportData(logDir + "FilteredData_" + fileEnding + ".txt", filteredData)
return [filteredData]
def recordSample(self, env):
s = Sample()
s.time = env.time
s.simcarPos = env.simcar.p.myclone()
s.simcarHeading = env.simcar.h.myclone()
s.simcarFracIndex = env.simcar.fracIndex
s.simcarSpeed = env.simcar.speed
s.simcarRoadIndex = env.simcar.roadIndex
def calculateEstSample(self, interpolationSample, currentTime):
estimatedSample = Sample()
estimatedSample.time = currentTime
estimatedSample.position = self.calculateEstPosition(interpolationSample, currentTime)
return estimatedSample
def findFirstSample(self):
timeDiff = self.rawSignal[0].sample.time % self.samplingInterval
if timeDiff == 0:
return deepcopy(self.rawSignal[0].sample)
else:
change = self.samplingInterval - timeDiff
newSample = Sample()
newSample.time = self.rawSignal[0].sample.time + change
newSample.position = deepcopy(self.rawSignal[0].sample.position)
return newSample
def findFirstSample(self):
timeDiff = self.rawSignal[0].sample.time % self.samplingInterval
if timeDiff == 0:
return deepcopy(self.rawSignal[0].sample)
else:
change = self.samplingInterval - timeDiff
newSample = Sample()
newSample.time = self.rawSignal[0].sample.time + change
newSample.position = deepcopy(self.rawSignal[0].sample.position)
return newSample
def shiftData(self):
tempData = []
firstTime = self.rawData[0].time
for sample in self.rawData:
if sample.time >= firstTime:
newSample = Sample()
newSample.time = sample.time - firstTime + ( 10 * self.numberOfPaddingSamples)
newSample.position = copy( sample.position )
tempData.append(newSample)
self.rawData = tempData
def shiftData(self):
tempData = []
firstTime = self.rawData[0].time
for sample in self.rawData:
if sample.time >= firstTime:
newSample = Sample()
newSample.time = sample.time - firstTime + (
10 * self.numberOfPaddingSamples)
newSample.position = copy(sample.position)
tempData.append(newSample)
self.rawData = tempData
def scaleData(self):
tempData = []
for sample in self.rawData:
newSample = Sample()
newSample.time = sample.time
newSample.position = ( sample.position + self.scalingFactorA )
newSample.position.x = newSample.position.x * self.scalingFactorB.x
newSample.position.y = newSample.position.y * self.scalingFactorB.y
newSample.position.z = newSample.position.z * self.scalingFactorB.z
newSample.position = newSample.position - Vector(1,1,1)
tempData.append(newSample)
self.rawData = tempData
def scaleData(self):
tempData = []
for sample in self.rawData:
newSample = Sample()
newSample.time = sample.time
newSample.position = (sample.position + self.scalingFactorA)
newSample.position.x = newSample.position.x * self.scalingFactorB.x
newSample.position.y = newSample.position.y * self.scalingFactorB.y
newSample.position.z = newSample.position.z * self.scalingFactorB.z
newSample.position = newSample.position - Vector(1, 1, 1)
tempData.append(newSample)
self.rawData = tempData
def importData(self, fileName):
input = open( fileName, 'r' )
for line in input:
tokens = line.split('\t')
sample = Sample()
minutes = int( tokens[self.minutePosition] )
seconds = int( tokens[self.secondPosition] )
milliseconds = int( tokens[self.millisecondPosition] )
sample.time = minutes * 60 * 1000 + seconds * 1000 + milliseconds
sample.position.x = float( tokens[self.xPosition] )
sample.position.y = float( tokens[self.yPosition] )
sample.position.z = float( tokens[self.zPosition] )
self.rawData.append(sample)
input.close()
def importData(self, fileName):
input = open(fileName, 'r')
for line in input:
tokens = line.split('\t')
sample = Sample()
minutes = int(tokens[self.minutePosition])
seconds = int(tokens[self.secondPosition])
milliseconds = int(tokens[self.millisecondPosition])
sample.time = minutes * 60 * 1000 + seconds * 1000 + milliseconds
sample.position.x = float(tokens[self.xPosition])
sample.position.y = float(tokens[self.yPosition])
sample.position.z = float(tokens[self.zPosition])
self.rawData.append(sample)
input.close()
def executeAlgorithm(self):
self.reconstructedSignal = []
nextTimeToRecord = 0
timeDiff = self.rawSignal[0].sample.time % self.samplingInterval
if timeDiff == 0:
self.reconstructedSignal.append( deepcopy(self.rawSignal[0].sample) )
nextTimeToRecord = self.rawSignal[0].sample.time + \
self.samplingInterval
else:
change = self.samplingInterval - timeDiff
newSample = Sample()
newSample.time = self.rawSignal[0].sample.time + change
newSample.position = deepcopy(self.rawSignal[0].sample.position)
self.reconstructedSignal.append(newSample)
nextTimeToRecord = newSample.time + self.samplingInterval
interpolationSample = PredictionSample()
interpolationSample.sample = self.reconstructedSignal[0]
interpolationSample.velocity = self.rawSignal[0].velocity
for index, predictionSample in enumerate(self.rawSignal):
if predictionSample.sample.time == nextTimeToRecord:
estimatedSample = self.calculateEstSample(interpolationSample, \
nextTimeToRecord)
self.reconstructedSignal.append(estimatedSample)
targetSample = self.findTarget(predictionSample)
interpolationSample = self.findInterpolationSample(estimatedSample, \
targetSample)
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
elif predictionSample.sample.time > nextTimeToRecord:
while predictionSample.sample.time > nextTimeToRecord:
estimatedSample = self.calculateEstSample(interpolationSample, \
nextTimeToRecord)
self.reconstructedSignal.append(estimatedSample)
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
if predictionSample.sample.time == nextTimeToRecord:
estimatedSample = self.calculateEstSample(interpolationSample, \
nextTimeToRecord)
self.reconstructedSignal.append(estimatedSample)
targetSample = self.findTarget(predictionSample)
interpolationSample = self.findInterpolationSample(estimatedSample, \
targetSample)
nextTimeToRecord = nextTimeToRecord + self.samplingInterval
else:
targetSample = self.findTarget(predictionSample)
interpolationSample = self.findInterpolationSample(self.reconstructedSignal[-1], \
targetSample)
def interpolate(self, samples, currentTime):
newSample = Sample()
newSample.time = currentTime
time = [ samples[0].time, samples[1].time ]
x = [ samples[0].position.x, samples[1].position.x ]
y = [ samples[0].position.y, samples[1].position.y ]
z = [ samples[0].position.z, samples[1].position.z ]
interpolationFunction = interpolate.interp1d( time, x )
newSample.position.x = interpolationFunction(currentTime).flatten()[0]
interpolationFunction = interpolate.interp1d( time, y )
newSample.position.y = interpolationFunction(currentTime).flatten()[0]
interpolationFunction = interpolate.interp1d( time, z )
newSample.position.z = interpolationFunction(currentTime).flatten()[0]
return newSample
def interpolate(self, samples, currentTime):
newSample = Sample()
newSample.time = currentTime
time = [samples[0].time, samples[1].time]
x = [samples[0].position.x, samples[1].position.x]
y = [samples[0].position.y, samples[1].position.y]
z = [samples[0].position.z, samples[1].position.z]
interpolationFunction = interpolate.interp1d(time, x)
newSample.position.x = interpolationFunction(currentTime).flatten()[0]
interpolationFunction = interpolate.interp1d(time, y)
newSample.position.y = interpolationFunction(currentTime).flatten()[0]
interpolationFunction = interpolate.interp1d(time, z)
newSample.position.z = interpolationFunction(currentTime).flatten()[0]
return newSample
