Exemplo n.º 1
0
void
MSCFModel::applyHeadwayAndSpeedDifferencePerceptionErrors(const MSVehicle* const veh, double speed, double& gap, double& predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
    UNUSED_PARAMETER(speed);
    UNUSED_PARAMETER(predMaxDecel);
    assert(veh->hasDriverState());

    // Obtain perceived gap and headway from the driver state
    const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap, pred);
    const double perceivedSpeedDifference = veh->getDriverState()->getPerceivedSpeedDifference(predSpeed - speed, gap, pred);

#ifdef DEBUG_DRIVER_ERRORS
    if DEBUG_COND {
    if (!veh->getDriverState()->debugLocked()) {
            veh->getDriverState()->lockDebug();
            std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayAndSpeedDifferencePerceptionErrors()\n"
                      << "  speed=" << speed << " gap=" << gap << " leaderSpeed=" << predSpeed
                      << "\n  perceivedGap=" << perceivedGap << " perceivedLeaderSpeed=" << speed + perceivedSpeedDifference
                      << " perceivedSpeedDifference=" << perceivedSpeedDifference
                      << std::endl;
            const double exactFollowSpeed = followSpeed(veh, speed, gap, predSpeed, predMaxDecel);
            const double errorFollowSpeed = followSpeed(veh, speed, perceivedGap, speed + perceivedSpeedDifference, predMaxDecel);
            const double accelError = SPEED2ACCEL(errorFollowSpeed - exactFollowSpeed);
            std::cout << "  gapError=" << perceivedGap - gap << "  dvError=" << perceivedSpeedDifference - (predSpeed - speed)
                      << "\n  resulting accelError: " << accelError << std::endl;
            veh->getDriverState()->unlockDebug();
        }
    }
