public Map<State_Ctrl, Double> getTransitionStatesAndProbs(State_Ctrl cstate, int actionCode) {
Map<State_Ctrl, Double> TransitionStatesAndProbs = new LinkedHashMap<State_Ctrl, Double>();
double targetV = Utils.getActionV(actionCode);
double accel = Utils.getActionA(actionCode);
ArrayList<AbstractMap.SimpleEntry<State_Ctrl, Double>> nextStateMapProbabilities =
new ArrayList<>();
if ((accel > 0 && targetV > cstate.getoVy() && cstate.getoVy() < UPPER_VY)
|| (accel < 0
&& targetV < cstate.getoVy()
&& cstate.getoVy() > -UPPER_VY)) { // own aircraft follows a RA other than COC
for (ThreeTuple<Double, Double, Double> sigmaPoint : sigmaPointsA) {
double oAy = accel;
double iAy = sigmaPoint.x2;
double sigmaP = sigmaPoint.x3;
double hP = cstate.getH() + (cstate.getiVy() - cstate.getoVy()) + 0.5 * (iAy - oAy);
double oVyP = Math.max(-UPPER_VY, Math.min(UPPER_VY, cstate.getoVy() + oAy));
double iVyP = Math.max(-UPPER_VY, Math.min(UPPER_VY, cstate.getiVy() + iAy));
int raP = actionCode;
int hIdxL = (int) Math.floor(hP / hRes);
int oVyIdxL = (int) Math.floor(oVyP / oVRes);
int iVyIdxL = (int) Math.floor(iVyP / iVRes);
for (int i = 0; i <= 1; i++) {
int hIdx = (i == 0 ? hIdxL : hIdxL + 1);
int hIdxP = hIdx < -nh ? -nh : (hIdx > nh ? nh : hIdx);
for (int j = 0; j <= 1; j++) {
int oVzIdx = (j == 0 ? oVyIdxL : oVyIdxL + 1);
int oVzIdxP = oVzIdx < -noVy ? -noVy : (oVzIdx > noVy ? noVy : oVzIdx);
for (int k = 0; k <= 1; k++) {
int iVzIdx = (k == 0 ? iVyIdxL : iVyIdxL + 1);
int iVzIdxP = iVzIdx < -niVy ? -niVy : (iVzIdx > niVy ? niVy : iVzIdx);
State_Ctrl nextState = new State_Ctrl(hIdxP, oVzIdxP, iVzIdxP, raP);
double probability =
sigmaP
* (1 - Math.abs(hIdx - hP / hRes))
* (1 - Math.abs(oVzIdx - oVyP / oVRes))
* (1 - Math.abs(iVzIdx - iVyP / iVRes));
nextStateMapProbabilities.add(
new SimpleEntry<State_Ctrl, Double>(nextState, probability));
}
}
}
}
} else {
for (ThreeTuple<Double, Double, Double> sigmaPoint : sigmaPointsB) {
double oAy = sigmaPoint.x1;
double iAy = sigmaPoint.x2;
double sigmaP = sigmaPoint.x3;
double hP = cstate.getH() + (cstate.getiVy() - cstate.getoVy()) + 0.5 * (iAy - oAy);
double oVyP = Math.max(-UPPER_VY, Math.min(UPPER_VY, cstate.getoVy() + oAy));
double iVyP = Math.max(-UPPER_VY, Math.min(UPPER_VY, cstate.getiVy() + iAy));
int raP = actionCode;
int hIdxL = (int) Math.floor(hP / hRes);
int oVyIdxL = (int) Math.floor(oVyP / oVRes);
int iVyIdxL = (int) Math.floor(iVyP / iVRes);
for (int i = 0; i <= 1; i++) {
int hIdx = (i == 0 ? hIdxL : hIdxL + 1);
int hIdxP = hIdx < -nh ? -nh : (hIdx > nh ? nh : hIdx);
for (int j = 0; j <= 1; j++) {
int oVyIdx = (j == 0 ? oVyIdxL : oVyIdxL + 1);
int oVyIdxP = oVyIdx < -noVy ? -noVy : (oVyIdx > noVy ? noVy : oVyIdx);
for (int k = 0; k <= 1; k++) {
int iVyIdx = (k == 0 ? iVyIdxL : iVyIdxL + 1);
int iVyIdxP = iVyIdx < -niVy ? -niVy : (iVyIdx > niVy ? niVy : iVyIdx);
State_Ctrl nextState = new State_Ctrl(hIdxP, oVyIdxP, iVyIdxP, raP);
double probability =
sigmaP
* (1 - Math.abs(hIdx - hP / hRes))
* (1 - Math.abs(oVyIdx - oVyP / oVRes))
* (1 - Math.abs(iVyIdx - iVyP / iVRes));
nextStateMapProbabilities.add(
new SimpleEntry<State_Ctrl, Double>(nextState, probability));
}
}
}
}
}
for (AbstractMap.SimpleEntry<State_Ctrl, Double> nextStateMapProb : nextStateMapProbabilities) {
State_Ctrl nextState = nextStateMapProb.getKey();
if (TransitionStatesAndProbs.containsKey(nextState)) {
TransitionStatesAndProbs.put(
nextState, TransitionStatesAndProbs.get(nextState) + nextStateMapProb.getValue());
} else {
TransitionStatesAndProbs.put(nextState, nextStateMapProb.getValue());
}
}
return TransitionStatesAndProbs;
}
