Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 240:d2b68111f87e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added module for extrapolation
author Sarah@Sarah-PC.polymtl.ca
date Fri, 13 Jul 2012 17:08:31 -0400
parents
children e0988a8ace0c
comparison
equal deleted inserted replaced
239:93c26e45efd8 240:d2b68111f87e
1 ##########
2 # Extrapolation Hypothesis
3 ##########
4
5 import sys
6
7 sys.path.append("G:/0-phdstart/Code/traffic-intelligence1/python")
8
9 import moving
10
11 #Default values
12 FPS= 25 # No. of frame per second (FPS)
13 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec
14 deltaT= FPS*5 # extrapolatation time Horizon = 5 second
15
16 def ExtrapolationPosition (movingObject1, instant,deltaT):
17 ''' extrapolation hypothesis: constant velocity'''
18 return movingObject1.getPositionAtInstant(instant) + movingObject1.getVelocityAtInstant(instant). multiply(deltaT)
19
20 def motion (position, velocity, acceleration):
21 ''' extrapolation hypothesis: constant acceleration'''
22 from math import atan2,cos,sin
23 vInit= velocity
24 vInitial= velocity.norm2()
25 theta= atan2(velocity.y,velocity.x)
26 vFinal= vInitial+acceleration
27
28 if acceleration<= 0:
29 v= max(0,vFinal)
30 velocity= moving.Point(v* cos(theta),v* sin(theta))
31 position= position+ (velocity+vInit). multiply(0.5)
32 else:
33 v= min(vLimit,vFinal)
34 velocity= moving.Point(v* cos(theta),v* sin(theta))
35 position= position+ (velocity+vInit). multiply(0.5)
36 return(position,velocity)
37
38 def motionPET (position, velocity, acceleration, deltaT):
39 ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
40 from math import atan2,cos,sin,fabs
41 vInit= velocity
42 vInitial= velocity.norm2()
43 theta= atan2(velocity.y,velocity.x)
44 vFinal= vInitial+acceleration * deltaT
45 if acceleration< 0:
46 if vFinal> 0:
47 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
48 position= position+ (vInit+ velocity). multiply(0.5*deltaT)
49 else:
50 T= fabs(vInitial/acceleration)
51 position= position + vInit. multiply(0.5*T)
52 elif acceleration> 0 :
53 if vFinal<= vLimit:
54 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
55 position= position+ (vInit+ velocity). multiply(0.5*deltaT)
56 else:
57 time1= fabs((vLimit-vInitial)/acceleration)
58 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
59 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1))
60 elif acceleration == 0:
61 position= position + velocity. multiply(deltaT)
62
63 return position
64
65 def timePET (position, velocity, acceleration, intersectedPoint ):
66 ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
67 from math import atan2,cos,sin,fabs
68 vInit= velocity
69 vInitial= velocity.norm2()
70 theta= atan2(velocity.y,velocity.x)
71 vFinal= vInitial+acceleration * deltaT
72 if acceleration< 0:
73 if vFinal> 0:
74 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
75 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
76 else:
77 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x)))
78 elif acceleration> 0 :
79 if vFinal<= vLimit:
80 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
81 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
82 else:
83 time1= fabs((vLimit-vInitial)/acceleration)
84 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
85 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
86 if time2<=time1:
87 time= time2
88 else:
89 position2= (position+ (velocity+vInit). multiply(0.5*time1))
90 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x))
91 elif acceleration == 0:
92 time= fabs((intersectedPoint.x-position.x)/(velocity.x))
93
94 return time
95
96 def motionSteering (position, velocity, deltaTheta, deltaT ):
97 ''' extrapolation hypothesis: steering with deltaTheta'''
98 from math import atan2,cos,sin
99 vInitial= velocity.norm2()
100 theta= atan2(velocity.y,velocity.x)
101 newTheta= theta + deltaTheta
102 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
103 position= position+ (velocity). multiply(deltaT)
104 return position
105
106 def MonteCarlo(movingObject1,movingObject2, instant):
107 ''' Monte Carlo Simulation : estimate the probability of collision'''
108 from random import uniform
109 from math import pow, sqrt, sin, cos,atan2
110 N=1000
111 ProbOfCollision = 0
112 for n in range (1, N):
113 # acceleration limit
114 acc1 = uniform(-0.040444,0)
115 acc2 = uniform(-0.040444,0)
116 p1= movingObject1.getPositionAtInstant(instant)
117 p2= movingObject2.getPositionAtInstant(instant)
118 v1= movingObject1.getVelocityAtInstant(instant)
119 v2= movingObject2.getVelocityAtInstant(instant)
120 distance= (p1-p2).norm2()
121 distanceThreshold= 1.8
122 t=1
123 while distance > distanceThreshold and t <= deltaT:
124 # Extrapolation position
125 (p1,v1) = motion(p1,v1,acc1)
126 (p2,v2) = motion(p2,v2,acc2)
127 distance= (p1-p2).norm2()
128 if distance <=distanceThreshold:
129 ProbOfCollision= ProbOfCollision+1
130 t+=1
131 POC= float(ProbOfCollision)/N
132 return POC
133
134 def velocitySteering(velocity,steering):
135 from math import atan2,cos,sin
136 vInitial= velocity.norm2()
137 theta= atan2(velocity.y,velocity.x)
138 newTheta= theta + steering
139 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
140 return v
141
142 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2):
143 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering'''
144 from random import uniform
145 from math import pow, sqrt, sin, cos,atan2
146 N=1000
147 L= 2.4
148 ProbOfCollision = 0
149 for n in range (1, N):
150 # acceleration limit
151 acc1 = uniform(-0.040444,0)
152 acc2 = uniform(-0.040444,0)
153 p1= movingObject1.getPositionAtInstant(instant)
154 p2= movingObject2.getPositionAtInstant(instant)
155 vInit1= movingObject1.getVelocityAtInstant(instant)
156 v1= velocitySteering (vInit1,steering1)
157 vInit2= movingObject2.getVelocityAtInstant(instant)
158 v2= velocitySteering (vInit2,steering2)
159 distance= (p1-p2).norm2()
160 distanceThreshold= 1.8
161 t=1
162 while distance > distanceThreshold and t <= deltaT:
163 # Extrapolation position
164 (p1,v1) = motion(p1,v1,acc1)
165 (p2,v2) = motion(p2,v2,acc2)
166 distance= (p1-p2).norm2()
167 if distance <=distanceThreshold:
168 ProbOfCollision= ProbOfCollision+1
169 t+=1
170 POC= float(ProbOfCollision)/N
171 return POC
172
173