#endif

    gap = perceivedGap;
    predSpeed = speed + perceivedSpeedDifference;
}
Exemplo n.º 2
0
void
MSCFModel::applyHeadwayPerceptionError(const MSVehicle* const veh, double speed, double& gap) const {
    UNUSED_PARAMETER(speed);
    assert(veh->hasDriverState());

    // Obtain perceived gap from driver state
    const double perceivedGap = veh->getDriverState()->getPerceivedHeadway(gap);

#ifdef DEBUG_DRIVER_ERRORS
    if DEBUG_COND {
    if (!veh->getDriverState()->debugLocked()) {
            veh->getDriverState()->lockDebug();
            std::cout << SIMTIME << " veh '" << veh->getID() << "' -> MSCFModel_Krauss::applyHeadwayPerceptionError()\n"
                      << "  speed=" << speed << " gap=" << gap << "\n  perceivedGap=" << perceivedGap << std::endl;
            const double exactStopSpeed = stopSpeed(veh, speed, gap);
            const double errorStopSpeed = stopSpeed(veh, speed, perceivedGap);
            const double accelError = SPEED2ACCEL(errorStopSpeed - exactStopSpeed);
            std::cout << "  gapError=" << perceivedGap - gap << "\n  resulting accelError: " << accelError << std::endl;
            veh->getDriverState()->unlockDebug();
        }
    }
#endif

    gap = perceivedGap;
}
Exemplo n.º 3
0
SUMOReal
MSCFModel_PWag2009::moveHelper(MSVehicle* const veh, SUMOReal vPos) const {
    const SUMOReal vNext = MSCFModel::moveHelper(veh, vPos);
    VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
    SUMOReal apref = SPEED2ACCEL(vNext - veh->getSpeed());
    vars->aOld = apref;
    return vNext;
}
Exemplo n.º 4
0
// uses the safe speed and preferred acceleration with the same NORMAL tau to compute stopSpeed
SUMOReal
MSCFModel_PWag2009::stopSpeed(const MSVehicle* const /* veh */, const SUMOReal speed, SUMOReal gap) const {
    if (gap < 0.01) {
        return 0;
    }
    const SUMOReal vsafe = -myTauDecel + sqrt(myTauDecel * myTauDecel +  2.0 * myDecel * gap);
    const SUMOReal asafe = SPEED2ACCEL(vsafe - speed);
//    VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
    SUMOReal apref = myDecelDivTau * (gap  - 2 * speed * myHeadwayTime) / (speed + myTauDecel);
    if (apref <= asafe) {
        apref = MIN2(apref, myAccel);
        apref = MAX2(apref, -myDecel);
    } else {
        apref = asafe;
    }
    return MAX2((SUMOReal)0, vsafe + ACCEL2SPEED(apref));
}
Exemplo n.º 5
0
SUMOReal
MSCFModel_KraussOrig1::moveHelper(MSVehicle* const veh, SUMOReal vPos) const {
    const SUMOReal oldV = veh->getSpeed(); // save old v for optional acceleration computation
    const SUMOReal vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
    // we need the acceleration for emission computation;
    //  in this case, we neglect dawdling, nonetheless, using
    //  vSafe does not incorporate speed reduction due to interaction
    //  on lane changing
    veh->setPreDawdleAcceleration(SPEED2ACCEL(vSafe - oldV));
    const SUMOReal vMin = MAX2((SUMOReal) 0, oldV - ACCEL2SPEED(myDecel));
    const SUMOReal vMax = MIN3(veh->getLane()->getMaxSpeed(), maxNextSpeed(oldV), vSafe);
#ifdef _DEBUG
    if (vMin > vMax) {
        WRITE_WARNING("Vehicle's '" + veh->getID() + "' maximum speed is lower than the minimum speed (min: " + toString(vMin) + ", max: " + toString(vMax) + ").");
    }
#endif
    return veh->getLaneChangeModel().patchSpeed(vMin, MAX2(vMin, dawdle(vMax)), vMax, *this);
}
SUMOReal
MSCFModel_KraussOrig1::moveHelper(MSVehicle * const veh, const MSLane * const lane, SUMOReal vPos) const throw() {
    SUMOReal oldV = veh->getSpeed(); // save old v for optional acceleration computation
    SUMOReal vSafe = MIN2(vPos, veh->processNextStop(vPos)); // process stops
    // we need the acceleration for emission computation;
    //  in this case, we neglect dawdling, nonetheless, using
    //  vSafe does not incorporate speed reduction due to interaction
    //  on lane changing
    veh->setPreDawdleAcceleration(SPEED2ACCEL(vSafe-oldV));
    //
    SUMOReal vNext = dawdle(MIN3(lane->getMaxSpeed(), maxNextSpeed(oldV), vSafe));
    vNext =
        veh->getLaneChangeModel().patchSpeed(
            MAX2((SUMOReal) 0, oldV-(SUMOReal)ACCEL2SPEED(myDecel)), //!!! reverify
            vNext,
            MIN3(vSafe, veh->getLane().getMaxSpeed(), maxNextSpeed(oldV)),//vaccel(myState.mySpeed, myLane->maxSpeed())),
            vSafe);
    return MIN4(vNext, vSafe, veh->getLane().getMaxSpeed(), maxNextSpeed(oldV));
}
Exemplo n.º 7
0
SUMOReal
MSCFModel_PWag2009::followSpeed(const MSVehicle* const veh, SUMOReal speed, SUMOReal gap, SUMOReal predSpeed, SUMOReal /*predMaxDecel*/) const {
    if (predSpeed == 0 && gap < 0.01) {
        return 0;
    }
    const SUMOReal vsafe = -myTauLastDecel + sqrt(myTauLastDecel * myTauLastDecel + predSpeed * predSpeed + 2.0 * myDecel * gap);
    const SUMOReal asafe = SPEED2ACCEL(vsafe - speed);
    VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
    SUMOReal apref = vars->aOld;
    if (apref <= asafe && RandHelper::rand() <= myActionPointProbability * TS) {
        apref = myDecelDivTau * (gap + (predSpeed - speed) * myHeadwayTime - speed * myHeadwayTime) / (speed + myTauDecel);
        apref = MIN2(apref, myAccel);
        apref = MAX2(apref, -myDecel);
        apref += myDawdle * RandHelper::rand((SUMOReal) - 1., (SUMOReal)1.);
    }
    if (apref > asafe) {
        apref = asafe;
    }
    return MAX2((SUMOReal)0, speed + ACCEL2SPEED(apref));
}
Exemplo n.º 8
0
/** Returns the SK-vsafe. */
double
MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
    // the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
    // the leaders starts braking hard until stopped
    // unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
    // (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
    // To make things safe, we ensure that the leaders brake distance is computed with an deceleration that is at least as high as the follower's.
    // @todo: this is a conservative estimate for safe speed which could be increased

//    // For negative gaps, we return the lowest meaningful value by convention
//    // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)

//    //      It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below

