Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 250:59f547aebaac

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
modified prediction functions, added norm/angle representation of Points
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 23 Jul 2012 02:50:26 -0400
parents bd8ab323c198
children 13ec22bec5d4
comparison
equal deleted inserted replaced
249:99173da7afae 250:59f547aebaac
1 #! /usr/bin/env python 1 #! /usr/bin/env python
2 '''Library for moving object extrapolation hypotheses''' 2 '''Library for moving object extrapolation hypotheses'''
3 3
4 import moving 4 import moving
5 import math
5 6
6 class ExtrapolatedTrajectory: 7 class ExtrapolatedTrajectory:
7 '''Class for extrapolated trajectories with lazy evaluation 8 '''Class for extrapolated trajectories with lazy evaluation
8 if the predicted position has not been already computed, compute it 9 if the predicted position has not been already computed, compute it
9 10
14 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): 15 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory):
15 '''Extrapolated trajectory at constant speed or acceleration 16 '''Extrapolated trajectory at constant speed or acceleration
16 TODO add limits if acceleration 17 TODO add limits if acceleration
17 TODO generalize by passing a series of velocities/accelerations''' 18 TODO generalize by passing a series of velocities/accelerations'''
18 19
19 def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1): 20 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
20 self.initialPosition = initialPosition 21 self.control = control
21 self.initialVelocity = initialVelocity 22 self.maxSpeed = maxSpeed
22 self.initialAccleration = initialAccleration
23 self.probability = probability 23 self.probability = probability
24 self.predictedPositions = {} 24 self.predictedPositions = {0: initialPosition}
25 self.predictedVelocities = {} 25 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
26 26
27 def predictPosition(self, nTimeSteps): 27 def predictPosition(self, nTimeSteps):
28 if not nTimeSteps in self.predictedPositions.keys(): 28 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
29 self.predictedPositions[nTimeSteps] = moving.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration) 29 self.predictedPositions[nTimeSteps], self.predictedspeedOrientations[nTimeSteps] = predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedspeedOrientations[nTimeSteps-1], self.control, maxSpeed)
30 return self.predictedPositions[nTimeSteps] 30 return self.predictedPositions[nTimeSteps]
31
32 class ExtrapolatedTrajectoryAdaptation(ExtrapolatedTrajectoryConstant):
33 '''Random small adaptation of vehicle control '''
34 def __init__(self, initialPosition, initialVelocity, accelerationBounds, steeringBounds, probability = 1, maxSpeed = None):
35 self.accelerationBounds = accelerationBounds
36 self.steeringBounds = steeringBounds
37 self.maxSpeed = maxSpeed
38 self.probability = probability
39 self.predictedPositions = {0: initialPosition}
40 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
41
42 def predictPosition(self, nTimeSteps):
43 # draw acceleration and steering
44 # TODO should the predictPosition be in the base class ??
45 self.control = moving.NormAngle(0,0)
46 return ExtrapolatedTrajectoryConstant.predictPosition(self, nTimeSteps)
31 47
32 # Default values: to remove because we cannot tweak that from a script where the value may be different 48 # Default values: to remove because we cannot tweak that from a script where the value may be different
33 FPS= 25 # No. of frame per second (FPS) 49 FPS= 25 # No. of frame per second (FPS)
34 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec 50 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec
35 deltaT= FPS*5 # extrapolatation time Horizon = 5 second 51 deltaT= FPS*5 # extrapolatation time Horizon = 5 second