changeset 1100:1e833fd8490d

working version of Newell car following model
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 20 Feb 2019 16:16:19 -0500
parents 4ab5c63c13a3
children e23828659a66
files trafficintelligence/moving.py
diffstat 1 files changed, 34 insertions(+), 30 deletions(-) [+]
line wrap: on
line diff
--- a/trafficintelligence/moving.py	Wed Feb 20 00:08:20 2019 -0500
+++ b/trafficintelligence/moving.py	Wed Feb 20 16:16:19 2019 -0500
@@ -1257,46 +1257,50 @@
     def updatePositions(self):
         inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
 
+    def addNewellAttributes(self, desiredSpeed, tau, d, initialCumulatedHeadway, initialAlignmentIdx):
+        '''adds attributes necessary for Newell car following model
+        using curvilinear trajectories'''
+        # Newell model parameters
+        self.desiredSpeed = desiredSpeed
+        self.tau = tau
+        self.d = d
+        self.leader = None
+        # other attributes necessary for computation
+        self.initialCumulatedHeadway = initialCumulatedHeadway
+        self.initialAlignmentIdx = initialAlignmentIdx
+        self.timeAtS0 = None # time at which the vehicle's position is s=0 on the alignment
+
     def updateCurvilinearPositions(self, method, instant, timeStep, _nextAlignmentIdx = None, maxSpeed = None, acceleration = None):
         '''Update curvilinear position of user at new instant'''
 
         if method == 'newell':
-            if self.curvilinearPositions is None: # vehicle without positions, all vehicles should have leader (?)
-                if self.instantAtX0 is None:
+            if self.curvilinearPositions is None: # vehicle without positions
+                if self.timeAtS0 is None:
                     if self.leader is None:
-                        s = (timeStep*instant-self.initialHeadway)*self.desiredSpeed
-                        self.timeInterval = TimeInterval(instant, instant)
-                        self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx])
-                        self.curvilinearVelocities = CurvilinearTrajectory()
-                        self.instantAtX0 = self.initialHeadway# to avoid next test
+                        self.timeAtS0 = self.initialCumulatedHeadway
                     elif self.leader.curvilinearPositions is not None and self.leader.curvilinearPositions.getSCoordAt(-1) > self.d and len(self.leader.curvilinearPositions) >=2:
                         firstInstantAfterD = self.leader.getLastInstant()
                         while self.leader.existsAtInstant(firstInstantAfterD) and self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD-1)[0] > self.d:# find first instant after d
                             firstInstantAfterD -= 1 # if not recorded position before self.d, we extrapolate linearly from first known position
                         leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(firstInstantAfterD-1)[0]
-                        self.instantAtX0 = self.tau + instant*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)/leaderSpeed # second part is the time at which leader is at self.d
-                        if self.instantAtX0 < self.initialHeadway: #obj appears at instant initialHeadway at x=0 with desiredSpeed
-                            self.instantAtX0 = self.initialHeadway
-                elif instant*timeStep >= self.instantAtX0:
-                    firstInstant = int(ceil(self.instantAtX0/timeStep))
-                    self.timeInterval = TimeInterval(firstInstant, firstInstant)
-                    freeFlowCoord = (firstInstant*timeStep - self.instantAtX0)*self.desiredSpeed
-                    # constrainedCoord at firstInstant = xn-1(t = firstInstant*timeStep-self.tau)-self.d
-                    t = firstInstant*timeStep-self.tau
-                    i = int(floor(t/timeStep))
-                    leaderSpeed = self.leader.getCurvilinearVelocityAtInstant(i)[0]
-                    constrainedCoord = self.leader.getCurvilinearPositionAtInstant(i)[0]+leaderSpeed*(t-i*timeStep)-self.d
-                    self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx])
-                    self.curvilinearVelocities = CurvilinearTrajectory()
-                    for i in range(firstInstant+1, instant+1):
-                        s1 = self.curvilinearPositions.getSCoordAt(-1)
-                        freeFlowCoord = s1 + timeStep*self.desiredSpeed
-                        constrainedCoord = self.leader.interpolateCurvilinearPositions(i-self.tau/timeStep)[0]-self.d
-                        s2 = min(freeFlowCoord, constrainedCoord)
-                        self.curvilinearPositions.addPositionSYL(s2, 0., self.initialAlignmentIdx)
-                        self.setLastInstant(i)
-                        print(i, self.timeInterval)
-                        self.curvilinearVelocities.addPositionSYL(s2-s1, 0., None)
+                        self.timeAtS0 = self.tau + firstInstantAfterD*timeStep - (self.leader.getCurvilinearPositionAtInstant(firstInstantAfterD)[0]-self.d)*timeStep/leaderSpeed # second part is the time at which leader is at self.d
+                        if self.timeAtS0 < self.initialCumulatedHeadway: #obj appears at instant initialCumulatedHeadway at x=0 with desiredSpeed
+                            self.timeAtS0 = self.initialCumulatedHeadway
+                elif instant*timeStep > self.timeAtS0:
+                    #firstInstant = int(ceil(self.timeAtS0/timeStep))# this first instant is instant by definition
+                    leaderInstant = instant-self.tau/timeStep
+                    if self.leader is None:
+                        s = (timeStep*instant-self.timeAtS0)*self.desiredSpeed
+                        self.timeInterval = TimeInterval(instant, instant)
+                        self.curvilinearPositions = CurvilinearTrajectory([s], [0.], [self.initialAlignmentIdx])
+                        self.curvilinearVelocities = CurvilinearTrajectory()
+                    elif self.leader.existsAtInstant(leaderInstant):
+                        self.timeInterval = TimeInterval(instant, instant)
+                        freeFlowCoord = (instant*timeStep - self.timeAtS0)*self.desiredSpeed
+                        # constrainedCoord at instant = xn-1(t = instant*timeStep-self.tau)-self.d
+                        constrainedCoord = self.leader.interpolateCurvilinearPositions(leaderInstant)[0]-self.d
+                        self.curvilinearPositions = CurvilinearTrajectory([min(freeFlowCoord, constrainedCoord)], [0.], [self.initialAlignmentIdx])
+                        self.curvilinearVelocities = CurvilinearTrajectory()
             else:
                 if _nextAlignmentIdx is not None:
                     laneChange = (self.curvilinearPositions.getLaneAt(-1), _nextAlignmentIdx)