src/drone.cpp
/*
 * Regular step function.  Simulates transition from state with action and returns observation and reward
*/
Regular step function.  Simulates transition from state with action and returns observation and reward
Regular step function . Simulates transition from state with action and returns observation and reward
bool DRONE::StepNormal(STATE& state, int action,
                        int& observation, double& reward) const
{
    DRONE_STATE& droneState = safe_cast<DRONE_STATE&>(state);
    reward = 0;
    observation = O_NONE;
    double reward_check = -0.5;
    int reward_move = -1;
    int reward_wait = -1;
    double reward_identify = -0.5;
    int wrong_photo = -10;
    int right_photo = 5;
    int terminal_success = 10;
    int terminal_failure = -100;
    int reward_wrongIdentify = -1; 
    int outOfBounds = -100;
    int deadBattery = -100; 
    for(int i=0; i<NumFeatures; i++) {
        if(i < NumCreatures)
            MoveFeature(droneState, i);
        droneState.Features[i].ProbPosition *= (1 - PROB_MOVING);
    }
    if (action < A_LEAVE) 
    {
        COORD pos;
        reward = reward_move;
        switch (action)
        {
            case A_WAIT:
                reward = reward_wait;
                break;
            case COORD::E_EAST:
                if (droneState.AgentPos.X + 1 < Size){
                    droneState.AgentPos.X++;
                }
                else{
                    reward = outOfBounds;
                }
                break;
            case COORD::E_NORTH:
                if (droneState.AgentPos.Y + 1 < Size){
                    droneState.AgentPos.Y++;
                }
                else
                    reward = outOfBounds;
                break;
            case COORD::E_SOUTH:
                if (droneState.AgentPos.Y - 1 >= 0){
                    droneState.AgentPos.Y--;
                }
                else
                    reward = outOfBounds;
                break;
            case COORD::E_WEST:
                if (droneState.AgentPos.X - 1 >= 0){
                    droneState.AgentPos.X--;
                }
                else
                    reward = outOfBounds;
                break;
        }
    }
    if(action == A_LEAVE){
        if(droneState.TargetPhotosTaken >= PhotosNeeded) {
            reward = terminal_success;
        }
        else {
            reward = terminal_failure;
        }
        return true;
    }
    if (action >= E_CHECK && action < E_IDENTIFY)
    {
        int cell = action - E_CHECK;
        assert(cell < NumCells);
        observation = Observe(droneState, cell); 
        int featureN = observation - O_FEATURE;
        if (featureN >= 0) 
        {
            double distance = COORD::EuclideanDistance(droneState.AgentPos, Grid.Coord(cell));
            double efficiency = (1 + pow(2, -distance / HalfEfficiencyDistance)) * 0.5;
            droneState.Features[featureN].ObservedPosition = Grid.Coord(cell);
            droneState.Features[featureN].ProbPosition = efficiency;
        }
        reward += reward_check;
    }
    if(action >= E_IDENTIFY && action < E_PHOTO){
        int feature = action - E_IDENTIFY;
        if(droneState.Features[feature].ObservedPosition != droneState.AgentPos){
            observation = O_NONE;
            reward += reward_wrongIdentify;
            return false;
        }
        if(droneState.Features[feature].Position != droneState.AgentPos){
            observation = O_NONE;
            reward += reward_identify;
            return false;
        }
        if(droneState.Features[feature].ProbTarget == 0 || droneState.Features[feature].ProbTarget == 1){
            droneState.Features[feature].measured++;
            observation = droneState.Features[feature].Target? O_TARGET : O_NOTARGET;
            reward += reward_identify;
            return false;
        }
        observation = Identify(droneState, feature);
        droneState.Features[feature].measured++;
        double efficiency = DRONE::RECOGNITION_RATE;
        if (observation == O_TARGET) {
            droneState.Features[feature].count++;
            droneState.Features[feature].LikelihoodTarget *= efficiency;
            droneState.Features[feature].LikelihoodNotTarget *= 1.0 - efficiency;
        } else {
            droneState.Features[feature].count--;
            droneState.Features[feature].LikelihoodNotTarget *= efficiency;
            droneState.Features[feature].LikelihoodTarget *= 1.0 - efficiency;
        }
        double denom = (0.5 * droneState.Features[feature].LikelihoodTarget) +
                       (0.5 * droneState.Features[feature].LikelihoodNotTarget);
        droneState.Features[feature].ProbTarget = (0.5 * droneState.Features[feature].LikelihoodTarget) / denom;
        if(!droneState.Features[feature].AssumedTarget) {
            if (BinEntropyCheck(droneState.Features[feature].ProbTarget)) {
                if (round(droneState.Features[feature].ProbTarget))
                    droneState.Features[feature].AssumedTarget = true;
                else
                    droneState.Features[feature].AssumedTarget = false;
            }
        }
        reward += reward_identify;
    }
    if (action >= E_PHOTO){
        droneState.PhotosTaken++;
        int feature = action - E_PHOTO;
        if(FeatureAt(droneState, feature, droneState.AgentPos)){
            droneState.Features[feature].numPhotos++; 
            if(droneState.Features[feature].Target){
                droneState.Features[feature].ProbTarget = 1.0;
                droneState.Features[feature].AssumedTarget = true;
                observation = O_TARGET;
                droneState.TargetPhotosTaken++;
                reward += right_photo / droneState.TargetPhotosTaken;
            }
            else{
                droneState.Features[feature].ProbTarget = 0.0;
                droneState.Features[feature].AssumedTarget = false;
                observation = O_NOTARGET;
                droneState.NoTargetPhotosTaken++;
                reward += wrong_photo;
            }
        }
        else
        {
            observation = O_NONE;
            reward += wrong_photo; 
        }
    }
    return false;
}
bool DRONE :: StepNormal ( STATE & state , int action , int & observation , double & reward ) const { DRONE_STATE & droneState = safe_cast < DRONE_STATE & > ( state ) ; reward = 0 ; observation = O_NONE ; double reward_check = -0.5 ; int reward_move = -1 ; int reward_wait = -1 ; double reward_identify = -0.5 ; int wrong_photo = -10 ; int right_photo = 5 ; int terminal_success = 10 ; int terminal_failure = -100 ; int reward_wrongIdentify = -1 ; int outOfBounds = -100 ; int deadBattery = -100 ; for ( int i = 0 ; i < NumFeatures ; i ++ ) { if ( i < NumCreatures ) MoveFeature ( droneState , i ) ; droneState . Features [ i ] . ProbPosition *= ( 1 - PROB_MOVING ) ; } if ( action < A_LEAVE ) { COORD pos ; reward = reward_move ; switch ( action ) { case A_WAIT : reward = reward_wait ; break ; case COORD :: E_EAST : if ( droneState . AgentPos . X + 1 < Size ) { droneState . AgentPos . X ++ ; } else { reward = outOfBounds ; } break ; case COORD :: E_NORTH : if ( droneState . AgentPos . Y + 1 < Size ) { droneState . AgentPos . Y ++ ; } else reward = outOfBounds ; break ; case COORD :: E_SOUTH : if ( droneState . AgentPos . Y - 1 >= 0 ) { droneState . AgentPos . Y -- ; } else reward = outOfBounds ; break ; case COORD :: E_WEST : if ( droneState . AgentPos . X - 1 >= 0 ) { droneState . AgentPos . X -- ; } else reward = outOfBounds ; break ; } } if ( action == A_LEAVE ) { if ( droneState . TargetPhotosTaken >= PhotosNeeded ) { reward = terminal_success ; } else { reward = terminal_failure ; } return true ; } if ( action >= E_CHECK && action < E_IDENTIFY ) { int cell = action - E_CHECK ; assert ( cell < NumCells ) ; observation = Observe ( droneState , cell ) ; int featureN = observation - O_FEATURE ; if ( featureN >= 0 ) { double distance = COORD :: EuclideanDistance ( droneState . AgentPos , Grid . Coord ( cell ) ) ; double efficiency = ( 1 + pow ( 2 , - distance / HalfEfficiencyDistance ) ) * 0.5 ; droneState . Features [ featureN ] . ObservedPosition = Grid . Coord ( cell ) ; droneState . Features [ featureN ] . ProbPosition = efficiency ; } reward += reward_check ; } if ( action >= E_IDENTIFY && action < E_PHOTO ) { int feature = action - E_IDENTIFY ; if ( droneState . Features [ feature ] . ObservedPosition != droneState . AgentPos ) { observation = O_NONE ; reward += reward_wrongIdentify ; return false ; } if ( droneState . Features [ feature ] . Position != droneState . AgentPos ) { observation = O_NONE ; reward += reward_identify ; return false ; } if ( droneState . Features [ feature ] . ProbTarget == 0 || droneState . Features [ feature ] . ProbTarget == 1 ) { droneState . Features [ feature ] . measured ++ ; observation = droneState . Features [ feature ] . Target ? O_TARGET : O_NOTARGET ; reward += reward_identify ; return false ; } observation = Identify ( droneState , feature ) ; droneState . Features [ feature ] . measured ++ ; double efficiency = DRONE :: RECOGNITION_RATE ; if ( observation == O_TARGET ) { droneState . Features [ feature ] . count ++ ; droneState . Features [ feature ] . LikelihoodTarget *= efficiency ; droneState . Features [ feature ] . LikelihoodNotTarget *= 1.0 - efficiency ; } else { droneState . Features [ feature ] . count -- ; droneState . Features [ feature ] . LikelihoodNotTarget *= efficiency ; droneState . Features [ feature ] . LikelihoodTarget *= 1.0 - efficiency ; } double denom = ( 0.5 * droneState . Features [ feature ] . LikelihoodTarget ) + ( 0.5 * droneState . Features [ feature ] . LikelihoodNotTarget ) ; droneState . Features [ feature ] . ProbTarget = ( 0.5 * droneState . Features [ feature ] . LikelihoodTarget ) / denom ; if ( ! droneState . Features [ feature ] . AssumedTarget ) { if ( BinEntropyCheck ( droneState . Features [ feature ] . ProbTarget ) ) { if ( round ( droneState . Features [ feature ] . ProbTarget ) ) droneState . Features [ feature ] . AssumedTarget = true ; else droneState . Features [ feature ] . AssumedTarget = false ; } } reward += reward_identify ; } if ( action >= E_PHOTO ) { droneState . PhotosTaken ++ ; int feature = action - E_PHOTO ; if ( FeatureAt ( droneState , feature , droneState . AgentPos ) ) { droneState . Features [ feature ] . numPhotos ++ ; if ( droneState . Features [ feature ] . Target ) { droneState . Features [ feature ] . ProbTarget = 1.0 ; droneState . Features [ feature ] . AssumedTarget = true ; observation = O_TARGET ; droneState . TargetPhotosTaken ++ ; reward += right_photo / droneState . TargetPhotosTaken ; } else { droneState . Features [ feature ] . ProbTarget = 0.0 ; droneState . Features [ feature ] . AssumedTarget = false ; observation = O_NOTARGET ; droneState . NoTargetPhotosTaken ++ ; reward += wrong_photo ; } } else { observation = O_NONE ; reward += wrong_photo ; } } return false ; }
Regular step function.
Regular step function .
//Define reward distribution ///Punish any general mistakes (eg. identifying a person in a different room) //TODO: implement battery consumption //if(action > NumActions){ //    cout << "Starting step function, action no." << action << endl; //    DisplayAction(action, cout); //     DisplayState(state, cout); //} ///Setup environmental state transition //Move everything (movable) with probability and update their positionProbs //Update the prob of everything, including trees // move & wait // A_WAIT is implicitly represented with reward_move but no action //pos = COORD(droneState.AgentPos.X+1, droneState.AgentPos.Y); // Maze exit //pos = COORD(cellarstate.AgentPos.X, cellarstate.AgentPos.Y+1); //pos = COORD(cellarstate.AgentPos.X, cellarstate.AgentPos.Y-1); //pos = COORD(cellarstate.AgentPos.X-1, cellarstate.AgentPos.Y); //Terminal state // Cell/Tile check: Returns observation (Empty, Creature ID) and if positive updates the feature's position // O_EMPTY, O_FEATURE...O_FEATURE+NumFeatures //droneState.Rooms[cell].measured++; //Observed something /* Determine whether a feature is Big Foot or not.  Updates ProbTarget
    */ //Identify feature //cout << "UCB: Attempting to identify feature " << feature << endl; ///Attempting to ID needs at least an 'idea' of the feature's location ///But if the feature is not really there, it fails (eg.: system cannot find object in picture) ///Identifying a known target has no further effect ///Only succeed if the object really is there //cout << "UCB: Attempting to identify feature " << feature << endl; //DisplayObservation(droneState, observation, cout); //Normalized Recognition rate (0-1) //Update target probability //If entropy is reduced, target status may be assumed //Take photo of feature when standing directly above //Should reward if belief that target is in room is high, unless photo reveals target ///Limit number of pictures? /*if(droneState.PhotosTaken > MaxPhotos){
            observation = O_NONE;
            reward = wrong_photo;
            return false;
        }*/ ///Succeed only if taking a picture directly above a real target //Photo successful ///Option 1: reward photos of assumed targets.  Problematic if info is wrong /*if(droneState.Features[feature].AssumedTarget){
                observation = O_TARGET;
                droneState.TargetPhotosTaken++;
                reward += right_photo / droneState.TargetPhotosTaken;
            }
            else{
                observation = O_NOTARGET;
                droneState.NoTargetPhotosTaken++;
                reward += wrong_photo;
            }*/ ///Option 2: Reward only real targets.  Must reveal their type for consistency //droneState.NoTargetPhotosTaken++; //TODO: also scale these rewards? //cout << "Step done. " << endl; //DisplayObservation(droneState, observation, cout);