Eclipse SUMO - Simulation of Urban MObility
MSCFModel_EIDM.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2022 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
17 
24 // The Extended Intelligent Driver Model (EIDM) car-following model
25 //
26 // Publication: Salles, Dominik, S. Kaufmann and H. Reuss. “Extending the Intelligent Driver
27 // Model in SUMO and Verifying the Drive Off Trajectories with Aerial
28 // Measurements.” (2020).
29 /****************************************************************************/
30 
31 
32 // ===========================================================================
33 // included modules
34 // ===========================================================================
35 #include <config.h>
36 
37 #include "MSCFModel_EIDM.h"
38 #include <microsim/MSVehicle.h>
39 #include <microsim/MSLane.h>
40 #include <microsim/MSEdge.h>
41 #include <microsim/MSLink.h>
43 #include <utils/common/SUMOTime.h>
44 
45 //#define DEBUG_V
46 
47 #define EST_REAC_THRESHOLD 3. // under this threshold estimation, error and reaction time variables don't get taken into account
48 
49 // ===========================================================================
50 // method definitions
51 // ===========================================================================
53  MSCFModel(vtype), myDelta(vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_DELTA, 4.)),
54  myTwoSqrtAccelDecel(double(2 * sqrt(myAccel * myDecel))),
55  myIterations(MAX2(1, int(TS / vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_STEPPING, .25) + .5))),
56  myTPersDrive(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_DRIVE, 3)), myTreaction(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_REACTION, 0.5)),
57  myTpreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_LOOK_AHEAD, 4)), myTPersEstimate(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_ESTIMATE, 10)),
58  myCcoolness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_C_COOLNESS, 0.99)), mySigmaleader(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_LEADER, 0.02)),
59  mySigmagap(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_GAP, 0.1)), mySigmaerror(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_ERROR, 0.1)),
60  myJerkmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_JERK_MAX, 3.)), myEpsilonacc(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_EPSILON_ACC, 1.)),
61  myTaccmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_ACC_MAX, 1.2)), myMflatness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_FLATNESS, 2.)),
62  myMbegin(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_BEGIN, 0.7)), myUseVehDynamics(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_USEVEHDYNAMICS, 0))
63  //, myMaxVehPreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_MAX_VEH_PREVIEW, 0))
64 {
65  // IDM does not drive very precise and may violate minGap on occasion
67 }
68 
70 
71 double
72 MSCFModel_EIDM::insertionFollowSpeed(const MSVehicle* const /*veh*/, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
74  return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
75  } else {
76  // Not Done/checked yet for the ballistic update model!!!!
77  // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
78 // return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
79  return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
80  }
81 }
82 
83 
84 double
85 MSCFModel_EIDM::insertionStopSpeed(const MSVehicle* const /*veh*/, double speed, double gap) const {
87  return maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime);
88  } else {
89  // Not Done/checked yet for the ballistic update model!!!!
90 // return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0.), myType->getMaxSpeed());
91  return MIN2(maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime), myType->getMaxSpeed());
92  }
93 }
94 
95 double
96 MSCFModel_EIDM::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
97  gap -= NUMERICAL_EPS;
98  if (gap <= 0) {
99  return 0;
100  }
101  double a = 1.;
102  double b = myHeadwayTime * myTwoSqrtAccelDecel - predSpeed;
103  double c = -sqrt(1 + myDecel / (2 * myAccel)) * gap * myTwoSqrtAccelDecel;
104  // with myDecel/myAccel, the intended deceleration is myDecel
105  // with myDecel/(2*myAccel), the intended deceleration is myDecel/2
106  // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term v/vMax is not present
107 
108  // double c = -sqrt(1 - pow(egoSpeed / desSpeed, myDelta) + myDecel / (2 * myAccel)) * gap * myTwoSqrtAccelDecel; // c calculation when using the IDM!
109 
110  // myDecel is positive, but is intended as negative value here, therefore + instead of -
111  // quadratic formula
112  double x = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
113 
114  if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
115  double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
116  if (origSafeDecel > myDecel + NUMERICAL_EPS) {
117  // Braking harder than myDecel was requested -> calculate required emergency deceleration.
118  // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
119  // can result in corrupted values (leading to intersecting trajectories) if, e.g. leader and follower are fast (leader still faster) and the gap is very small,
120  // such that braking harder than myDecel is required.
121 
122 #ifdef DEBUG_EMERGENCYDECEL
123  if (DEBUG_COND2) {
124  std::cout << SIMTIME << " initial vsafe=" << x
125  << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
126  << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
127  << std::endl;
128  }
129 #endif
130 
131  double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
132  // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
133  safeDecel = MAX2(safeDecel, myDecel);
134  // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
135  safeDecel = MIN2(safeDecel, origSafeDecel);
136  x = egoSpeed - ACCEL2SPEED(safeDecel);
138  x = MAX2(x, 0.);
139  }
140 
141 #ifdef DEBUG_EMERGENCYDECEL
142  if (DEBUG_COND2) {
143  std::cout << " -> corrected emergency deceleration: " << safeDecel << std::endl;
144  }
145 #endif
146 
147  }
148  }
149  assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
150  assert(!ISNAN(x));
151  return x;
152 }
153 
154 double
155 MSCFModel_EIDM::maximumSafeStopSpeed(double g /*gap*/, double decel, double v /*currentSpeed*/, bool onInsertion, double headway) const {
156  double vsafe;
158  g -= NUMERICAL_EPS;
159  if (g <= 0) {
160  return 0;
161  }
162  double a = 1.;
163  double b = headway * myTwoSqrtAccelDecel - 0.;
164  double c = -sqrt(1 + decel / (2 * myAccel)) * g * myTwoSqrtAccelDecel;
165  // with decel/myAccel, the intended deceleration is decel
166  // with decel/(2*myAccel), the intended deceleration is decel/2
167  // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term v/vMax is not present
168 
169  // double c = -sqrt(1 - pow(v / desSpeed, myDelta) + decel / (2 * myAccel)) * g * myTwoSqrtAccelDecel; // c calculation when using the IDM!
170 
171  // decel is positive, but is intended as negative value here, therefore + instead of -
172  // quadratic formula
173  vsafe = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
174  } else {
175  // Not Done/checked yet for the ballistic update model!!!!
176  vsafe = maximumSafeStopSpeedBallistic(g, decel, v, onInsertion, headway);
177  }
178 
179  return vsafe;
180 }
181 
182 double
183 MSCFModel_EIDM::patchSpeedBeforeLCEIDM(const MSVehicle* /*veh*/, double vMin, double vMax, VehicleVariables* vars) const {
184  // The default value of SUMO_ATTR_JM_SIGMA_MINOR is sigma, not sigmaerror (used in EIDM),
185  // so we do not use SUMO_ATTR_JM_SIGMA_MINOR as parameter here, because this could confuse users...
186  //const double sigma = (veh->passingMinor()
187  // ? veh->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_SIGMA_MINOR, myDawdle)
188  // : myDawdle);
189 
190  // dawdling/drivingerror is now calculated here (in finalizeSpeed, not in stop-/follow-/freeSpeed anymore):
191  // Instead of just multiplying mySigmaerror with vars->myw_error, we add a factor depending on the criticality of the situation,
192  // measured with s*/gap. Because when the driver drives "freely" (nothing in front) he may dawdle more than in e.g. congested traffic!
193  double drivingerror = 0.0;
194  const double s = myType->getMinGap() + MAX2(0., vars->myv_est * myHeadwayTime + vars->myv_est * (vars->myv_est - vars->myv_est_l) / myTwoSqrtAccelDecel);
195  if (vMax > EST_REAC_THRESHOLD) {
196  if (s / vars->mys_est >= 0.5) {
197  drivingerror = mySigmaerror * vars->myw_error;
198  } else if (s / vars->mys_est < 0.1) {
199  drivingerror = mySigmaerror * vars->myw_error * 2.5;
200  } else if (s / vars->mys_est < 0.15) {
201  drivingerror = mySigmaerror * vars->myw_error * 2.1;
202  } else if (s / vars->mys_est < 0.2) {
203  drivingerror = mySigmaerror * vars->myw_error * 1.8;
204  } else if (s / vars->mys_est < 0.25) {
205  drivingerror = mySigmaerror * vars->myw_error * 1.5;
206  } else if (s / vars->mys_est < 0.3) {
207  drivingerror = mySigmaerror * vars->myw_error * 1.3;
208  } else if (s / vars->mys_est < 0.5) {
209  drivingerror = mySigmaerror * vars->myw_error * 1.1;
210  }
211  }
212 
213  //const double vDawdle = MAX2(vMin, dawdle2(vMax, sigma, veh->getRNG()));
214  const double vDawdle = MAX2(vMin, vMax + ACCEL2SPEED(drivingerror));
215  return vDawdle;
216 }
217 
218 double
219 MSCFModel_EIDM::finalizeSpeed(MSVehicle* const veh, double vPos) const {
220  // finalizeSpeed is only called once every timestep!
221 
223  // save old v for optional acceleration computation
224  const double oldV = veh->getSpeed();
225 
226  // @ToDo: Maybe change whole calculation to calculate real freeSpeed/stopSpeed/followSpeed in every call and here calculate resulting speed with reaction Time and update?!
227  // @ToDo: Could check which call resulted in speed update with stop-vector containing all calculated accelerations!
228  // Check whether the speed update results from a stop calculation, if so, run _v-function again with the saved variables from stopSpeed
229  if (!(vPos > oldV + ACCEL2SPEED(vars->realacc) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(vars->realacc) + NUMERICAL_EPS)) {
230  for (auto it = vars->stop.cbegin(); it != vars->stop.cend(); ++it) {
231  if (vPos > oldV + ACCEL2SPEED(it->first) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(it->first) + NUMERICAL_EPS) {
232  vPos = _v(veh, it->second, oldV, 0, vars->v0_int, false, 1);
233  }
234  }
235  }
236 
237  // process stops (includes update of stopping state)
238  const double vStop = MIN2(vPos, veh->processNextStop(vPos));
239  // apply deceleration bounds
240  const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
241  // vPos contains the uppper bound on safe speed. allow emergency braking here
242  const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(vPos, vMinEmergency));
243  // apply planned speed constraints and acceleration constraints
244  double vMax = MIN2(maxNextSpeed(oldV, veh), vStop);
245  vMax = MAX2(vMin, vMax);
246 
247  // apply further speed adaptations
248  double vNext = patchSpeedBeforeLCEIDM(veh, vMin, vMax, vars);
249 
251 
252  // The model does not directly use vNext from patchSpeed (like the original MSCFModel::finalizeSpeed function),
253  // but rather slowly adapts to vNext.
254  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
255 
256  // Bound the positive change of the acceleration with myJerkmax
257  if (vNext > oldV && oldV > EST_REAC_THRESHOLD) {
258  // At junctions with minor priority acceleration will still jump because after finalizeSpeed "MAX2(vNext, vSafeMin)" is called, vSafeMin is higher and vNext from finalizeSpeed is then ignored!!!
259  // If we put this jmax-Part into _v-function (via old calc_gap implementation), then vehicle can't drive over junction because it thinks it won't make it in time before a foe may appear!
260  if (myJerkmax * TS + veh->getAcceleration() < 0.) { // If driver wants to accelerate, but is still decelerating, then we use a factor of 2!
261  vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * 2 * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax*2
262  } else {
263  vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax
264  }
265  } else if (vNext <= oldV && vNext < vMax - NUMERICAL_EPS && oldV > EST_REAC_THRESHOLD) {
266  // Slowing down the deceleration like this may be critical!!! Vehicle can also not come back from Emergency braking fast enough!
267  /*if (vNext - oldV < -myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake harder than before, change in acceleration is then bounded by -myJerkmax
268  vNext = MAX2(oldV + (-myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
269  } else if (vNext - oldV > myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake less harder than before, change in acceleration is then bounded by +myJerkmax
270  vNext = MAX2(oldV + (myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
271  } else {
272  vNext = vNext; // Do nothing, as the new vNext is in the range of +/- jerk!
273  }*/
274 
275  // Workaround letting the vehicle not brake hard for Lane Change reasons (vNext), but only for safety Car following / stopping reasons (vMax)!
276  vNext = MAX2(oldV + MIN2(vMax - oldV, MAX2(vNext - oldV, (-myJerkmax * TS + veh->getAcceleration()) * TS)), 0.);
277  }
278 
279  } else {
280  // Not Done/checked yet for the ballistic update model!!!!
281 
282  // for ballistic update, negative vnext must be allowed to
283  // indicate a stop within the coming timestep (i.e., to attain negative values)
284  vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
285  // (Leo) finalizeSpeed() is responsible for assuring that the next
286  // velocity is chosen in accordance with maximal decelerations.
287  // At this point vNext may also be negative indicating a stop within next step.
288  // Moreover, because maximumSafeStopSpeed() does not consider deceleration bounds
289  // vNext can be a large negative value at this point. We cap vNext here.
290  vNext = MAX2(vNext, vMin);
291  }
292 
293  // Update the desired speed
294  internalspeedlimit(veh, oldV);
295 
296  if (vNext > EST_REAC_THRESHOLD) { // update the Wiener-Prozess variables
297  vars->myw_gap = pow(2.7183, -TS / myTPersEstimate) * vars->myw_gap + sqrt(2 * TS / myTPersEstimate) * RandHelper::randNorm(0, 0.5); // variance of 1 can create very high values and may be too high!
298  vars->myw_speed = pow(2.7183, -TS / myTPersEstimate) * vars->myw_speed + sqrt(2 * TS / myTPersEstimate) * RandHelper::randNorm(0, 0.5); // variance of 1 can create very high values and may be too high!
299  vars->myw_error = pow(2.7183, -TS / myTPersDrive) * vars->myw_error + sqrt(2 * TS / myTPersDrive) * RandHelper::randNorm(0, 1);
300  } // else all those w_... are zero by default
301 
302  // Update the Action-point reaction time
303  if (vars->myap_update == 0) { // Update all saved acceleration variables for further calculcation between two action points
304  vars->lastacc = vars->minaccel;
305  vars->wouldacc = vars->minaccel;
306  vars->lastrealacc = vars->realacc;
307  vars->lastleaderacc = vars->realleaderacc;
308  }
309 
310  // Set myap_update back to 0 when maximal reaction time is reached,
311  // else add 1 for the next time step
312  if (vars->myap_update == int(myTreaction / TS - 1) || vars->myap_update > int(myTreaction / TS - 1)) {
313  vars->myap_update = 0;
314  } else {
315  vars->myap_update = vars->myap_update + 1;
316  }
317 
318  // Here the acceleration the vehicle would adapt to without a reaction time is compared to the last acceleration update at the last action point.
319  // If between two action points the vehicle would like to brake harder than -myEpsilonacc, then an action point is called for the next time step (myap_update = 0).
320  // This update is only used when wouldacc becomes myEpsilonacc lower than lastacc! When accelerating (wouldacc > lastacc), always the maximal reaction time is used!
321  // @ToDo: Check how to use a stable reaction time below EST_REAC_THRESHOLD m/s when braking without oscillating acceleration, then this boundary could be eliminated.
322  // @ToDo: Use this asynchron action point update also for accelerating (like introduced by Wagner/Hoogendorn/Treiber), not only for keeping the CF-model stable!
323  if ((vars->wouldacc - vars->lastacc) < -myEpsilonacc || (oldV < EST_REAC_THRESHOLD && vNext < oldV)) { // when this holds, then the driver should react immediately!
324  vars->myap_update = 0;
325  }
326 
327  // Set the "inner" variables of the car-following model back to the default values for the next time step
328  vars->minaccel = 100;
329  vars->realacc = 100;
330  vars->realleaderacc = 100;
331 
332  vars->stop.clear();
333 
334  return vNext;
335 }
336 
337 
338 double
339 MSCFModel_EIDM::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double /*predMaxDecel*/, const MSVehicle* const /*pred*/) const {
340 // applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
341 #ifdef DEBUG_V
342  gDebugFlag1 = veh->isSelected();
343 #endif
345 
346  // This update-variable is used to check what called followSpeed (LC- or CF-model)
347  // Here we don't directly use gComputeLC, which is also 0 and 1, because in freeSpeed we have another call (update = 2),
348  // which is needed to differentiate between the different cases/calculations/needed output/saved variables
349  int update;
350  if (MSGlobals::gComputeLC) {
351  update = 0;
352  } else {
353  update = 1;
354  }
355 
356  double result = _v(veh, gap2pred, speed, predSpeed, vars->v0_int, true, update);
357  return result;
358 }
359 
360 
361 double
362 MSCFModel_EIDM::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double /*decel*/) const {
363 // applyHeadwayPerceptionError(veh, speed, gap);
364 // if (gap < 0.01) {
365 // return 0;
366 // }
368 
369  double result = _v(veh, gap, speed, 0, vars->v0_int, false, 0);
370 // From Sumo_IDM-implementation:
371 // if (gap > 0 && speed < NUMERICAL_EPS && result < NUMERICAL_EPS) {
372 // // ensure that stops can be reached:
373 // result = maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs());
374 // }
375  return result;
376 }
377 
378 double
379 MSCFModel_EIDM::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion) {
380  // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
381  // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
382  // changes to a road with a lower speed limit).
383 
385  // adapt speed to succeeding lane, no reaction time is involved
386  // when breaking for y steps the following distance g is covered
387  // (drive with v in the final step)
388  // g = (y^2 + y) * 0.5 * b + y * v
389  // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
390  const double v = SPEED2DIST(targetSpeed);
391  if (dist < v) {
392  return targetSpeed;
393  }
394  const double b = ACCEL2DIST(decel);
395  const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
396  const double yFull = floor(y);
397  const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
398  const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
399  return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
400  } else {
401  // ballistic update (Leo)
402  // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
403  // and given a maximal deceleration b=decel, denote the current speed by v0.
404  // the distance covered by a trajectory that attains vN in the next timestep and decelerates afterwards
405  // with b is given as
406  // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
407  // where time t of arrival at d with speed vT is
408  // t = dt + (vN-vT)/b. (2)
409  // We insert (2) into (1) to obtain
410  // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
411  // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
412  // and solve for vN
413 
414  assert(currentSpeed >= 0);
415  assert(targetSpeed >= 0);
416 
417  const double dt = onInsertion ? 0 : TS; // handles case that vehicle is inserted just now (at the end of move)
418  const double v0 = currentSpeed;
419  const double vT = targetSpeed;
420  const double b = decel;
421  const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
422 
423  // Solvability for positive vN (if d is small relative to v0):
424  // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
425  // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
426  // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
427  // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
428  // (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
429 
430  if (0.5 * (v0 + vT)*dt >= d) {
431  return vT; // (#)
432  }
433 
434  const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
435  const double p = 0.5 * b * dt;
436  return -p + sqrt(p * p - q);
437  }
438 }
439 
440 double
441 MSCFModel_EIDM::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion) const {
442 
443  // @ToDo: Set new internal speed limit/desired speed <maxSpeed> here and change it over time in internalspeedlimit()!
444 
445  if (maxSpeed < 0.) {
446  // can occur for ballistic update (in context of driving at red light)
447  return maxSpeed;
448  }
449 
450  // This update-variable is used to check what called freeSpeed (LC- or CF-model)
451  // Here we don't directly use gComputeLC, which is also 0 and 1, because in we have another possible call (update = 2),
452  // which is needed to differentiate between the different cases/calculations/needed output/saved variables
453  int update;
454  if (MSGlobals::gComputeLC) {
455  update = 0;
456  } else {
457  update = 1;
458  }
459 
461 
462  double vSafe, remaining_time, targetDecel;
463  double secGap;
464  if (onInsertion) {
465  // Needed for the Insertion-Calculation to check, if insertion at this "speed" is possible to reach "maxSpeed" in the given distance "seen" (vehicle can max decelerate with myDecel!)!
466  // @ToDo: Could maybe be changed to maximumSafeFollowSpeed instead of freeSpeed-Krauss calculation!
467  vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion);
468  } else {
469  // driver needs to brake, because he is faster than the desired speed limit <maxSpeed> on the next lane or the next upcoming event (e.g. red light violation)
470  // The adaption/braking starts when the <seen> time-distance is lower than myTpreview+myTreaction
471  update = update == 0 ? 0 : 2;
472  if (maxSpeed < speed && seen < speed * (myTpreview + myTreaction)) {
473 
474  remaining_time = MAX3((seen - speed * myTreaction) / speed, myTreaction / 2, TS); // The remaining time is at least a time step or the reaction time of the driver
475  targetDecel = (speed - maxSpeed) / remaining_time; // The needed constant deceleration to reach maxSpeed before reaching the next lane/event
476 
477  // targetDecel is not set immediatly, if the vehicle is far enough away from the event (bounded by myJerkmax)
478  if (remaining_time > myTpreview - targetDecel / myJerkmax) {
479  targetDecel = (myTpreview - remaining_time) * myJerkmax;
480  }
481 
482  // calculate distance which would result in the accel-value targetDecel at this <speed> and leaderspeed <0>
483  if (vars->myap_update == 0 || update == 0) { // at action point update
484  secGap = internalsecuregap(veh, speed, 0., targetDecel);
485  } else { // between two action points
486  secGap = internalsecuregap(veh, vars->myv_est + vars->lastrealacc * vars->myap_update * TS, 0., targetDecel);
487  }
488 
489  vSafe = _v(veh, MAX2(seen, secGap), speed, 0., vars->v0_int, true, update);
490 
491  // Add this for "old implementation" when vehicle doesn't HAVE to reach maxspeed at seen-distance!
492  // @ToDo: See #7644: <double v = MIN2(maxV, laneMaxV);> in MSVehicle::planMoveInternal! -> DONE!
493  // But still: Is it better, if the vehicle brakes early enough to reach the next lane with its speed limit?
494  // Instead of driving too fast for a while on the new lane, which can be more "human", but leads to other problems (junction model, traffic light braking...)
495  /* if (seen < speed*myTpreview || seen < veh->getLane()->getVehicleMaxSpeed(veh)*myTpreview / 2) {
496  remaining_time = speed < veh->getLane()->getVehicleMaxSpeed(veh) / 2 ? seen / (veh->getLane()->getVehicleMaxSpeed(veh) / 2) : seen / MAX2(speed, 0.01);
497  if (vars->v0_int > maxSpeed + NUMERICAL_EPS && vars->v0_old > vars->v0_int + NUMERICAL_EPS) {
498  maxSpeed = MAX2(maxSpeed, MIN2(vars->v0_int, vars->v0_old - (vars->v0_old - maxSpeed) / myTpreview * (myTpreview - remaining_time)));
499  vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update);
500  } else if (vars->v0_int > maxSpeed + NUMERICAL_EPS) {
501  maxSpeed = MAX2(maxSpeed, vars->v0_int - (vars->v0_int - maxSpeed) / myTpreview * (myTpreview - remaining_time));
502  vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update);
503  } else {
504  vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update);
505  }
506  */
507  } else { // when the <speed> is lower than <maxSpeed> or the next lane/event is not seen with myTpreview+myTreaction yet
508  vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update);
509  }
510  }
511 
512  return vSafe;
513 }
514 
516 double
517 MSCFModel_EIDM::interactionGap(const MSVehicle* const veh, double vL) const {
518  // Resolve the IDM equation to gap. Assume predecessor has
519  // speed != 0 and that vsafe will be the current speed plus acceleration,
520  // i.e that with this gap there will be no interaction.
521  const double acc = myAccel * (1. - pow(veh->getSpeed() / veh->getLane()->getVehicleMaxSpeed(veh), myDelta));
522  const double vNext = veh->getSpeed() + acc;
523  const double gap = (vNext - vL) * (veh->getSpeed() + vL) / (2 * myDecel) + vL;
524 
525  // Don't allow timeHeadWay < deltaT situations.
526  return MAX2(gap, SPEED2DIST(vNext));
527 
528  // Only needed for old Lane Change Model????
529 }
530 
531 double
532 MSCFModel_EIDM::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /*leaderMaxDecel*/) const {
533  // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
534  //VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
535  const double delta_v = speed - leaderSpeed;
536  double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / myTwoSqrtAccelDecel); // is calculated without MinGap because it is compared to a gap without MinGap!
537  // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
538  // For the IIDM: Left out the case check for estSpeed > v0, assuming this is not needed here. The vehicle therefore may brake harder when newSpeed > v0 occurs!
539  // The secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
540 
541  double erg = sqrt((s * s) / (myDecel / myAccel + 1.0));
542  return MIN2(s, erg);
543 }
544 
545 // Only needed when vehicle has to reach laneMaxV before entering the new lane, see #7644
546 double
547 MSCFModel_EIDM::internalsecuregap(const MSVehicle* const veh, const double speed, const double leaderSpeed, const double targetDecel) const {
548  // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
549  // internalsecuregap uses a targetDecel instead of myDecel!
551  const double delta_v = speed - leaderSpeed;
552  double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / myTwoSqrtAccelDecel); // is calculated without MinGap because it is compared to a gap without MinGap!
553  // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
554  // the secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
555 
556  double erg;
557  if (speed <= vars->v0_int) {
558  erg = sqrt((s * s) / (MAX2(targetDecel / myAccel + 1.0, 1.0)));
559  } else { // speed > v0
560  double a_free = - myDecel * (1.0 - pow(vars->v0_int / speed, myAccel * myDelta / myDecel));
561  erg = sqrt((s * s) / (MAX2(targetDecel / myAccel + 1.0 + a_free / myAccel, 1.0)));
562  }
563 
564  return erg;
565 }
566 
567 void
568 MSCFModel_EIDM::internalspeedlimit(MSVehicle* const veh, const double oldV) const {
569 
571 
572  double v_limcurr, v_limprev;
573  v_limcurr = vars->v0_int; // model internal desired speed limit
574  v_limprev = vars->v0_old; // previous desired speed limit for calculation reasons
575 
576  const MSLane* lane = veh->getLane();
577  const std::vector<MSLane*>& bestLaneConts = veh->getBestLanesContinuation();
578  int view = 1;
579  std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
580  double seen = lane->getLength() - veh->getPositionOnLane();
581  double v0 = lane->getVehicleMaxSpeed(veh); // current desired lane speed
582 
583  // @ToDo: nextTurn is only defined in sublane-model calculation?!
584  // @ToDo: So cannot use it yet, but the next turn or speed recommendation for the street curvature (e.g. vmax=sqrt(a_transverse*Radius), a_transverse=3-4m/s^2)
585  // @ToDo: should not come from here, but from MSVehicle/the street network
586  // const std::pair<double, LinkDirection> nextTurn = veh->getNextTurn();
587 
588  // When driving on the last lane/link, the vehicle shouldn't adapt to the lane after anymore.
589  // Otherwise we check the <seen> time-distance and whether is lower than myTpreview
590  if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2)) {
591  double speedlim = 200;
592  while (true) { // loop over all upcoming edges/lanes/links until the <seen> time-distance is higher than myTpreview
593  if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2)) { // @ToDo: add && (*link)->havePriority()???
594  // @ToDo: When vehicles already brake because of a minor link, it may not be necessary to adapt the internal desired speed when turning...
595  // @ToDo: The vehicle brakes anyway and it may happen, that is brakes too hard because of the low internal desired speed and takes too long
596  // @ToDo: to accelerate again, because the internal desired speed must rise again?!
597  // @ToDo: It can't be done via (*link)->havePriority() (vehicle will not brake for other vehicles, so it needs to brake for curve radius),
598  // @ToDo: because then turning at traffic lights or somewhere else might be missing (traffic light links don't have priority definition?!)
599  LinkDirection dir = (*link)->getDirection();
600  switch (dir) {
602  break;
604  break;
605  case LinkDirection::TURN:
606  speedlim = 4;
607  break;
609  speedlim = 4;
610  break;
611  case LinkDirection::LEFT:
612  speedlim = 8;
613  break;
615  speedlim = 6;
616  break;
618  speedlim = 12;
619  break;
621  speedlim = 12;
622  break;
623  }
624 
625  if (v0 > speedlim * veh->getChosenSpeedFactor() + NUMERICAL_EPS) {
626  v0 = speedlim * veh->getChosenSpeedFactor();
627  }
628  } else {
629  break;
630  }
631  if ((*link)->getViaLane() == nullptr) {
632  ++view;
633  }
634  lane = (*link)->getViaLaneOrLane();
635  // @ToDo: Previously: (seen < oldV*myTpreview / 2 || seen < v0*myTpreview / 4)! Changed freeSpeed, so also changed v0-calculation here.
636  // @ToDo: Vehicle now decelerates to new Speedlimit before reaching new edge (not /2 anymore)!
637  // @ToDo: v0 for changing speed limits when seen < oldV*myTpreview, not seen < oldV*myTpreview/2 anymore!!!
638  if (v0 > lane->getVehicleMaxSpeed(veh)) {
639  v0 = lane->getVehicleMaxSpeed(veh);
640  }
641  seen += lane->getLength();
642  link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
643  }
644 
645  if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // if v_limprev!=v0, then the upcoming v0 is different, than the old desired v_limprev and therefore v0_int must change slowly to the new v0
646  (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) { // When v_limprev==v0, but v_limprev!=v_limcurr, then we may have a special case and need to slowly change v_limcurr to v0
647 
648  if ((v_limcurr < v_limprev && v_limcurr < v0 && v_limprev > v0) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
649  (v_limcurr > v_limprev && v_limcurr > v0 && v_limprev < v0)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
650  vars->v0_old = v_limcurr;
651  } else {
652  if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
653  v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
654  } else { // v_limcurr is too low and needs to increase
655  v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
656  }
657  }
658 
659  // when v_limcurr reaches v0, then update v_limprev=v0
660  if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
661  vars->v0_old = v0;
662  vars->v0_int = v0;
663  } else { // else just update the internal desired speed with v_limcurr
664  vars->v0_int = v_limcurr;
665  }
666  }
667 
668  } else if (!(v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // if v_limprev!=v0, then the upcoming v0 is different, than the old desired v_limprev and therefore v0_int must change slowly to the new v0
669  (v_limprev < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS && !(v_limprev < v_limcurr + NUMERICAL_EPS && v_limprev > v_limcurr - NUMERICAL_EPS))) { // When v_limprev==v0, but v_limprev!=v_limcurr, then we may have a special case and need to slowly change v_limcurr to v0
670 
671  if ((v_limcurr < v_limprev && v_limcurr < v0 && v_limprev > v0) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
672  (v_limcurr > v_limprev && v_limcurr > v0 && v_limprev < v0)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
673  vars->v0_old = v_limcurr;
674  } else {
675  if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
676  v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
677  } else { // v_limcurr is too low and needs to increase
678  v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
679  }
680  }
681 
682  // when v_limcurr reaches v0, then update v_limprev=v0
683  if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
684  vars->v0_old = v0;
685  vars->v0_int = v0;
686  } else { // else just update the internal desired speed with v_limcurr
687  vars->v0_int = v_limcurr;
688  }
689  }
690 }
691 
692 double
693 MSCFModel_EIDM::_v(const MSVehicle* const veh, const double gap2pred, const double egoSpeed,
694  const double predSpeed, const double desSpeed, const bool respectMinGap, const int update) const {
695 
696  double v0 = desSpeed;
698 
699  // @ToDo: Where to put such an insertion function/update, which only needs to be calculated once at the first step?????!
700  // For the first iteration
701  if (vars->v0_old == 0) {
702  vars = (VehicleVariables*)veh->getCarFollowVariables();
703  vars->v0_old = veh->getLane()->getVehicleMaxSpeed(veh);
704  vars->v0_int = veh->getLane()->getVehicleMaxSpeed(veh);
705  v0 = veh->getLane()->getVehicleMaxSpeed(veh);
706  }
707 
708  double wantedacc = 0., a_free;
709  double wouldacc = 0., woulds, woulda_free;
710 
711  double estSpeed, estleaderSpeed, estGap;
712  double current_estSpeed, current_estGap, current_estleaderSpeed;
713  double current_gap;
714  double acc = 0.;
715  double a_leader = 0.01; // Default without a leader, should not be 0!
716  double newSpeed = egoSpeed;
717 
718  // When doing the Follow-Calculation in adapttoLeader (MSVehicle.cpp) the mingap gets subtracted from the current gap (maybe this is needed for the Krauss-Model!).
719  // For the IDM this Mingap is needed or else the vehicle will stop at two times mingap behind the leader!
720  if (respectMinGap) {
721  current_gap = gap2pred + MAX2(0., myType->getMinGap() - 0.25); // 0.25m tolerance because of reaction time and estimated variables
722  } else {
723  // @ToDo: Possible to reduce this tolerance and use the implementation done hereafter??? (else if (!respectMinGap && current_gap < myType->getMinGap())
724  // 0.10m tolerance or else the vehicle might drive onto the junction or over a light signal with estimated variables,
725  // because gap2pred may go to 0 when offset is reached and (e.g. 1m Offset -> gap2pred=0, when vehicle stands at 0.95m, gap2pred is still 0 and does not become -0.05m (negative)!)
726 
727  // If we don't add the Mingap, then gap and s* goes to 0 and may oscillate there because of estimated variables and minimal differences!
728  current_gap = gap2pred + MAX2(0., myType->getMinGap() - 0.05);
729  if (current_gap < myType->getMinGap() - 0.04) {
730  current_gap = gap2pred + MAX2(0., myType->getMinGap() - 0.25); // make sure the vehicle stops
731  }
732  }
733 
734  if (current_gap <= NUMERICAL_EPS) {
735  return 0.;
736  }
737 
738  double newGap = current_gap;
739 
740  for (int i = 0; i < myIterations; i++) {
741 
742  // Using Action-Point reaction time: update the variables, when myap_update is zero and update is 1
743  current_estSpeed = newSpeed;
744  current_estleaderSpeed = MAX2(predSpeed - newGap * mySigmaleader * vars->myw_speed, 0.0); // estimated variable with Wiener Prozess
745  if (update == 2) { // For freeSpeed
746  current_estGap = newGap; // not-estimated variable
747  } else if (!respectMinGap && newGap < myType->getMinGap()) { // differentiate between the cases when the gap is very small and else
748  current_estGap = newGap / myType->getMinGap() * newGap * pow(2.7183, mySigmagap * vars->myw_gap); // estimated variable with Wiener Prozess
749  } else {
750  current_estGap = newGap * pow(2.7183, mySigmagap * vars->myw_gap); // estimated variable with Wiener Prozess
751  }
752 
753  if (vars->myap_update == 0 && update != 0) { // update variables with current observation
754  estSpeed = current_estSpeed;
755  estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
756  estGap = current_estGap; // estimated variable with Wiener Prozess
757  } else if (update != 0) { // use stored variables (reaction time)
758  estSpeed = MAX2(vars->myv_est + vars->lastrealacc * vars->myap_update * TS, 0.0);
759  // estSpeed = vars->myv_est;
760  estleaderSpeed = MAX2(vars->myv_est_l + vars->lastleaderacc * vars->myap_update * TS - vars->mys_est * mySigmaleader * vars->myw_speed, 0.0);
761  // estleaderSpeed = MAX2(vars->myv_est_l - vars->mys_est * mySigmaleader*vars->myw_speed, 0.0);
762  if (update == 2) { // For freeSpeed
763  estGap = newGap; // not-estimated variable
764  } else if (!respectMinGap && vars->mys_est < myType->getMinGap()) { // differentiate between the cases when the gap is very small and else
765  estGap = vars->mys_est / myType->getMinGap() * vars->mys_est * pow(2.7183, mySigmagap * vars->myw_gap) - (vars->myv_est - vars->myv_est_l) * vars->myap_update * TS; // estimated variable with Wiener Prozess
766  } else {
767  estGap = vars->mys_est * pow(2.7183, mySigmagap * vars->myw_gap) - (vars->myv_est - vars->myv_est_l) * vars->myap_update * TS; // estimated variable with Wiener Prozess
768  }
769  } else { // use actual variables without reaction time
770  estSpeed = current_estSpeed;
771  estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
772  estGap = current_estGap; // estimated variable with Wiener Prozess
773  }
774 
775  // ToDo: The headway can change for IDMM based on the scenario, should something like that also be integrated here???
776  double headwayTime = myHeadwayTime;
777  double korrektor = 0.5;
778  MSVehicle* leader;
779 
780  // @ToDo: Add this part??? driver would always know, that junctions, traffic lights, etc. have v=0!
781  // @ToDo: With estimated variables predSpeed > 0 is possible!
782  // if (!respectMinGap) {
783  // estleaderSpeed = predSpeed;
784  // }
785 
786  // @ToDo: Original IDM implementation was: When behind a vehicle (followSpeed) use MinGap and when at a junction then not (stopSpeed).
787  // @ToDo: That way for stopSpeed MinGap wasn't added to s* and also not to gap2pred!
788  double s = MAX2(0., estSpeed * headwayTime + estSpeed * (estSpeed - estleaderSpeed) / myTwoSqrtAccelDecel);
789  s += myType->getMinGap();
790 
791  // Because of the reaction time and estimated variables, s* can become lower than gap when the vehicle needs to brake/is braking, that results in the vehicle accelerating again...
792  // Here: When the gap is very small, s* is influenced to then always be bigger than the gap. With this there are no oscillations in accel at small gaps!
793  // @ToDo: Is this still needed??? Can this be changed???
794  if (respectMinGap) {
795  leader = (MSVehicle*)veh->getLeader(100).first;
796  if (current_estSpeed < 2 && leader != nullptr && (vars->myap_update == 0 || vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - TS * (myIterations - i - 1) / myIterations))) {
797  if (leader->getAcceleration() > 0.5) {
798  korrektor = 0.3; // I use this when I break AND when driving off!!! Maybe change this for reaction time drive off???
799  }
800  }
801  if (estGap < myType->getMinGap() + korrektor && s < estGap && predSpeed < veh->getLane()->getVehicleMaxSpeed(veh) - 1) {
802  s = estGap + 0.05;
803  }
804  } else {
805  if (estGap < myType->getMinGap() && s < estGap) {
806  s = estGap + 0.05;
807  }
808  }
809 
810  double a_corr = 1; // Variable for correction term
811  // initialised here with 1., because wantedacc and wouldacc is used and calculation without the correction term (is added/multiplied later)
812 
813  // IDM calculation:
814  // wantedacc = a_corr*myAccel * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (estGap * estGap));
815 
816  // IIDM calculation -NOT- from the original Treiber/Kesting publication:
817  // With the saved variables from the last Action Point
818  /*double wantedacc;
819  double a_free = a_corr*myAccel * (1. - pow(estSpeed / v0, myDelta));
820  if (s >= estGap) { // This is the IIDM
821  wantedacc = a_corr*myAccel * (1. - (s * s) / (estGap * estGap));
822  } else {
823  wantedacc = a_free * (1. - pow(s / estGap, 2*a_corr*myAccel / fabs(a_free)));
824  }*/ // Old calculation form without the distinction between v > v0 and v <= v0!!! Published it in the EIDM with this form, but may be worse than original IIDM!
825 
826  // IIDM calculation from the original Treiber/Kesting publication:
827  // With the saved variables from the last Action Point
828  if (estSpeed <= v0) {
829  a_free = a_corr * myAccel * (1. - pow(estSpeed / v0, myDelta));
830  if (s >= estGap) {
831  wantedacc = a_corr * myAccel * (1. - (s * s) / (estGap * estGap));
832  } else {
833  wantedacc = a_free * (1. - pow(s / estGap, 2 * a_corr * myAccel / a_free));
834  }
835  } else { // estSpeed > v0
836  a_free = - a_corr * myDecel * (1. - pow(v0 / estSpeed, myAccel * myDelta / myDecel));
837  if (s >= estGap) {
838  wantedacc = a_free + a_corr * myAccel * (1. - (s * s) / (estGap * estGap));
839  } else {
840  wantedacc = a_free;
841  }
842  }
843 
844  // IIDM calculation from the original Treiber/Kesting publication:
845  // With the current variables (what would the acceleration be without reaction time)
846  if (update != 0) {
847  woulds = MAX2(0., current_estSpeed * headwayTime + current_estSpeed * (current_estSpeed - current_estleaderSpeed) / myTwoSqrtAccelDecel); // s_soll
848  woulds += myType->getMinGap(); // when behind a vehicle use MinGap and when at a junction then not????
849 
850  if (current_estSpeed <= v0) {
851  woulda_free = a_corr * myAccel * (1. - pow(current_estSpeed / v0, myDelta));
852  if (woulds >= current_estGap) {
853  wouldacc = a_corr * myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
854  } else {
855  wouldacc = woulda_free * (1. - pow(woulds / current_estGap, 2 * a_corr * myAccel / woulda_free));
856  }
857  } else { // current_estSpeed > v0
858  woulda_free = - a_corr * myDecel * (1. - pow(v0 / current_estSpeed, myAccel * myDelta / myDecel));
859  if (woulds >= current_estGap) {
860  wouldacc = woulda_free + a_corr * myAccel * (1. - (woulds * woulds) / (current_estGap * current_estGap));
861  } else {
862  wouldacc = woulda_free;
863  }
864  }
865  }
866 
867  // When a vehicle is standing, multiple calls to followSpeed/stopSpeed/freeSpeed during one time step can result in different cases:
868  // e.g. first case: No vehicle in front, so ego-vehicle should drive off (followSpeed), but second case: vehicle is standing at a red traffic light, so can't drive off!
869  // When a previous call resulted in the activation of the drive off term, but the current call does not, the drive off activation is reset!
870  if ((estSpeed < 2 || vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - TS * (myIterations - i - 1) / myIterations)) && vars->minaccel > wantedacc - NUMERICAL_EPS && vars->t_off == (SIMTIME - TS * (myIterations - i - 1) / myIterations) && wantedacc <= 0 && update != 0) {
871  vars->t_off = SIMTIME - 10.;
872  }
873 
874  // Drive Off Activation and Term
875  if (estSpeed < 2 || vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - TS * (myIterations - i - 1) / myIterations)) {
876  // @ToDo: Check if all clauses are still needed or if we need to add more for all possible drive off cases?!
877  // Activation of the Drive Off term a_corr, when
878  if (vars->minaccel > wantedacc - NUMERICAL_EPS && estSpeed < 2 && // the call to _v was identified as the resulting acceleration update AND the start speed is lower than 2m/s
879  vars->t_off + 4. - NUMERICAL_EPS < (SIMTIME - TS * (myIterations - i - 1) / myIterations) && vars->myap_update == 0 && // the last activation is at least 4 seconds ago AND an Action Point was reached
880  ((estleaderSpeed > 5 && veh->getAcceleration() >= -NUMERICAL_EPS) || (estGap > myType->getMinGap() && s < estGap)) // a leader is accelerating and at least 5m/s fast (is needed for junctions) OR the current estGap is higher than s*
881  && veh->getAcceleration() < 0.2 && update != 0) { // && respectMinGap) { // the driver hasn't started accelerating yet (<0.2)
882  vars->t_off = (SIMTIME - TS * (myIterations - i - 1) / myIterations); // activate the drive off term
883  }
884  // Calculation of the Drive Off term a_corr
885  if (s < estGap && vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - TS * (myIterations - i - 1) / myIterations)) {
886  a_corr = (tanh((((SIMTIME - TS * (myIterations - i - 1) / myIterations) - vars->t_off) * 2 / myTaccmax - myMbegin) * myMflatness) + 1) / 2;
887  }
888  }
889 
890  // @ToDo: calc_gap is just estGap here, used to have an extra calc_gap calculation (like jmax), but doesn't work well here with the junction calculation:
891  // @ToDo: The driver would slowly start accelerating when he thinks the junction is clear, but may still decelerate for a bit and not jump to acceleration.
892  // @ToDo: This causes the driver not to drive over the junction because he thinks he won't make it in time before a foe may appear!
893 
894  // IIDM calculation -NOT- from the original Treiber/Kesting publication:
895  // Resulting acceleration also with the correct drive off term.
896  double calc_gap = estGap;
897  /*a_free = a_corr*myAccel * (1. - pow(estSpeed / v0, myDelta));
898  if (s >= calc_gap) { // This is the IIDM
899  acc = a_corr*myAccel * (1. - (s * s) / (calc_gap * calc_gap));
900  } else {
901  acc = a_free * (1. - pow(s / calc_gap, 2*a_corr*myAccel / fabs(a_free)));
902  } */ // Old calculation form without the distinction between v > v0 and v <= v0!!! Published it in the EIDM with this form, but may be worse than original IIDM!
903 
904  // IDM calculation:
905  // acc = a_corr*myAccel * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (calc_gap * calc_gap));
906 
907  // IIDM calculation from the original Treiber/Kesting publication:
908  // Resulting acceleration also with the correct drive off term.
909  if (estSpeed <= v0) {
910  a_free = a_corr * myAccel * (1. - pow(estSpeed / v0, myDelta));
911  if (s >= calc_gap) {
912  acc = a_corr * myAccel * (1. - (s * s) / (calc_gap * calc_gap));
913  } else {
914  acc = a_free * (1. - pow(s / calc_gap, 2 * a_corr * myAccel / a_free));
915  }
916  } else { // estSpeed > v0
917  a_free = - a_corr * myDecel * (1. - pow(v0 / estSpeed, myAccel * myDelta / myDecel));
918  if (s >= calc_gap) {
919  acc = a_free + a_corr * myAccel * (1. - (s * s) / (calc_gap * calc_gap));
920  } else {
921  acc = a_free;
922  }
923  }
924 
925  double a_cah;
926  // Coolness from Enhanced Intelligent Driver Model, when gap "jump" to a smaller gap accurs
927  // @ToDo: Maybe without update != 0??? To let all calculations profit from Coolness??? (e.g. also lane change calculation)
928  if (vars->minaccel > wantedacc - NUMERICAL_EPS && update != 0) {
929 
930  leader = (MSVehicle*)veh->getLeader(estGap + 25).first;
931  if (leader != nullptr && respectMinGap && estleaderSpeed >= 0.01) {
932  a_leader = MIN2(leader->getAcceleration(), myAccel);
933  // Change a_leader to lower values when far away from leader or else far away leaders influence the ego-vehicle!
934  if (estGap > s * 3 / 2) { // maybe estGap > 2*s???
935  a_leader = a_leader * (s * 3 / 2) / estGap;
936  }
937  }
938 
939  // speed of the leader shouldnt become zero, because then problems with the calculation occur
940  if (estleaderSpeed < 0.01) {
941  estleaderSpeed = 0.01;
942  }
943 
944  if (vars->t_off + myTaccmax + NUMERICAL_EPS < (SIMTIME - TS * (myIterations - i - 1) / myIterations) && estSpeed > 0.1) {
945 
946  // Enhanced Intelligent Driver Model
947  if (estleaderSpeed * (estSpeed - estleaderSpeed) <= -2 * estGap * a_leader) {
948  a_cah = (estSpeed * estSpeed * a_leader) / (estleaderSpeed * estleaderSpeed - 2 * estGap * a_leader);
949  } else {
950  if (estSpeed - estleaderSpeed >= 0) {
951  a_cah = a_leader - ((estSpeed - estleaderSpeed) * (estSpeed - estleaderSpeed)) / (2 * estGap);
952  } else {
953  a_cah = a_leader;
954  }
955  }
956 
957  if (acc >= a_cah) {
958  // do nothing, meaning acc = acc_IDM;
959  } else {
960  acc = (1 - myCcoolness) * acc + myCcoolness * (a_cah + myDecel * tanh((acc - a_cah) / myDecel));
961  }
962  }
963  }
964 
965  newSpeed = MAX2(0.0, current_estSpeed + ACCEL2SPEED(acc) / myIterations);
966 
967  newGap -= MAX2(0., SPEED2DIST(newSpeed - predSpeed) / myIterations);
968 
969  }
970 
971  // The "real" acceleration after iterations
972  acc = SPEED2ACCEL(MIN2(newSpeed, maxNextSpeed(egoSpeed, veh)) - veh->getSpeed());
973 
974  // wantedacc is already calculated at this point. acc may still change (because of coolness and drive off), but the ratio should stay the same!
975  // this means when vars->minaccel > wantedacc stands, so should vars->minaccel > acc!
976  // When updating at an Action Point, store the observed variables for the next time steps until the next Action Point.
977  if (vars->minaccel > wantedacc - NUMERICAL_EPS && vars->myap_update == 0 && update != 0) {
978  vars->myv_est_l = predSpeed;
979  vars->myv_est = egoSpeed;
980  if (update == 2) { // For freeSpeed
981  vars->mys_est = current_gap + myTreaction * egoSpeed;
982  } else {
983  vars->mys_est = current_gap;
984  }
985  }
986 
987  if (update != 0 && vars->wouldacc > wouldacc) {
988  vars->wouldacc = wouldacc;
989  }
990 
991  // It can happen that wantedacc ist higher than previous calculated wantedacc, BUT acc is lower than prev calculated values!
992  // This often occurs because of "coolness"+Iteration and in this case "acc" is set to the previous (higher) calculated value!
993  if (vars->realacc > acc && vars->minaccel <= wantedacc - NUMERICAL_EPS && update != 0) {
994  acc = vars->realacc;
995  newSpeed = MAX2(0.0, egoSpeed + ACCEL2SPEED(acc));
996  }
997 
998  // Capture the relevant variables, because it was determined, that this call will result in the acceleration update (vars->minaccel > wantedacc)
999  if (vars->minaccel > wantedacc - NUMERICAL_EPS && update != 0) {
1000  vars->minaccel = wantedacc;
1001  if (vars->realacc > acc) {
1002  vars->realacc = acc;
1003  }
1004  vars->realleaderacc = a_leader;
1005  }
1006 
1007  // Save the parameters for a potential update in finalizeSpeed, when _v was called in a stopSpeed-function!!!
1008  if (vars->minaccel > wantedacc - NUMERICAL_EPS && update == 0 && !MSGlobals::gComputeLC) {
1009  vars->stop.push_back(std::make_pair(acc, gap2pred));
1010  }
1011 
1012  return MAX2(0., newSpeed);
1013 }
1014 
1015 
1016 MSCFModel*
1018  return new MSCFModel_EIDM(vtype);
1019 }
#define DEBUG_COND2(obj)
Definition: MESegment.cpp:52
#define EMERGENCY_DECEL_AMPLIFIER
Definition: MSCFModel.h:33
#define EST_REAC_THRESHOLD
#define SPEED2DIST(x)
Definition: SUMOTime.h:43
#define ACCEL2SPEED(x)
Definition: SUMOTime.h:49
#define TS
Definition: SUMOTime.h:40
#define SIMTIME
Definition: SUMOTime.h:60
#define DIST2SPEED(x)
Definition: SUMOTime.h:45
#define ACCEL2DIST(x)
Definition: SUMOTime.h:47
#define SPEED2ACCEL(x)
Definition: SUMOTime.h:51
LinkDirection
The different directions a link between two lanes may take (or a stream between two edges)....
@ PARTLEFT
The link is a partial left direction.
@ RIGHT
The link is a (hard) right direction.
@ TURN
The link is a 180 degree turn.
@ LEFT
The link is a (hard) left direction.
@ STRAIGHT
The link is a straight direction.
@ TURN_LEFTHAND
The link is a 180 degree turn (left-hand network)
@ PARTRIGHT
The link is a partial right direction.
@ NODIR
The link has no direction (is a dead end link)
@ SUMO_ATTR_CF_EIDM_T_ACC_MAX
@ SUMO_ATTR_CF_EIDM_EPSILON_ACC
@ SUMO_ATTR_CF_EIDM_T_LOOK_AHEAD
@ SUMO_ATTR_CF_EIDM_USEVEHDYNAMICS
@ SUMO_ATTR_CF_EIDM_C_COOLNESS
@ SUMO_ATTR_CF_EIDM_SIG_ERROR
@ SUMO_ATTR_CF_IDM_DELTA
@ SUMO_ATTR_CF_EIDM_T_REACTION
@ SUMO_ATTR_CF_EIDM_T_PERSISTENCE_ESTIMATE
@ SUMO_ATTR_CF_EIDM_SIG_GAP
@ SUMO_ATTR_CF_EIDM_JERK_MAX
@ SUMO_ATTR_CF_IDM_STEPPING
@ SUMO_ATTR_COLLISION_MINGAP_FACTOR
@ SUMO_ATTR_CF_EIDM_M_FLATNESS
@ SUMO_ATTR_CF_EIDM_M_BEGIN
@ SUMO_ATTR_CF_EIDM_T_PERSISTENCE_DRIVE
@ SUMO_ATTR_CF_EIDM_SIG_LEADER
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:32
T MIN2(T a, T b)
Definition: StdDefs.h:74
T ISNAN(T a)
Definition: StdDefs.h:115
T MAX2(T a, T b)
Definition: StdDefs.h:80
T MAX3(T a, T b, T c)
Definition: StdDefs.h:94
virtual double patchSpeed(const double min, const double wanted, const double max, const MSCFModel &cfModel)=0
Called to adapt the speed in order to allow a lane change. It uses information on LC-related desired ...
virtual bool isSelected() const
whether this vehicle is selected in the GUI
double getChosenSpeedFactor() const
Returns the precomputed factor by which the driver wants to be faster than the speed limit.
std::vector< std::pair< double, double > > stop
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap, const int update) const
const double myTPersEstimate
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
const double myTPersDrive
~MSCFModel_EIDM()
Destructor.
const double myDelta
const double myCcoolness
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, double decel) const
Computes the vehicle's safe speed for approaching a non-moving obstacle.
const double myTaccmax
const double mySigmaleader
const double myEpsilonacc
double maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed (no dawdling) This method is used during the insertion stage....
double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
const double myTreaction
double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false) const
Computes the vehicle's safe speed without a leader.
const double myMbegin
const double myTwoSqrtAccelDecel
MSCFModel_EIDM(const MSVehicleType *vtype)
Constructor.
double insertionStopSpeed(const MSVehicle *const veh, double speed, double gap) const
Computes the vehicle's safe speed for approaching an obstacle at insertion without constraints due to...
double internalsecuregap(const MSVehicle *const veh, const double speed, const double leaderSpeed, const double targetDecel) const
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed.
const double mySigmagap
void internalspeedlimit(MSVehicle *const veh, const double oldV) const
double getSecureGap(const MSVehicle *const veh, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
const double myMflatness
const double myJerkmax
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
const double mySigmaerror
const double myTpreview
const int myIterations
double patchSpeedBeforeLCEIDM(const MSVehicle *veh, double vMin, double vMax, VehicleVariables *vars) const
Applies dawdling / driving error.
The car-following model abstraction.
Definition: MSCFModel.h:55
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
Definition: MSCFModel.cpp:237
virtual double minNextSpeedEmergency(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed after emergency braking, given the current speed (depends on the numerical ...
Definition: MSCFModel.cpp:254
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
Definition: MSCFModel.cpp:243
double myEmergencyDecel
The vehicle's maximum emergency deceleration [m/s^2].
Definition: MSCFModel.h:649
double myCollisionMinGapFactor
The factor of minGap that must be maintained to avoid a collision event.
Definition: MSCFModel.h:653
double calculateEmergencyDeceleration(double gap, double egoSpeed, double predSpeed, double predMaxDecel) const
Returns the minimal deceleration for following the given leader safely.
Definition: MSCFModel.cpp:933
double myDecel
The vehicle's maximum deceleration [m/s^2].
Definition: MSCFModel.h:647
double myAccel
The vehicle's maximum acceleration [m/s^2].
Definition: MSCFModel.h:644
const MSVehicleType * myType
The type to which this model definition belongs to.
Definition: MSCFModel.h:641
double maximumSafeStopSpeedBallistic(double gap, double decel, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap when using the ballistic positional update.
Definition: MSCFModel.cpp:794
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
Definition: MSCFModel.h:656
static bool gSemiImplicitEulerUpdate
Definition: MSGlobals.h:53
static bool gComputeLC
whether the simulationLoop is in the lane changing phase
Definition: MSGlobals.h:127
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
static std::vector< MSLink * >::const_iterator succLinkSec(const SUMOVehicle &veh, int nRouteSuccs, const MSLane &succLinkSource, const std::vector< MSLane * > &conts)
Definition: MSLane.cpp:2165
double getLength() const
Returns the lane's length.
Definition: MSLane.h:541
bool isLinkEnd(std::vector< MSLink * >::const_iterator &i) const
Definition: MSLane.h:763
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
Definition: MSLane.h:519
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5051
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Definition: MSVehicle.h:485
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5497
std::pair< const MSVehicle *const, double > getLeader(double dist=0) const
Returns the leader of the vehicle looking for a fixed distance.
Definition: MSVehicle.cpp:5727
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
Definition: MSVehicle.h:934
double getSpeed() const
Returns the vehicle's current speed.
Definition: MSVehicle.h:462
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
Definition: MSVehicle.cpp:1515
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:372
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:552
The car-following model and parameter.
Definition: MSVehicleType.h:62
double getMaxSpeed() const
Get vehicle's maximum speed [m/s].
double getMinGap() const
Get the free space in front of vehicles of this class.
const SUMOVTypeParameter & getParameter() const
static double randNorm(double mean, double variance, SumoRNG *rng=nullptr)
Access to a random number from a normal distribution.
Definition: RandHelper.cpp:82
double getCFParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.