functionality of Obs #1
Description
// Observation probability O(z | s', a), incorporating the motion uncertainty of obstacle vehicle
double POMDP_Plan::ObsProb(OBS_TYPE obs, const State& state, int action) const{
const POMDP_Plan_State& pomdp_state = static_cast<const POMDP_Plan_State&>(state);
if (pomdp_state.state_A[4] == 0 && goal_prob_[0] <=0.003)
return 1.0;
if (pomdp_state.state_A[4] == 1 && goal_prob_[1] <=0.003)
return 1.0;
if (pomdp_state.state_A[4] == 2 && goal_prob_[2] <=0.003)
return 1.0;
if (pomdp_state.state_A[0] >= 29.7){
if (action == Dec){
if (pomdp_state.state_A[4]==2)
return (obs==MakeObservation(pomdp_state)) ? 0.75 : 0.85;
else
return (obs==MakeObservation(pomdp_state)) ? 0.6 : 0.4;
}
else
return (obs==MakeObservation(pomdp_state)) ? 0.6 : 0.4;
}
else{
if (action!=Dec){
if (pomdp_state.state_A[4]==2)
return (obs==MakeObservation(pomdp_state)) ? 0.0 : 0.00;
else
return (obs==MakeObservation(pomdp_state)) ? 0.9 : 0.6;
}
else
return (obs==MakeObservation(pomdp_state)) ? 0.6 : 0.4;
}
//cout << "Belief: " << pomdp_state.state_A[4];
//return (obs == MakeObservation(pomdp_state)) ? 0.65:0.35;
}
First three if lines:
maintain the lower bound of belief. don't let the belief go to 0. Otherwise, it can not be updated anymore.
The next IF block: (it is messed up)
Original idea: increase the belief of stop intention if action is deceleration. Otherwise, the probability to observe the correct state is 0.6.
I guess I use some dirty hack in the second else structure.
I think the following belief update is enough:
if (action == Dec){
if (pomdp_state.state_A[4]==2)
return (obs==MakeObservation(pomdp_state)) ? 0.75 : 0.85;
else
return (obs==MakeObservation(pomdp_state)) ? 0.6 : 0.4;
}
else
return (obs==MakeObservation(pomdp_state)) ? 0.6 : 0.4;
You can try it in simulation.