//    if(gap<0){
//        if(MSGlobals::gSemiImplicitEulerUpdate){
//            return 0.;
//        } else {
//            return -INVALID_SPEED;
//        }
//    }

    // The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
    // if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
    // NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
    //       the follower accelerates for a short period of time. Refs #2310 (Leo)
    //    const double headway = predSpeed > 0. ? myHeadwayTime : 0.;

    const double headway = myHeadwayTime;
    double x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);

    if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
        double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
        if (origSafeDecel > myDecel + NUMERICAL_EPS) {
            // Braking harder than myDecel was requested -> calculate required emergency deceleration.
            // Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
            // 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,
            // such that braking harder than myDecel is required.

#ifdef DEBUG_EMERGENCYDECEL
            if (DEBUG_COND2) {
                std::cout << SIMTIME << " initial vsafe=" << x
                          << " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
                          << " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
                          << std::endl;
            }
#endif

            double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
            // Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
            safeDecel = MAX2(safeDecel, myDecel);
            // don't brake harder than originally planned (possible due to euler/ballistic mismatch)
            safeDecel = MIN2(safeDecel, origSafeDecel);
            x = egoSpeed - ACCEL2SPEED(safeDecel);
            if (MSGlobals::gSemiImplicitEulerUpdate) {
                x = MAX2(x, 0.);
            }

#ifdef DEBUG_EMERGENCYDECEL
            if (DEBUG_COND2) {
                std::cout << "     -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
            }
#endif

        }
    }
    assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
    assert(!ISNAN(x));
    return x;
}
Exemplo n.º 9
0
double
MSCFModel::passingTime(const double lastPos, const double passedPos, const double currentPos, const double lastSpeed, const double currentSpeed) {

    assert(passedPos <= currentPos);
    assert(passedPos >= lastPos);
    assert(currentPos > lastPos);
    assert(currentSpeed >= 0);

    if (passedPos > currentPos || passedPos < lastPos) {
        std::stringstream ss;
        // Debug (Leo)
        if (!MSGlobals::gSemiImplicitEulerUpdate) {
            // NOTE: error is guarded to maintain original test output for euler update (Leo).
            ss << "passingTime(): given argument passedPos = " << passedPos << " doesn't lie within [lastPos, currentPos] = [" << lastPos << ", " << currentPos << "]\nExtrapolating...";
            std::cout << ss.str() << "\n";
            WRITE_ERROR(ss.str());
        }
        const double lastCoveredDist = currentPos - lastPos;
        const double extrapolated = passedPos > currentPos ? TS * (passedPos - lastPos) / lastCoveredDist : TS * (currentPos - passedPos) / lastCoveredDist;
        return extrapolated;
    } else if (currentSpeed < 0) {
        WRITE_ERROR("passingTime(): given argument 'currentSpeed' is negative. This case is not handled yet.");
        return -1;
    }

    const double distanceOldToPassed = passedPos - lastPos; // assert: >=0

    if (MSGlobals::gSemiImplicitEulerUpdate) {
        // euler update (constantly moving with currentSpeed during [0,TS])
        const double t = distanceOldToPassed / currentSpeed;
        return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range

    } else {
        // ballistic update (constant acceleration a during [0,TS], except in case of a stop)

        // determine acceleration
        double a;
        if (currentSpeed > 0) {
            // the acceleration was constant within the last time step
            a = SPEED2ACCEL(currentSpeed - lastSpeed);
        } else {
            // the currentSpeed is zero (the last was not because lastPos<currentPos).
            assert(currentSpeed == 0 && lastSpeed != 0);
            // In general the stop has taken place within the last time step.
            // The acceleration (a<0) is obtained from
            // deltaPos = - lastSpeed^2/(2*a)
            a = lastSpeed * lastSpeed / (2 * (lastPos - currentPos));

            assert(a < 0);
        }

        // determine passing time t
        // we solve distanceOldToPassed = lastSpeed*t + a*t^2/2
        if (fabs(a) < NUMERICAL_EPS) {
            // treat as constant speed within [0, TS]
            const double t = 2 * distanceOldToPassed / (lastSpeed + currentSpeed);
            return MIN2(TS, MAX2(0., t)); //rounding errors could give results out of the admissible result range
        } else if (a > 0) {
            // positive acceleration => only one positive solution
            const double va = lastSpeed / a;
            const double t = -va + sqrt(va * va + 2 * distanceOldToPassed / a);
            assert(t < 1 && t >= 0);
            return t;
        } else {
            // negative acceleration => two positive solutions (pick the smaller one.)
            const double va = lastSpeed / a;
            const double t = -va - sqrt(va * va + 2 * distanceOldToPassed / a);
            assert(t < 1 && t >= 0);
            return t;
        }
    }
}