Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 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 : /****************************************************************************/
14 : /// @file MSCFModel_EIDM.cpp
15 : /// @author Dominik Salles
16 : /// @date Fri, 06 Jul 2018
17 :
18 : /// Originalfile MSCFModel_IDM.cpp from
19 : /// @author Tobias Mayer
20 : /// @author Daniel Krajzewicz
21 : /// @author Michael Behrisch
22 : /// @date Thu, 03 Sep 2009
23 : ///
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>
42 : #include <microsim/devices/MSDevice_GLOSA.h>
43 : #include <utils/common/RandHelper.h>
44 : #include <utils/common/SUMOTime.h>
45 : #include <utils/xml/SUMOSAXAttributes.h>
46 :
47 : //#define DEBUG_V
48 :
49 : #define EST_REAC_THRESHOLD 3. // under this threshold estimation, error and reaction time variables don't get taken into account
50 : #define ClutchEngageSpeed 0.5 // When a vehicle is below this speed, we assume a "slow to start", that is because of clutch operation / powertrain inertia
51 : #define EIDM_POS_ACC_EPS 0.05 // some slack number to ensure smoother position, speed and acceleration update
52 :
53 : // ===========================================================================
54 : // method definitions
55 : // ===========================================================================
56 302 : MSCFModel_EIDM::MSCFModel_EIDM(const MSVehicleType* vtype) :
57 302 : MSCFModel(vtype), myDelta(vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_DELTA, 4.)),
58 302 : myIterations(MAX2(1, int(TS / vtype->getParameter().getCFParam(SUMO_ATTR_CF_IDM_STEPPING, .25) + .5))),
59 302 : myTPersDrive(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_DRIVE, 3)),
60 302 : myTreaction(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_REACTION, 0.5)),
61 302 : myTpreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_LOOK_AHEAD, 4)),
62 302 : myTPersEstimate(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_PERSISTENCE_ESTIMATE, 10)),
63 302 : myCcoolness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_C_COOLNESS, 0.99)),
64 302 : mySigmaleader(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_LEADER, 0.02)),
65 302 : mySigmagap(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_GAP, 0.1)),
66 302 : mySigmaerror(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_SIG_ERROR, 0.04)),
67 302 : myJerkmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_JERK_MAX, 3.)),
68 302 : myEpsilonacc(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_EPSILON_ACC, 1.)),
69 302 : myTaccmax(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_T_ACC_MAX, 1.2)),
70 302 : myMflatness(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_FLATNESS, 2.)),
71 302 : myMbegin(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_M_BEGIN, 0.7)),
72 604 : myUseVehDynamics(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_USEVEHDYNAMICS, 0) == 1)
73 : //, myMaxVehPreview(vtype->getParameter().getCFParam(SUMO_ATTR_CF_EIDM_MAX_VEH_PREVIEW, 0))
74 : {
75 : // IDM does not drive very precise and may violate minGap on occasion
76 302 : myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
77 302 : if (vtype->getActionStepLength() != DELTA_T) {
78 12 : WRITE_WARNINGF("CarFollowModel EIDM is not compatible with actionStepLength % in vType '%'", STEPS2TIME(vtype->getActionStepLength()), vtype->getID());
79 : }
80 302 : }
81 :
82 604 : MSCFModel_EIDM::~MSCFModel_EIDM() {}
83 :
84 : void
85 10 : MSCFModel_EIDM::VehicleVariables::saveState(OutputDevice& out, const MSCFModel& /*cfm*/) const {
86 10 : out.openTag(SUMO_TAG_CFM_VARIABLES);
87 10 : out.writeAttr(SUMO_ATTR_ID, "EIDM");
88 10 : std::ostringstream internals;
89 10 : internals << minaccel << " ";
90 10 : internals << wouldacc << " ";
91 10 : internals << lastacc << " ";
92 10 : internals << realacc << " ";
93 10 : internals << lastrealacc << " ";
94 10 : internals << realleaderacc << " ";
95 10 : internals << lastleaderacc << " ";
96 10 : internals << v0_int << " ";
97 10 : internals << v0_old << " ";
98 10 : internals << t_off << " ";
99 10 : internals << myw_gap << " ";
100 10 : internals << myw_speed << " ";
101 10 : internals << myw_error << " ";
102 10 : internals << myv_est_l << " ";
103 10 : internals << myv_est << " ";
104 10 : internals << mys_est << " ";
105 10 : internals << myrespectMinGap << " ";
106 10 : internals << myap_update;
107 10 : out.writeAttr(SUMO_ATTR_STATE, internals.str());
108 10 : out.closeTag();
109 10 : }
110 :
111 :
112 : void
113 10 : MSCFModel_EIDM::VehicleVariables::loadState(const SUMOSAXAttributes& attrs) {
114 10 : bool ok = true;
115 10 : const std::string cfmID = attrs.get<std::string>(SUMO_ATTR_ID, nullptr, ok);
116 10 : if (cfmID != "EIDM") {
117 0 : throw ProcessError(TLF("incompatible carFollowModel '%' when loading state for EIDM", cfmID));
118 : }
119 10 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
120 10 : bis >> minaccel;
121 10 : bis >> wouldacc;
122 10 : bis >> lastacc;
123 10 : bis >> realacc;
124 10 : bis >> lastrealacc;
125 10 : bis >> realleaderacc;
126 10 : bis >> lastleaderacc;
127 10 : bis >> v0_int;
128 10 : bis >> v0_old;
129 10 : bis >> t_off;
130 10 : bis >> myw_gap;
131 10 : bis >> myw_speed;
132 10 : bis >> myw_error;
133 10 : bis >> myv_est_l;
134 10 : bis >> myv_est;
135 10 : bis >> mys_est;
136 10 : bis >> myrespectMinGap;
137 10 : bis >> myap_update;
138 20 : }
139 :
140 :
141 :
142 : double
143 32161 : MSCFModel_EIDM::insertionFollowSpeed(const MSVehicle* const /*veh*/, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
144 32161 : if (MSGlobals::gSemiImplicitEulerUpdate) {
145 32161 : return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
146 : } else {
147 : // Not Done/checked yet for the ballistic update model!!!!
148 : // NOTE: Even for ballistic update, the current speed is irrelevant at insertion, therefore passing 0. (Leo)
149 : // return maximumSafeFollowSpeed(gap2pred, 0., predSpeed, predMaxDecel, true);
150 0 : return maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel, true);
151 : }
152 : }
153 :
154 :
155 : double
156 4 : MSCFModel_EIDM::insertionStopSpeed(const MSVehicle* const /*veh*/, double speed, double gap) const {
157 4 : if (MSGlobals::gSemiImplicitEulerUpdate) {
158 4 : return maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime);
159 : } else {
160 : // Not Done/checked yet for the ballistic update model!!!!
161 : // return MIN2(maximumSafeStopSpeed(gap, myDecel, 0., true, 0.), myType->getMaxSpeed());
162 0 : return MIN2(maximumSafeStopSpeed(gap, myDecel, speed, true, myHeadwayTime), myType->getMaxSpeed());
163 : }
164 : }
165 :
166 : double
167 32161 : MSCFModel_EIDM::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion, const CalcReason /* usage */) const {
168 : double x;
169 32161 : if (gap >= 0 || MSGlobals::gComputeLC) {
170 : double a = 1.;
171 32161 : double b = myHeadwayTime * (2 * sqrt(getCurrentAccel(egoSpeed) * myDecel)) - predSpeed;
172 32161 : double c = -sqrt(1 + myDecel / (2 * getCurrentAccel(egoSpeed))) * gap * (2 * sqrt(getCurrentAccel(egoSpeed) * myDecel));
173 : // with myDecel/myAccel, the intended deceleration is myDecel
174 : // with myDecel/(2*myAccel), the intended deceleration is myDecel/2
175 : // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term v/vMax is not present
176 :
177 : // double c = -sqrt(1 - pow(egoSpeed / MAX2(NUMERICAL_EPS, desSpeed), myDelta) + myDecel / (2 * getCurrentAccel(egoSpeed))) * gap * (2 * sqrt(getCurrentAccel(egoSpeed) * myDecel)); // c calculation when using the IDM!
178 :
179 : // myDecel is positive, but is intended as negative value here, therefore + instead of -
180 : // quadratic formula
181 32161 : x = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
182 32161 : } else {
183 0 : x = egoSpeed - ACCEL2SPEED(myEmergencyDecel);
184 0 : if (MSGlobals::gSemiImplicitEulerUpdate) {
185 : x = MAX2(x, 0.);
186 : }
187 : }
188 :
189 32161 : if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
190 0 : double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
191 0 : if (origSafeDecel > myDecel + NUMERICAL_EPS) {
192 : // Braking harder than myDecel was requested -> calculate required emergency deceleration.
193 : // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
194 : // 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,
195 : // such that braking harder than myDecel is required.
196 :
197 0 : double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
198 : #ifdef DEBUG_EMERGENCYDECEL
199 : if (DEBUG_COND2) {
200 : std::cout << SIMTIME << " initial vsafe=" << x
201 : << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
202 : << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
203 : << " safeDecel=" << safeDecel
204 : << std::endl;
205 : }
206 : #endif
207 : // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
208 0 : safeDecel = MAX2(safeDecel, myDecel);
209 : // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
210 : safeDecel = MIN2(safeDecel, origSafeDecel);
211 0 : x = egoSpeed - ACCEL2SPEED(safeDecel);
212 0 : if (MSGlobals::gSemiImplicitEulerUpdate) {
213 : x = MAX2(x, 0.);
214 : }
215 :
216 : #ifdef DEBUG_EMERGENCYDECEL
217 : if (DEBUG_COND2) {
218 : std::cout << " -> corrected emergency deceleration: " << safeDecel << std::endl;
219 : }
220 : #endif
221 :
222 : }
223 : }
224 : assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
225 : assert(!std::isnan(x));
226 32161 : return x;
227 : }
228 :
229 : double
230 4 : MSCFModel_EIDM::maximumSafeStopSpeed(double gap, double decel, double currentSpeed, bool onInsertion, double headway) const {
231 : double vsafe;
232 4 : if (MSGlobals::gSemiImplicitEulerUpdate) {
233 4 : const double g = gap - NUMERICAL_EPS;
234 4 : if (g < 0) {
235 : return 0;
236 : }
237 : double a = 1.;
238 4 : double b = headway * (2 * sqrt(getCurrentAccel(currentSpeed) * myDecel)) - 0.;
239 4 : double c = -sqrt(1 + decel / (2 * getCurrentAccel(currentSpeed))) * g * (2 * sqrt(getCurrentAccel(currentSpeed) * myDecel));
240 : // with decel/myAccel, the intended deceleration is decel
241 : // with decel/(2*myAccel), the intended deceleration is decel/2
242 : // with the IIDM, if gap=s, then the acceleration is zero and if gap<s, then the term currentSpeed/vMax is not present
243 :
244 : // double c = -sqrt(1 - pow(currentSpeed / MAX2(NUMERICAL_EPS, desSpeed), myDelta) + decel / (2 * getCurrentAccel(currentSpeed))) * g * (2 * sqrt(getCurrentAccel(currentSpeed) * myDecel)); // c calculation when using the IDM!
245 :
246 : // decel is positive, but is intended as negative value here, therefore + instead of -
247 : // quadratic formula
248 4 : vsafe = (-b + sqrt(b * b - 4.*a * c)) / (2.*a);
249 : } else {
250 : // Not Done/checked yet for the ballistic update model!!!!
251 0 : vsafe = maximumSafeStopSpeedBallistic(gap, decel, currentSpeed, onInsertion, headway);
252 : }
253 :
254 : return vsafe;
255 : }
256 :
257 : double
258 5763397 : MSCFModel_EIDM::patchSpeedBeforeLCEIDM(const MSVehicle* /*veh*/, double vMin, double vMax, const VehicleVariables* vars) const {
259 : // The default value of SUMO_ATTR_JM_SIGMA_MINOR is sigma, not sigmaerror (used in EIDM),
260 : // so we do not use SUMO_ATTR_JM_SIGMA_MINOR as parameter here, because this could confuse users...
261 : //const double sigma = (veh->passingMinor()
262 : // ? veh->getVehicleType().getParameter().getJMParam(SUMO_ATTR_JM_SIGMA_MINOR, myDawdle)
263 : // : myDawdle);
264 :
265 : // dawdling/drivingerror is now calculated here (in finalizeSpeed, not in stop-/follow-/freeSpeed anymore):
266 : // Instead of just multiplying mySigmaerror with vars->myw_error, we add a factor depending on the criticality of the situation,
267 : // measured with s*/gap. Because when the driver drives "freely" (nothing in front) he may dawdle more than in e.g. congested traffic!
268 5763397 : double Accel_vMax = getCurrentAccel(vMax);
269 5763397 : double s = MAX2(0., vars->myv_est * myHeadwayTime + vars->myv_est * (vars->myv_est - vars->myv_est_l) / (2 * sqrt(Accel_vMax * myDecel)));
270 5763397 : if (vars->myrespectMinGap) {
271 5608217 : s += myType->getMinGap() + EIDM_POS_ACC_EPS;
272 : } else {
273 155180 : const double minGapStop_EPS = 0.05 + 0.20 * MAX2(0.25, getCurrentAccel(0.0));
274 155180 : s += minGapStop_EPS + EIDM_POS_ACC_EPS;
275 : }
276 5763397 : const double intensity = MIN3(Accel_vMax, 1.5, MAX2(vMax - 0.5 * Accel_vMax, 0.0));
277 5763397 : const double criticality = MIN2(MAX2(s / vars->mys_est - 0.5, -0.4), 0.0);
278 :
279 5763397 : const double drivingerror = mySigmaerror * vars->myw_error * intensity * (2.75 * 2.75 * criticality * criticality + 1.0);
280 :
281 : // else: the vehicle is very slow and we do not add driving error (= 0), because
282 : // we should not prevent vehicles from driving just due to dawdling
283 : // if someone is starting, he should definitely start
284 :
285 : //const double vDawdle = MAX2(vMin, dawdle2(vMax, sigma, veh->getRNG()));
286 5763397 : const double vDawdle = MAX2(vMin, vMax + ACCEL2SPEED(drivingerror));
287 5763397 : return vDawdle;
288 : }
289 :
290 : double
291 5763397 : MSCFModel_EIDM::slowToStartTerm(MSVehicle* const veh, const double newSpeed, const double currentSpeed, const double vMax, VehicleVariables* vars) const {
292 : // Drive Off Activation and Term
293 :
294 5763397 : if (newSpeed == 0 || newSpeed <= currentSpeed) {
295 : return newSpeed;
296 : }
297 :
298 : double remainingDelay = 0.0;
299 2539619 : if (newSpeed != vMax) {
300 0 : remainingDelay = STEPS2TIME(DELTA_T - (myStartupDelay - (veh->getTimeSinceStartup() - DELTA_T)));
301 : }
302 :
303 : double v_corr = currentSpeed;
304 10900928 : for (int i = 0; i < myIterations; i++) {
305 : // @ToDo: Check if all clauses are still needed or if we need to add more for all possible drive off cases?!
306 : // When we reach this point, "newSpeed > currentSpeed" already holds
307 : // Activation of the Drive Off term, when
308 67216 : if (currentSpeed < ClutchEngageSpeed && // The start speed is lower than ClutchEngageSpeed m/s
309 67216 : vars->t_off + 4. - NUMERICAL_EPS < (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations) && // the last activation is at least 4 seconds ago
310 8382228 : vars->myap_update == 0 && // the last activation is at least 4 seconds ago AND an Action Point was reached
311 10189 : veh->getAcceleration() < MIN2(getCurrentAccel(currentSpeed) / 4, 0.2)) { // && respectMinGap) { // the driver hasn't started accelerating yet (<0.2)
312 10117 : vars->t_off = (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations); // activate the drive off term
313 : }
314 : // Calculation of the Drive Off term
315 8361309 : if (vars->t_off + myTaccmax + NUMERICAL_EPS > (SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations)) {
316 56989 : v_corr = v_corr + (newSpeed - currentSpeed) / myIterations * (tanh((((SIMTIME - remainingDelay - TS * (myIterations - i - 1.) / myIterations) - vars->t_off) * 2. / myTaccmax - myMbegin) * myMflatness) + 1.) / 2.;
317 : } else {
318 8304320 : v_corr = v_corr + (newSpeed - currentSpeed) / myIterations;
319 : }
320 : }
321 : return v_corr;
322 : }
323 :
324 : double
325 5763400 : MSCFModel_EIDM::finalizeSpeed(MSVehicle* const veh, double vPos) const {
326 : // finalizeSpeed is only called once every timestep!
327 :
328 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
329 : // save old v for optional acceleration computation
330 5763400 : const double oldV = veh->getSpeed();
331 :
332 : // @ToDo: Maybe change whole calculation to calculate real freeSpeed/stopSpeed/followSpeed in every call and here calculate resulting speed with reaction Time and update?!
333 : // @ToDo: Could check which call resulted in speed update with stop-vector containing all calculated accelerations!
334 : // Check whether the speed update results from a stop calculation, if so, run _v-function again with the saved variables from stopSpeed
335 : double _vPos = vPos;
336 5763400 : if ((vPos <= SUMO_const_haltingSpeed && vPos <= oldV) || !(vPos > oldV + ACCEL2SPEED(vars->realacc) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(vars->realacc) + NUMERICAL_EPS)) {
337 1385576 : for (auto it = vars->stop.cbegin(); it != vars->stop.cend(); ++it) {
338 74939 : if (vPos > oldV + ACCEL2SPEED(it->first) - NUMERICAL_EPS && vPos < oldV + ACCEL2SPEED(it->first) + NUMERICAL_EPS) {
339 43849 : _vPos = _v(veh, it->second, oldV, 0, vars->v0_int, false, 1, CalcReason::CURRENT);
340 : }
341 : }
342 : }
343 :
344 : // process stops (includes update of stopping state)
345 5763397 : const double vStop = MIN2(_vPos, veh->processNextStop(_vPos));
346 : // apply deceleration bounds
347 5763397 : const double vMinEmergency = minNextSpeedEmergency(oldV, veh);
348 : // _vPos contains the uppper bound on safe speed. allow emergency braking here
349 5763397 : const double vMin = MIN2(minNextSpeed(oldV, veh), MAX2(_vPos, vMinEmergency));
350 : // apply planned speed constraints and acceleration constraints
351 5763397 : double vMax = MIN2(maxNextSpeed(oldV, veh), vStop);
352 : vMax = MAX2(vMin, vMax);
353 :
354 : #ifdef DEBUG_V
355 : if (veh->isSelected()) {
356 : std::cout << SIMTIME
357 : << " EIDM::finalizeSpeed "
358 : << " veh=" << veh->getID()
359 : << " oldV=" << oldV
360 : << " vPos=" << vPos
361 : << " _vPos=" << _vPos
362 : << " vStop=" << vStop
363 : << " vMinEmergency=" << vMinEmergency
364 : << " vMin=" << vMin
365 : << " vMax=" << vMax
366 : << "\n";
367 : }
368 : #endif
369 :
370 : // apply further speed adaptations
371 5763397 : double vNext = patchSpeedBeforeLCEIDM(veh, vMin, vMax, vars);
372 :
373 5763397 : if (MSGlobals::gSemiImplicitEulerUpdate) {
374 :
375 : // The model does not directly use vNext from patchSpeed (like the original MSCFModel::finalizeSpeed function),
376 : // but rather slowly adapts to vNext.
377 5763333 : vNext = veh->getLaneChangeModel().patchSpeed(vMin, vNext, vMax, *this);
378 :
379 : // Bound the positive change of the acceleration with myJerkmax
380 5763333 : if (vNext > oldV && oldV > ClutchEngageSpeed * 2 && vars->t_off + myTaccmax + NUMERICAL_EPS < SIMTIME) {
381 : // 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!!!
382 : // 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!
383 2501995 : if (myJerkmax * TS + veh->getAcceleration() < 0.) { // If driver wants to accelerate, but is still decelerating, then we use a factor of 2!
384 1082 : vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * 2 * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax*2
385 : } else {
386 2503410 : vNext = MAX2(oldV + MIN2(vNext - oldV, (myJerkmax * TS + veh->getAcceleration()) * TS), 0.); // change in acceleration (jerk) is bounded by myJerkmax
387 : }
388 3261338 : } else if (vNext <= oldV && vNext < vMax - NUMERICAL_EPS && oldV > ClutchEngageSpeed * 2) {
389 : // Slowing down the deceleration like this may be critical!!! Vehicle can also not come back from Emergency braking fast enough!
390 : /*if (vNext - oldV < -myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake harder than before, change in acceleration is then bounded by -myJerkmax
391 : vNext = MAX2(oldV + (-myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
392 : } else if (vNext - oldV > myJerkmax * TS + veh->getAcceleration()) { // driver wants to brake less harder than before, change in acceleration is then bounded by +myJerkmax
393 : vNext = MAX2(oldV + (myJerkmax * TS + veh->getAcceleration()) * TS, 0.);
394 : } else {
395 : vNext = vNext; // Do nothing, as the new vNext is in the range of +/- jerk!
396 : }*/
397 :
398 : // Workaround letting the vehicle not brake hard for Lane Change reasons (vNext), but only for safety Car following / stopping reasons (vMax)!
399 473518 : vNext = MAX2(oldV + MIN2(vMax - oldV, MAX2(vNext - oldV, (-myJerkmax * TS + veh->getAcceleration()) * TS)), 0.);
400 : }
401 :
402 : } else {
403 : // Not Done/checked yet for the ballistic update model!!!!
404 :
405 : // for ballistic update, negative vnext must be allowed to
406 : // indicate a stop within the coming timestep (i.e., to attain negative values)
407 64 : vNext = veh->getLaneChangeModel().patchSpeed(vMin, vMax, vMax, *this);
408 : // (Leo) finalizeSpeed() is responsible for assuring that the next
409 : // velocity is chosen in accordance with maximal decelerations.
410 : // At this point vNext may also be negative indicating a stop within next step.
411 : // Moreover, because maximumSafeStopSpeed() does not consider deceleration bounds
412 : // vNext can be a large negative value at this point. We cap vNext here.
413 : vNext = MAX2(vNext, vMin);
414 : }
415 :
416 : // Driving off correction term: First we check for StartupDelay, then calculate Speed with SlowToStartTerm
417 : // Apply Startup delay (time) before driving off
418 5763397 : SUMOTime addTime = vars->myap_update * DELTA_T;
419 5763397 : if (myStartupDelay + addTime - (veh->getTimeSinceStartup() - DELTA_T) < DELTA_T) {
420 : addTime = (SUMOTime)0;
421 : }
422 5763397 : double vDelay = applyStartupDelay(veh, vMin, vNext, addTime);
423 : // Apply the slow to start term to the acceleration/speed when driving off
424 5763397 : vNext = slowToStartTerm(veh, vDelay, oldV, vNext, vars);
425 : #ifdef DEBUG_V
426 : if (veh->isSelected()) {
427 : std::cout << SIMTIME
428 : << " EIDM::finalizeSpeed (2) "
429 : << " veh=" << veh->getID()
430 : << " timeSinceStartup=" << veh->getTimeSinceStartup()
431 : << " myap_update=" << vars->myap_update
432 : << " addTime=" << addTime
433 : << " vDelay=" << vDelay
434 : << " oldV=" << oldV
435 : << " vNext=" << vNext
436 : << "\n";
437 : }
438 : #endif
439 :
440 : // Update the desired speed
441 5763397 : internalspeedlimit(veh, oldV);
442 :
443 5763397 : if (vNext > EST_REAC_THRESHOLD) { // update the Wiener-Prozess variables
444 4287135 : vars->myw_gap = exp(-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!
445 4287135 : vars->myw_speed = exp(-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!
446 4287135 : vars->myw_error = exp(-TS / myTPersDrive) * vars->myw_error + sqrt(2 * TS / myTPersDrive) * RandHelper::randNorm(0, 1);
447 : } // else all those w_... are zero by default
448 :
449 : // Update the Action-point reaction time
450 5763397 : if (vars->myap_update == 0) { // Update all saved acceleration variables for further calculcation between two action points
451 3803403 : vars->lastacc = vars->minaccel;
452 3803403 : vars->wouldacc = vars->minaccel;
453 3803403 : vars->lastrealacc = vars->realacc;
454 3803403 : vars->lastleaderacc = vars->realleaderacc;
455 : }
456 :
457 : #ifdef DEBUG_V
458 : if (veh->isSelected()) {
459 : std::cout << SIMTIME
460 : << " EIDM::finalizeSpeed (3) "
461 : << " veh=" << veh->getID()
462 : << " vars->myw_gap=" << vars->myw_gap
463 : << " vars->myw_speed=" << vars->myw_speed
464 : << " vars->myw_error=" << vars->myw_error
465 : << " vars->lastacc=" << vars->lastacc
466 : << " vars->lastrealacc=" << vars->lastrealacc
467 : << " vars->lastleaderacc=" << vars->lastleaderacc
468 : << "\n";
469 : }
470 : #endif
471 :
472 : // Set myap_update back to 0 when maximal reaction time is reached,
473 : // else add 1 for the next time step
474 5763397 : if (double(vars->myap_update) >= double(myTreaction / TS - 1 - NUMERICAL_EPS)) {
475 3765839 : vars->myap_update = 0;
476 : } else {
477 1997558 : vars->myap_update = vars->myap_update + 1;
478 : }
479 :
480 : // Here the acceleration the vehicle would adapt to without a reaction time is compared to the last acceleration update at the last action point.
481 : // 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).
482 : // This update is only used when wouldacc becomes myEpsilonacc lower than lastacc! When accelerating (wouldacc > lastacc), always the maximal reaction time is used!
483 : // @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.
484 : // @ToDo: Use this asynchron action point update also for accelerating (like introduced by Wagner/Hoogendorn/Treiber), not only for keeping the CF-model stable!
485 5763397 : if ((vars->wouldacc - vars->lastacc) < -myEpsilonacc || vars->wouldacc < -getEmergencyDecel() || (oldV < EST_REAC_THRESHOLD && vNext < oldV)) {
486 : // When this if-clause holds, then the driver should react immediately!
487 : // 1. When the change in deceleration is lower than -myEpsilonacc
488 : // 2. When the intended deceleration is lower than emergencyDecel
489 : // 3. When the vehicle is slow and decelerating
490 82233 : vars->myap_update = 0;
491 : }
492 :
493 : // Set the "inner" variables of the car-following model back to the default values for the next time step
494 5763397 : vars->minaccel = 100;
495 5763397 : vars->realacc = 100;
496 5763397 : vars->realleaderacc = 100;
497 :
498 : vars->stop.clear();
499 :
500 5763397 : return vNext;
501 : }
502 :
503 :
504 : double
505 33020725 : MSCFModel_EIDM::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double /*predMaxDecel*/, const MSVehicle* const /*pred*/, const CalcReason usage) const {
506 : // applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
507 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
508 :
509 : // This update-variable is used to check what called followSpeed (LC- or CF-model), see enum CalcReason for more information
510 : // Here we don't directly use gComputeLC, which is also 0 and 1, because in freeSpeed we have another call (update = 2),
511 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
512 : int update = 1;
513 : CalcReason _vUsage = usage;
514 33020725 : if (MSGlobals::gComputeLC) {
515 : _vUsage = CalcReason::LANE_CHANGE;
516 : }
517 33020725 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE) {
518 : update = 0;
519 : }
520 :
521 : #ifdef DEBUG_V
522 : if (veh->isSelected()) {
523 : std::cout << SIMTIME
524 : << " EIDM::followSpeed "
525 : << " veh=" << veh->getID()
526 : << " speed=" << speed
527 : << " gap2pred=" << gap2pred
528 : << " predSpeed=" << predSpeed
529 : << " vars->v0_int=" << vars->v0_int
530 : << " update=" << update
531 : << "\n";
532 : }
533 : #endif
534 :
535 33020725 : double result = _v(veh, gap2pred, speed, predSpeed, vars->v0_int, true, update, _vUsage);
536 33020725 : return result;
537 : }
538 :
539 :
540 : double
541 14840887 : MSCFModel_EIDM::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double /*decel*/, const CalcReason usage) const {
542 : // applyHeadwayPerceptionError(veh, speed, gap);
543 : // if (gap < 0.01) {
544 : // return 0;
545 : // }
546 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
547 :
548 : // This update-variable is used to check what called stopSpeed (LC- or CF-model), see enum CalcReason for more information
549 : // Here we don't directly use gComputeLC, which is also 0 and 1, because in freeSpeed we have another call (update = 2),
550 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
551 : int update = 1;
552 : CalcReason _vUsage = usage;
553 14840887 : if (MSGlobals::gComputeLC) {
554 : _vUsage = CalcReason::LANE_CHANGE;
555 : }
556 14840887 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE || usage == CalcReason::CURRENT_WAIT) {
557 : update = 0;
558 : }
559 :
560 : #ifdef DEBUG_V
561 : if (veh->isSelected()) {
562 : std::cout << SIMTIME
563 : << " EIDM::stopSpeed "
564 : << " veh=" << veh->getID()
565 : << " speed=" << speed
566 : << " gap=" << gap
567 : << " vars->v0_int=" << vars->v0_int
568 : << " update=" << update
569 : << "\n";
570 : }
571 : #endif
572 :
573 14840887 : double result = _v(veh, gap, speed, 0, vars->v0_int, false, update, _vUsage);
574 : // From Sumo_IDM-implementation:
575 : // if (gap > 0 && speed < NUMERICAL_EPS && result < NUMERICAL_EPS) {
576 : // // ensure that stops can be reached:
577 : // result = maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs());
578 : // }
579 14840887 : return result;
580 : }
581 :
582 : double
583 20 : MSCFModel_EIDM::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion) {
584 : // XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
585 : // by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
586 : // changes to a road with a lower speed limit).
587 :
588 20 : if (MSGlobals::gSemiImplicitEulerUpdate) {
589 : // adapt speed to succeeding lane, no reaction time is involved
590 : // when breaking for y steps the following distance g is covered
591 : // (drive with v in the final step)
592 : // g = (y^2 + y) * 0.5 * b + y * v
593 : // y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
594 20 : const double v = SPEED2DIST(targetSpeed);
595 20 : if (dist < v) {
596 : return targetSpeed;
597 : }
598 0 : const double b = ACCEL2DIST(decel);
599 0 : const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
600 0 : const double yFull = floor(y);
601 0 : const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
602 0 : const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
603 0 : return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
604 : } else {
605 : // ballistic update (Leo)
606 : // calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
607 : // and given a maximal deceleration b=decel, denote the current speed by v0.
608 : // the distance covered by a trajectory that attains vN in the next timestep and decelerates afterwards
609 : // with b is given as
610 : // d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
611 : // where time t of arrival at d with speed vT is
612 : // t = dt + (vN-vT)/b. (2)
613 : // We insert (2) into (1) to obtain
614 : // d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
615 : // 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
616 : // and solve for vN
617 :
618 : assert(currentSpeed >= 0);
619 : assert(targetSpeed >= 0);
620 :
621 0 : const double dt = onInsertion ? 0 : TS; // handles case that vehicle is inserted just now (at the end of move)
622 : const double v0 = currentSpeed;
623 : const double vT = targetSpeed;
624 : const double b = decel;
625 0 : const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
626 :
627 : // Solvability for positive vN (if d is small relative to v0):
628 : // 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
629 : // (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
630 : // the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
631 : // 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
632 : // (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
633 :
634 0 : if (0.5 * (v0 + vT)*dt >= d) {
635 : return vT; // (#)
636 : }
637 :
638 0 : const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
639 0 : const double p = 0.5 * b * dt;
640 0 : return -p + sqrt(p * p - q);
641 : }
642 : }
643 :
644 : double
645 9880475 : MSCFModel_EIDM::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason usage) const {
646 :
647 : // @ToDo: Set new internal speed limit/desired speed <maxSpeed> here and change it over time in internalspeedlimit()!
648 :
649 9880475 : if (maxSpeed < 0.) {
650 : // can occur for ballistic update (in context of driving at red light)
651 : return maxSpeed;
652 : }
653 :
654 : // This update-variable is used to check what called freeSpeed (LC- or CF-model), see enum CalcReason for more information
655 : // Here we don't directly use gComputeLC, which is also 0 and 1, because we have another call (update = 2),
656 : // which is needed to differentiate between the different cases/calculations/needed output/saved variables
657 : int update = 1;
658 : CalcReason _vUsage = usage;
659 9880475 : if (MSGlobals::gComputeLC) {
660 : _vUsage = CalcReason::LANE_CHANGE;
661 : }
662 9880475 : if (_vUsage == CalcReason::LANE_CHANGE || _vUsage == CalcReason::FUTURE) {
663 : update = 0;
664 : }
665 :
666 : #ifdef DEBUG_V
667 : if (veh->isSelected()) {
668 : std::cout << SIMTIME
669 : << " EIDM::freeSpeed "
670 : << " veh=" << veh->getID()
671 : << " speed=" << speed
672 : << " seen=" << seen
673 : << " maxSpeed=" << maxSpeed
674 : << " update=" << update
675 : << " onInsertion=" << onInsertion
676 : << "\n";
677 : }
678 : #endif
679 :
680 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
681 :
682 9880475 : if (veh->getDevice(typeid(MSDevice_GLOSA)) != nullptr &&
683 9880475 : static_cast<MSDevice_GLOSA*>(veh->getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive() &&
684 : maxSpeed < speed) {
685 0 : seen = speed * (1 - (vars->v0_old - vars->v0_int) / (vars->v0_old - maxSpeed)) * myTpreview;
686 : }
687 :
688 : double vSafe, remaining_time, targetDecel;
689 : double secGap;
690 9880475 : if (onInsertion) {
691 : // 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!)!
692 : // @ToDo: Could maybe be changed to maximumSafeFollowSpeed instead of freeSpeed-Krauss calculation!
693 20 : vSafe = freeSpeed(speed, myDecel, seen, maxSpeed, onInsertion);
694 : } else {
695 : // 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)
696 : // The adaption/braking starts when the <seen> time-distance is lower than myTpreview+myTreaction
697 9880455 : if (maxSpeed < speed && seen < speed * (myTpreview + myTreaction)) {
698 :
699 21177 : update = update == 0 ? 0 : 2;
700 :
701 21177 : 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
702 21177 : targetDecel = (speed - maxSpeed) / remaining_time; // The needed constant deceleration to reach maxSpeed before reaching the next lane/event
703 :
704 : // targetDecel is not set immediatly, if the vehicle is far enough away from the event (bounded by myJerkmax)
705 21177 : if (remaining_time > myTpreview - targetDecel / myJerkmax) {
706 30 : targetDecel = (myTpreview - remaining_time) * myJerkmax;
707 : }
708 :
709 : // calculate distance which would result in the accel-value targetDecel at this <speed> and leaderspeed <0>
710 21177 : if (vars->myap_update == 0 || update == 0) { // at action point update
711 19666 : secGap = internalsecuregap(veh, speed, 0., targetDecel);
712 : } else { // between two action points
713 1511 : secGap = internalsecuregap(veh, vars->myv_est + vars->lastrealacc * vars->myap_update * TS, 0., targetDecel);
714 : }
715 :
716 39381 : vSafe = _v(veh, MAX2(seen, secGap), speed, 0., vars->v0_int, true, update, _vUsage);
717 :
718 : // Add this for "old implementation" when vehicle doesn't HAVE to reach maxspeed at seen-distance!
719 : // @ToDo: See #7644: <double v = MIN2(maxV, laneMaxV);> in MSVehicle::planMoveInternal! -> DONE!
720 : // But still: Is it better, if the vehicle brakes early enough to reach the next lane with its speed limit?
721 : // 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...)
722 : /* if (seen < speed*myTpreview || seen < veh->getLane()->getVehicleMaxSpeed(veh)*myTpreview / 2) {
723 : remaining_time = speed < veh->getLane()->getVehicleMaxSpeed(veh) / 2 ? seen / (veh->getLane()->getVehicleMaxSpeed(veh) / 2) : seen / MAX2(speed, 0.01);
724 : if (vars->v0_int > maxSpeed + NUMERICAL_EPS && vars->v0_old > vars->v0_int + NUMERICAL_EPS) {
725 : maxSpeed = MAX2(maxSpeed, MIN2(vars->v0_int, vars->v0_old - (vars->v0_old - maxSpeed) / myTpreview * (myTpreview - remaining_time)));
726 : vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update, _vUsage);
727 : } else if (vars->v0_int > maxSpeed + NUMERICAL_EPS) {
728 : maxSpeed = MAX2(maxSpeed, vars->v0_int - (vars->v0_int - maxSpeed) / myTpreview * (myTpreview - remaining_time));
729 : vSafe = _v(veh, 500., speed, maxSpeed, maxSpeed, true, update, _vUsage);
730 : } else {
731 : vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update, _vUsage);
732 : }
733 : */
734 : } else { // when the <speed> is lower than <maxSpeed> or the next lane/event is not seen with myTpreview+myTreaction yet
735 9859278 : vSafe = _v(veh, 500., speed, maxSpeed, vars->v0_int, true, update, _vUsage);
736 : }
737 : }
738 :
739 : return vSafe;
740 : }
741 :
742 : /// @todo update interactionGap logic to IDM
743 : double
744 0 : MSCFModel_EIDM::interactionGap(const MSVehicle* const veh, double vL) const {
745 : // Resolve the IDM equation to gap. Assume predecessor has
746 : // speed != 0 and that vsafe will be the current speed plus acceleration,
747 : // i.e that with this gap there will be no interaction.
748 0 : const double acc = getCurrentAccel(veh->getSpeed()) * (1. - pow(veh->getSpeed() / veh->getLane()->getVehicleMaxSpeed(veh), myDelta));
749 0 : const double vNext = veh->getSpeed() + acc;
750 0 : const double gap = (vNext - vL) * (veh->getSpeed() + vL) / (2 * myDecel) + vL;
751 :
752 : // Don't allow timeHeadWay < deltaT situations.
753 0 : return MAX2(gap, SPEED2DIST(vNext));
754 :
755 : // Only needed for old Lane Change Model????
756 : }
757 :
758 : double
759 33516291 : MSCFModel_EIDM::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /*leaderMaxDecel*/) const {
760 : // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
761 : //VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
762 33516291 : const double delta_v = speed - leaderSpeed;
763 33516291 : double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / (2 * sqrt(getCurrentAccel(speed) * myDecel))); // is calculated without MinGap because it is compared to a gap without MinGap!
764 : // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
765 : // 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!
766 : // The secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
767 :
768 33516291 : double erg = sqrt((s * s) / (myDecel / getCurrentAccel(speed) + 1.0));
769 33516291 : return MIN2(s, erg);
770 : }
771 :
772 : // Only needed when vehicle has to reach laneMaxV before entering the new lane, see #7644
773 : double
774 21177 : MSCFModel_EIDM::internalsecuregap(const MSVehicle* const veh, const double speed, const double leaderSpeed, const double targetDecel) const {
775 : // SecureGap depends on v0 and may be higher than just looking at s* (In case of the IDM)
776 : // internalsecuregap uses a targetDecel instead of myDecel!
777 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
778 21177 : double Accel_speed = getCurrentAccel(speed);
779 21177 : const double delta_v = speed - leaderSpeed;
780 21177 : double s = MAX2(0.0, speed * myHeadwayTime + speed * delta_v / (2 * sqrt(Accel_speed * myDecel))); // is calculated without MinGap because it is compared to a gap without MinGap!
781 : // For the IDM: - pow(speed / veh->getLane()->getVehicleMaxSpeed(veh), myDelta)) must be added to (myDecel / myAccel + 1)!
782 : // the secure gap is calculated using -myDecel as secure maximal acceleration (using myDecel/myAccel)!
783 :
784 : double erg;
785 21177 : if (speed <= vars->v0_int) {
786 310 : erg = sqrt((s * s) / (MAX2(targetDecel / Accel_speed + 1.0, 1.0)));
787 : } else { // speed > v0
788 20867 : double a_free = - myDecel * (1.0 - pow(vars->v0_int / speed, Accel_speed * myDelta / myDecel));
789 30645 : erg = sqrt((s * s) / (MAX2(targetDecel / Accel_speed + 1.0 + a_free / Accel_speed, 1.0)));
790 : }
791 :
792 21177 : return erg;
793 : }
794 :
795 : void
796 5763397 : MSCFModel_EIDM::internalspeedlimit(MSVehicle* const veh, const double oldV) const {
797 :
798 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
799 :
800 : double v_limcurr, v_limprev;
801 5763397 : v_limcurr = vars->v0_int; // model internal desired speed limit
802 5763397 : v_limprev = vars->v0_old; // previous desired speed limit for calculation reasons
803 :
804 5763397 : const MSLane* lane = veh->getLane();
805 5763397 : const std::vector<MSLane*>& bestLaneConts = veh->getBestLanesContinuation();
806 : int view = 1;
807 5763397 : std::vector<MSLink*>::const_iterator link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
808 5763397 : double seen = lane->getLength() - veh->getPositionOnLane();
809 5763397 : double v0 = lane->getVehicleMaxSpeed(veh); // current desired lane speed
810 : bool activeGLOSA = false;
811 5763397 : if (veh->getDevice(typeid(MSDevice_GLOSA)) != nullptr) {
812 0 : activeGLOSA = static_cast<MSDevice_GLOSA*>(veh->getDevice(typeid(MSDevice_GLOSA)))->isSpeedAdviceActive();
813 : }
814 :
815 : // @ToDo: nextTurn is only defined in sublane-model calculation?!
816 : // @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)
817 : // @ToDo: should not come from here, but from MSVehicle/the street network
818 : // const std::pair<double, LinkDirection> nextTurn = veh->getNextTurn();
819 :
820 : // When driving on the last lane/link, the vehicle shouldn't adapt to the lane after anymore.
821 : // Otherwise we check the <seen> time-distance and whether is lower than myTpreview
822 5763397 : if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2 || activeGLOSA)) {
823 : double speedlim = 200;
824 : while (true) { // loop over all upcoming edges/lanes/links until the <seen> time-distance is higher than myTpreview
825 2022155 : if (lane->isLinkEnd(link) != 1 && (seen < oldV * myTpreview || seen < v0 * myTpreview / 2)) { // @ToDo: add && (*link)->havePriority()???
826 : // @ToDo: When vehicles already brake because of a minor link, it may not be necessary to adapt the internal desired speed when turning...
827 : // @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
828 : // @ToDo: to accelerate again, because the internal desired speed must rise again?!
829 : // @ToDo: It can't be done via (*link)->havePriority() (vehicle will not brake for other vehicles, so it needs to brake for curve radius),
830 : // @ToDo: because then turning at traffic lights or somewhere else might be missing (traffic light links don't have priority definition?!)
831 1323341 : LinkDirection dir = (*link)->getDirection();
832 1323341 : switch (dir) {
833 : case LinkDirection::NODIR:
834 : break;
835 : case LinkDirection::STRAIGHT:
836 : break;
837 : case LinkDirection::TURN:
838 : speedlim = 4;
839 : break;
840 : case LinkDirection::TURN_LEFTHAND:
841 : speedlim = 4;
842 : break;
843 58310 : case LinkDirection::LEFT:
844 : speedlim = 8;
845 58310 : break;
846 44551 : case LinkDirection::RIGHT:
847 : speedlim = 6;
848 44551 : break;
849 : case LinkDirection::PARTLEFT:
850 : speedlim = 12;
851 : break;
852 : case LinkDirection::PARTRIGHT:
853 : speedlim = 12;
854 : break;
855 : }
856 :
857 1323341 : if (v0 > speedlim * veh->getChosenSpeedFactor() + NUMERICAL_EPS) {
858 118651 : v0 = speedlim * veh->getChosenSpeedFactor();
859 : }
860 : } else {
861 : break;
862 : }
863 1323341 : if ((*link)->getViaLane() == nullptr) {
864 692084 : ++view;
865 : }
866 : lane = (*link)->getViaLaneOrLane();
867 : // @ToDo: Previously: (seen < oldV*myTpreview / 2 || seen < v0*myTpreview / 4)! Changed freeSpeed, so also changed v0-calculation here.
868 : // @ToDo: Vehicle now decelerates to new Speedlimit before reaching new edge (not /2 anymore)!
869 : // @ToDo: v0 for changing speed limits when seen < oldV*myTpreview, not seen < oldV*myTpreview/2 anymore!!!
870 1323341 : if (v0 > lane->getVehicleMaxSpeed(veh)) {
871 0 : if (!activeGLOSA) {
872 0 : v0 = lane->getVehicleMaxSpeed(veh);
873 : } else {
874 0 : v0 = MIN2(v0, static_cast<MSDevice_GLOSA*>(veh->getDevice(typeid(MSDevice_GLOSA)))->getOriginalSpeedFactor() * lane->getSpeedLimit());
875 : }
876 : }
877 1323341 : seen += lane->getLength();
878 1323341 : link = MSLane::succLinkSec(*veh, view, *lane, bestLaneConts);
879 : }
880 :
881 698814 : 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
882 639067 : (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
883 :
884 59751 : if ((v_limcurr < v_limprev + NUMERICAL_EPS && v_limcurr < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
885 59751 : (v_limcurr > v_limprev - NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS && v_limprev < v0 + NUMERICAL_EPS)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
886 4 : vars->v0_old = v_limcurr;
887 : } else {
888 59747 : if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
889 59747 : v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
890 : } else { // v_limcurr is too low and needs to increase
891 0 : v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
892 : }
893 : }
894 :
895 : // when v_limcurr reaches v0, then update v_limprev=v0
896 59751 : if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
897 1179 : vars->v0_old = v0;
898 1179 : vars->v0_int = v0;
899 : } else { // else just update the internal desired speed with v_limcurr
900 58572 : vars->v0_int = v_limcurr;
901 : }
902 : }
903 :
904 5064583 : } 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
905 5003865 : (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
906 :
907 61053 : if ((v_limcurr < v_limprev + NUMERICAL_EPS && v_limcurr < v0 + NUMERICAL_EPS && v_limprev > v0 - NUMERICAL_EPS) || // important when v_limcurr < v0 < v_limprev --> v_limcurr was decreasing, but needs to suddenly increase again
908 60718 : (v_limcurr > v_limprev - NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS && v_limprev < v0 + NUMERICAL_EPS)) { // important when v_limcurr > v0 > v_limprev --> v_limcurr was increasing, but needs to suddenly decrease again
909 335 : vars->v0_old = v_limcurr;
910 : } else {
911 60718 : if (v_limcurr >= v0 - NUMERICAL_EPS) { // v_limcurr is too high and needs to decrease
912 0 : v_limcurr = MAX2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
913 : } else { // v_limcurr is too low and needs to increase
914 60718 : v_limcurr = MIN2(v0, v_limcurr - (v_limprev - v0) * TS / myTpreview);
915 : }
916 : }
917 :
918 : // when v_limcurr reaches v0, then update v_limprev=v0
919 61053 : if (v_limcurr < v0 + NUMERICAL_EPS && v_limcurr > v0 - NUMERICAL_EPS) {
920 1508 : vars->v0_old = v0;
921 1508 : vars->v0_int = v0;
922 : } else { // else just update the internal desired speed with v_limcurr
923 59545 : vars->v0_int = v_limcurr;
924 : }
925 : }
926 5763397 : }
927 :
928 : double
929 57785916 : MSCFModel_EIDM::_v(const MSVehicle* const veh, const double gap2pred, const double egoSpeed,
930 : const double predSpeed, const double desSpeed, const bool respectMinGap, const int update, const CalcReason usage) const {
931 :
932 : double v0 = MAX2(NUMERICAL_EPS, desSpeed);
933 : VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
934 :
935 : // @ToDo: Where to put such an insertion function/update, which only needs to be calculated once at the first step?????!
936 : // For the first iteration
937 57785916 : if (vars->v0_old == 0) {
938 : vars = (VehicleVariables*)veh->getCarFollowVariables();
939 1763 : vars->v0_old = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
940 1763 : vars->v0_int = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
941 1763 : v0 = MAX2(NUMERICAL_EPS, veh->getLane()->getVehicleMaxSpeed(veh));
942 : }
943 :
944 : double wantedacc = 0., a_free;
945 : double wouldacc = 0., woulds, woulda_free;
946 :
947 : double estSpeed, estleaderSpeed, estGap;
948 : double current_estSpeed, current_estGap, current_estleaderSpeed;
949 : double current_gap;
950 : double acc = 0.;
951 : double a_leader = NUMERICAL_EPS; // Default without a leader, should not be 0!
952 : double newSpeed = egoSpeed;
953 : bool lastrespectMinGap = respectMinGap;
954 57785916 : const double minGapStop_EPS = 0.05 + 0.20 * MAX2(0.25, getCurrentAccel(0.0));
955 :
956 : // 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!).
957 : // For the IDM this Mingap is needed or else the vehicle will stop at two times mingap behind the leader!
958 57785916 : if (respectMinGap) {
959 85802360 : current_gap = MAX2(NUMERICAL_EPS, gap2pred + MAX2(NUMERICAL_EPS, myType->getMinGap() - 0.25)); // 0.25m tolerance because of reaction time and estimated variables
960 : } else {
961 : // gap2pred may go to 0 when offset is reached (e.g. 1m Offset -> gap2pred=0, when vehicle stands at 0.95m, gap2pred is still 0 and does not become -0.05m (negative)!)
962 14884736 : current_gap = MAX2(NUMERICAL_EPS, gap2pred + minGapStop_EPS);
963 : }
964 :
965 : double newGap = current_gap;
966 :
967 231875464 : for (int i = 0; i < myIterations; i++) {
968 :
969 : // Using Action-Point reaction time: update the variables, when myap_update is zero and update is 1
970 : current_estSpeed = newSpeed;
971 174089551 : if (respectMinGap) {
972 130848209 : current_estleaderSpeed = MAX2(predSpeed - newGap * mySigmaleader * vars->myw_speed, 0.0); // estimated variable with Wiener Prozess
973 : } else {
974 : // @ToDo: Use this??? driver would always know, that junctions, traffic lights, etc. have v=0!
975 : // @ToDo: With estimated variables predSpeed > 0 is possible! > 0 may result in oscillating
976 : current_estleaderSpeed = predSpeed;
977 : }
978 174089551 : if (update == 2) { // For freeSpeed
979 : current_estGap = newGap; // not-estimated variable
980 : } else {
981 174010688 : if (respectMinGap) {
982 130769346 : current_estGap = newGap * exp(mySigmagap * vars->myw_gap); // estimated variable with Wiener Prozess
983 : } else {
984 81496148 : current_estGap = newGap * exp(mySigmagap * vars->myw_gap * MIN2(current_estSpeed / EST_REAC_THRESHOLD, 1.0)); // estimated variable with Wiener Prozess
985 : }
986 : }
987 :
988 174089551 : if (vars->myap_update == 0 && usage == CalcReason::CURRENT) { // update variables with current observation
989 : estSpeed = current_estSpeed;
990 : estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
991 : estGap = current_estGap; // estimated variable with Wiener Prozess
992 14977000 : } else if (usage == CalcReason::CURRENT) { // use stored variables (reaction time)
993 7160825 : estSpeed = MAX2(vars->myv_est + vars->lastrealacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations), 0.0);
994 : // estSpeed = vars->myv_est;
995 7160825 : if (update == 2) { // For freeSpeed
996 : estGap = newGap; // not-estimated variable
997 1511 : estleaderSpeed = MAX2(vars->myv_est_l + vars->lastleaderacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations) - vars->mys_est * mySigmaleader * vars->myw_speed, 0.0);
998 : // estleaderSpeed = MAX2(vars->myv_est_l - vars->mys_est * mySigmaleader*vars->myw_speed, 0.0);
999 : } else {
1000 7159314 : lastrespectMinGap = vars->myrespectMinGap;
1001 7159314 : if (lastrespectMinGap) {
1002 6763503 : estleaderSpeed = MAX2(vars->myv_est_l + vars->lastleaderacc * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations) - vars->mys_est * mySigmaleader * vars->myw_speed, 0.0);
1003 : // estleaderSpeed = MAX2(vars->myv_est_l - vars->mys_est * mySigmaleader*vars->myw_speed, 0.0);
1004 6763503 : estGap = vars->mys_est * exp(mySigmagap * vars->myw_gap) - ((vars->myv_est + estSpeed) / 2. - (vars->myv_est_l + estleaderSpeed) / 2.) * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations); // estimated variable with Wiener Prozess
1005 : } else {
1006 : // @ToDo: Use this??? driver would always know, that junctions, traffic lights, etc. have v=0!
1007 : // @ToDo: With estimated variables predSpeed > 0 is possible! > 0 may result in oscillating
1008 395811 : estleaderSpeed = vars->myv_est_l;
1009 541539 : estGap = vars->mys_est * exp(mySigmagap * vars->myw_gap * MIN2(current_estSpeed / EST_REAC_THRESHOLD, 1.0)) - ((vars->myv_est + estSpeed) / 2. - (vars->myv_est_l + estleaderSpeed) / 2.) * (vars->myap_update * TS - TS * (myIterations - i - 1.) / myIterations); // estimated variable with Wiener Prozess
1010 : }
1011 : }
1012 : } else { // use actual variables without reaction time
1013 : estSpeed = current_estSpeed;
1014 : estleaderSpeed = current_estleaderSpeed; // estimated variable with Wiener Prozess
1015 : estGap = current_estGap; // estimated variable with Wiener Prozess
1016 : }
1017 :
1018 : // ToDo: The headway can change for IDMM based on the scenario, should something like that also be integrated here???
1019 174089551 : double headwayTime = myHeadwayTime;
1020 : MSVehicle* leader;
1021 :
1022 174089551 : double Accel_estSpeed = getCurrentAccel(estSpeed);
1023 174089551 : double Accel_current_estSpeed = getCurrentAccel(current_estSpeed);
1024 :
1025 174089551 : double s = MAX2(0., estSpeed * headwayTime + estSpeed * (estSpeed - estleaderSpeed) / (2 * sqrt(Accel_estSpeed * myDecel)));
1026 174089551 : if (lastrespectMinGap) {
1027 130595644 : s += myType->getMinGap() + EIDM_POS_ACC_EPS;
1028 : } else {
1029 43493907 : s += minGapStop_EPS + EIDM_POS_ACC_EPS;
1030 : }
1031 :
1032 : // 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...
1033 : // 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!
1034 174089551 : if (lastrespectMinGap) {
1035 : // The allowed position error when coming to a stop behind a leader is higher with higher timesteps (approx. 0.5m at 1.0s timstep, 0.1m at 0.1s)
1036 130595644 : if (estGap < myType->getMinGap() + (TS * 10 + 1) * EIDM_POS_ACC_EPS && estSpeed < EST_REAC_THRESHOLD && s < estGap * sqrt(1 + 2 * EIDM_POS_ACC_EPS / Accel_estSpeed)) {
1037 3937357 : s = estGap * sqrt(1 + 2 * EIDM_POS_ACC_EPS / Accel_estSpeed);
1038 : }
1039 : } else {
1040 43493907 : if (estGap < minGapStop_EPS + 2 * EIDM_POS_ACC_EPS && s < estGap * sqrt(1 + EIDM_POS_ACC_EPS / Accel_estSpeed)) {
1041 : // when the vehicle wants to stop (stopSpeed), it may take long to come to a full stop
1042 : // To lower this stop time, we restrict the deceleration to always be higher than 0.05m/s^2 when stopping
1043 120228 : s = estGap * sqrt(1 + EIDM_POS_ACC_EPS / Accel_estSpeed);
1044 : }
1045 : }
1046 :
1047 : // IDM calculation:
1048 : // wantedacc = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (estGap * estGap));
1049 :
1050 : // IIDM calculation -NOT- from the original Treiber/Kesting publication:
1051 : // With the saved variables from the last Action Point
1052 : /*double wantedacc;
1053 : double a_free = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta));
1054 : if (s >= estGap) { // This is the IIDM
1055 : wantedacc = Accel_estSpeed * (1. - (s * s) / (estGap * estGap));
1056 : } else {
1057 : wantedacc = a_free * (1. - pow(s / estGap, 2*Accel_estSpeed / fabs(a_free)));
1058 : }*/ // 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!
1059 :
1060 : // IIDM calculation from the original Treiber/Kesting publication:
1061 : // With the saved variables from the last Action Point
1062 174089551 : if (estSpeed <= v0) {
1063 169530513 : a_free = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta));
1064 169530513 : if (s >= estGap) {
1065 11510270 : wantedacc = Accel_estSpeed * (1. - (s * s) / (estGap * estGap));
1066 : } else {
1067 158020243 : wantedacc = a_free * (1. - pow(s / estGap, 2 * Accel_estSpeed / a_free));
1068 : }
1069 : } else { // estSpeed > v0
1070 4559038 : a_free = - myDecel * (1. - pow(v0 / estSpeed, Accel_estSpeed * myDelta / myDecel));
1071 4559038 : if (s >= estGap) {
1072 255658 : wantedacc = a_free + Accel_estSpeed * (1. - (s * s) / (estGap * estGap));
1073 : } else {
1074 : wantedacc = a_free;
1075 : }
1076 : }
1077 174089551 : wantedacc = SPEED2ACCEL(MAX2(0.0, estSpeed + ACCEL2SPEED(wantedacc)) - estSpeed);
1078 :
1079 : // IIDM calculation from the original Treiber/Kesting publication:
1080 : // With the current variables (what would the acceleration be without reaction time)
1081 174089551 : if (usage == CalcReason::CURRENT) {
1082 86164292 : woulds = MAX2(0., current_estSpeed * headwayTime + current_estSpeed * (current_estSpeed - current_estleaderSpeed) / (2 * sqrt(Accel_current_estSpeed * myDecel))); // s_soll
1083 86164292 : if (respectMinGap) {
1084 85970685 : woulds += myType->getMinGap() + EIDM_POS_ACC_EPS; // when behind a vehicle use MinGap and when at a junction then not????
1085 : } else {
1086 193607 : woulds += minGapStop_EPS + EIDM_POS_ACC_EPS;
1087 : }
1088 :
1089 86164292 : if (current_estSpeed <= v0) {
1090 83286678 : woulda_free = Accel_current_estSpeed * (1. - pow(current_estSpeed / v0, myDelta));
1091 83286678 : if (woulds >= current_estGap) {
1092 1807083 : wouldacc = Accel_current_estSpeed * (1. - (woulds * woulds) / (current_estGap * current_estGap));
1093 : } else {
1094 81479595 : wouldacc = woulda_free * (1. - pow(woulds / current_estGap, 2 * Accel_current_estSpeed / woulda_free));
1095 : }
1096 : } else { // current_estSpeed > v0
1097 2877614 : woulda_free = - myDecel * (1. - pow(v0 / current_estSpeed, Accel_current_estSpeed * myDelta / myDecel));
1098 2877614 : if (woulds >= current_estGap) {
1099 45298 : wouldacc = woulda_free + Accel_current_estSpeed * (1. - (woulds * woulds) / (current_estGap * current_estGap));
1100 : } else {
1101 : wouldacc = woulda_free;
1102 : }
1103 : }
1104 171897500 : wouldacc = SPEED2ACCEL(MAX2(0.0, current_estSpeed + ACCEL2SPEED(wouldacc)) - current_estSpeed);
1105 : }
1106 :
1107 : // @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:
1108 : // @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.
1109 : // @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!
1110 :
1111 : // IIDM calculation -NOT- from the original Treiber/Kesting publication:
1112 : // Resulting acceleration also with the correct drive off term.
1113 : double calc_gap = estGap;
1114 : /*a_free = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta));
1115 : if (s >= calc_gap) { // This is the IIDM
1116 : acc = Accel_estSpeed * (1. - (s * s) / (calc_gap * calc_gap));
1117 : } else {
1118 : acc = a_free * (1. - pow(s / calc_gap, 2*Accel_estSpeed / fabs(a_free)));
1119 : } */ // 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!
1120 :
1121 : // IDM calculation:
1122 : // acc = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta) - (s * s) / (calc_gap * calc_gap));
1123 :
1124 : // IIDM calculation from the original Treiber/Kesting publication:
1125 : // Resulting acceleration also with the correct drive off term.
1126 174089551 : if (estSpeed <= v0) {
1127 169530513 : a_free = Accel_estSpeed * (1. - pow(estSpeed / v0, myDelta));
1128 169530513 : if (s >= calc_gap) {
1129 11510270 : acc = Accel_estSpeed * (1. - (s * s) / (calc_gap * calc_gap));
1130 : } else {
1131 158020243 : acc = a_free * (1. - pow(s / calc_gap, 2 * Accel_estSpeed / a_free));
1132 : }
1133 : } else { // estSpeed > v0
1134 4559038 : a_free = - myDecel * (1. - pow(v0 / estSpeed, Accel_estSpeed * myDelta / myDecel));
1135 4559038 : if (s >= calc_gap) {
1136 255658 : acc = a_free + Accel_estSpeed * (1. - (s * s) / (calc_gap * calc_gap));
1137 : } else {
1138 : acc = a_free;
1139 : }
1140 : }
1141 :
1142 : double a_cah;
1143 : // Coolness from Enhanced Intelligent Driver Model, when gap "jump" to a smaller gap accurs
1144 : // @ToDo: Maybe without usage == CalcReason::CURRENT??? To let all calculations profit from Coolness??? (e.g. also lane change calculation)
1145 174089551 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1146 :
1147 76292298 : leader = (MSVehicle*)veh->getLeader(estGap + 25).first;
1148 76292295 : if (leader != nullptr && lastrespectMinGap && estleaderSpeed >= SPEED_EPS) {
1149 72674136 : a_leader = MIN2(leader->getAcceleration(), Accel_estSpeed);
1150 : // Change a_leader to lower values when far away from leader or else far away leaders influence the ego-vehicle!
1151 72674136 : if (estGap > s * 3 / 2) { // maybe estGap > 2*s???
1152 56149391 : a_leader = a_leader * (s * 3 / 2) / estGap;
1153 : }
1154 : }
1155 :
1156 : // speed of the leader shouldnt become zero, because then problems with the calculation occur
1157 76292295 : if (estleaderSpeed < SPEED_EPS) {
1158 : estleaderSpeed = SPEED_EPS;
1159 : }
1160 :
1161 76292295 : if (vars->t_off + myTaccmax + NUMERICAL_EPS < (SIMTIME - TS * (myIterations - i - 1.) / myIterations) && egoSpeed > SUMO_const_haltingSpeed) {
1162 :
1163 : // Enhanced Intelligent Driver Model
1164 72010824 : if (estleaderSpeed * (estSpeed - estleaderSpeed) <= -2 * estGap * a_leader) {
1165 45596544 : a_cah = (estSpeed * estSpeed * a_leader) / (estleaderSpeed * estleaderSpeed - 2 * estGap * a_leader);
1166 : } else {
1167 26414280 : if (estSpeed - estleaderSpeed >= 0) {
1168 21636080 : a_cah = a_leader - ((estSpeed - estleaderSpeed) * (estSpeed - estleaderSpeed)) / (2 * estGap);
1169 : } else {
1170 : a_cah = a_leader;
1171 : }
1172 : }
1173 :
1174 72010824 : if (acc >= a_cah) {
1175 : // do nothing, meaning acc = acc_IDM;
1176 : } else {
1177 4837639 : acc = (1 - myCcoolness) * acc + myCcoolness * (a_cah + myDecel * tanh((acc - a_cah) / myDecel));
1178 : }
1179 : }
1180 : }
1181 :
1182 174089548 : newSpeed = MAX2(0.0, current_estSpeed + ACCEL2SPEED(acc) / myIterations);
1183 :
1184 : // Euler Update as future gap prediction, this will be the "real" gap with this timestep and speed calculation
1185 : // Although this is the correct gap prediction, the calculation is unstable with this method
1186 : //newGap = MAX2(NUMERICAL_EPS, current_gap - SPEED2DIST(newSpeed - predSpeed) * ((i + 1.) / myIterations));
1187 :
1188 : // Ballistic Update as future gap prediction, this will be the "real" gap with this timestep and speed calculation
1189 : // Although this is the correct gap prediction, the calculation is unstable with this method
1190 : //newGap = MAX2(NUMERICAL_EPS, current_gap - SPEED2DIST(MAX2(0.0, current_estSpeed + 0.5 * ACCEL2SPEED(acc) ((i + 1.) / myIterations)) - predSpeed) * ((i + 1.) / myIterations));
1191 :
1192 : // We cannot rely on sub-timesteps, because the here calculated "sub"-gap will not match the "full"-gap calculation of the Euler/Ballistic Update.
1193 : // The "full"-gap is only calculated with the last measured newSpeed, while the "sub"-gap uses all "sub"-newSpeeds
1194 : // Example: In the last iteration-step newSpeed becomes 0. Therefore in the Euler Update, the vehicle does not move for the whole timestep!
1195 : // Example: But in the "sub"-gaps the vehicle may have moved. Therefore, stops can sometimes not be reached
1196 292353459 : newGap = MAX2(NUMERICAL_EPS, newGap - MAX2(0., SPEED2DIST(newSpeed - predSpeed) / myIterations));
1197 : // Ballistic:
1198 : //newGap = MAX2(NUMERICAL_EPS, newGap - MAX2(0., SPEED2DIST(MAX2(0.0, current_estSpeed + 0.5 * ACCEL2SPEED(acc) / myIterations) - predSpeed) / myIterations));
1199 :
1200 : // To always reach stops in high-timestep simulations, we adapt the speed to the actual distance that is covered:
1201 : // This may only be needed for Euler Update...
1202 174089548 : if (myIterations > 1 && newSpeed < EST_REAC_THRESHOLD * TS && !lastrespectMinGap) {
1203 2324373 : newSpeed = MAX2(0.0, predSpeed + DIST2SPEED(current_gap - newGap) * myIterations / (i + 1.));
1204 : }
1205 : }
1206 :
1207 : // The "real" acceleration after iterations
1208 115571826 : acc = SPEED2ACCEL(MIN2(newSpeed, maxNextSpeed(egoSpeed, veh)) - veh->getSpeed());
1209 :
1210 : #ifdef DEBUG_V
1211 : if (veh->isSelected()) {
1212 : std::cout << SIMTIME
1213 : << " EIDM::_v "
1214 : << " veh=" << veh->getID()
1215 : << " vars->minaccel=" << vars->minaccel
1216 : << " vars->myap_update=" << vars->myap_update
1217 : << " vars->myv_est_l=" << vars->myv_est_l
1218 : << " vars->myv_est=" << vars->myv_est
1219 : << " vars->mys_est=" << vars->mys_est
1220 : << " vars->wouldacc=" << vars->wouldacc
1221 : << " vars->realacc=" << vars->realacc
1222 : << "\n";
1223 : std::cout << SIMTIME
1224 : << " EIDM::_v (2) "
1225 : << " veh=" << veh->getID()
1226 : << " newSpeed=" << newSpeed
1227 : << " newGap=" << newGap
1228 : << " predSpeed=" << predSpeed
1229 : << " estSpeed=" << estSpeed
1230 : << " estleaderSpeed=" << estleaderSpeed
1231 : << " estGap=" << estGap
1232 : << " wantedacc=" << wantedacc
1233 : << " wouldacc=" << wouldacc
1234 : << " acc=" << acc
1235 : << "\n";
1236 : }
1237 : #endif
1238 :
1239 : // wantedacc is already calculated at this point. acc may still change (because of coolness and drive off), but the ratio should stay the same!
1240 : // this means when vars->minaccel > wantedacc stands, so should vars->minaccel > acc!
1241 : // When updating at an Action Point, store the observed variables for the next time steps until the next Action Point.
1242 57785913 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && vars->myap_update == 0 && usage == CalcReason::CURRENT) {
1243 18941488 : vars->myv_est_l = predSpeed;
1244 18941488 : vars->myv_est = egoSpeed;
1245 18941488 : if (update == 2) { // For freeSpeed
1246 17461 : vars->mys_est = current_gap + myTreaction * egoSpeed;
1247 : } else {
1248 18924027 : vars->mys_est = current_gap;
1249 : }
1250 18941488 : vars->myrespectMinGap = respectMinGap;
1251 : }
1252 :
1253 39018712 : if (usage == CalcReason::CURRENT && vars->wouldacc > wouldacc) {
1254 1368320 : vars->wouldacc = wouldacc;
1255 : }
1256 :
1257 : // It can happen that wantedacc ist higher than previous calculated wantedacc, BUT acc is lower than prev calculated values!
1258 : // This often occurs because of "coolness"+Iteration and in this case "acc" is set to the previous (higher) calculated value!
1259 57785913 : if (vars->realacc > acc && vars->minaccel <= wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1260 : acc = vars->realacc;
1261 3977 : newSpeed = MAX2(0.0, egoSpeed + ACCEL2SPEED(acc));
1262 : }
1263 :
1264 : // Capture the relevant variables, because it was determined, that this call will result in the acceleration update (vars->minaccel > wantedacc)
1265 57785913 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT) {
1266 26102160 : vars->minaccel = wantedacc;
1267 26102160 : if (vars->realacc > acc) {
1268 5950167 : vars->realacc = acc;
1269 : }
1270 26102160 : vars->realleaderacc = a_leader;
1271 : }
1272 :
1273 : // Save the parameters for a potential update in finalizeSpeed, when _v was called in a stopSpeed-function!!!
1274 57785913 : if (vars->minaccel > wantedacc - NUMERICAL_EPS && usage == CalcReason::CURRENT_WAIT && !respectMinGap) {
1275 9761811 : vars->stop.push_back(std::make_pair(acc, gap2pred));
1276 : }
1277 :
1278 57785913 : return MAX2(0., newSpeed);
1279 : }
1280 :
1281 :
1282 : MSCFModel*
1283 0 : MSCFModel_EIDM::duplicate(const MSVehicleType* vtype) const {
1284 0 : return new MSCFModel_EIDM(vtype);
1285 : }
|
