//循环点击,
//如果点中雷,则返回0,如果胜利返回1,如果首次点击即为雷,则返回为-1
int loop(){
//随机点击第一个点
int p=rand()%M,q=rand()%N;
//检测是否失败,失败返回-1
if(cnter[p][q]==MINEFLAG)
return -1;
//成功,则处理isClk以及prob
isClk[p][q]=1;
updateProb(p,q);
while(1){
//如果有概率为0,且未被点击的点,则点之
for(p=0;p!=M;p++){
for(q=0;q!=N;q++){
if(isClk[p][q]==0&&(prob[p][q]<1e-4&&prob[p][q]>1e-4)){
isClk[p][q]=1;
updateProb(p,q);
}
}
}
//选择概率最小的点,点之
//检测是否失败,失败则返回0
//检测是否成功,成功则返回1
}
}
void
KalmanDetectionFilter::detectionCallback(const samplereturn_msgs::NamedPointArrayConstPtr& msg)
{
if(msg->header.stamp > last_negative_measurement_)
{
double dt = (msg->header.stamp - last_negative_measurement_).toSec();
if(dt>config_.max_measurement_interval)
{
ROS_DEBUG("Large measurement interval, resetting");
current_filter_.reset();
}
if(current_filter_ && !updated_)
{
ROS_DEBUG("Negative measurement");
current_filter_->predict();
current_filter_->errorCovPre.copyTo(current_filter_->errorCovPost);;
current_filter_->certainty = updateProb(
current_filter_->certainty,
false,
config_.PDgO, config_.PDgo);
}
last_negative_measurement_ = msg->header.stamp;
updated_ = false;
}
for(const auto & np : msg->points)
{
samplereturn_msgs::NamedPoint fp(np);
if(!transformPointToFilter(fp))
continue;
if(!current_filter_)
{
addFilter(np);
updated_ = true;
}
else
{
updated_ |= checkObservation(np);
}
}
checkFilterAges();
publishFilteredDetections();
drawFilterStates();
printFilterState();
}
int Trajectory::step(double maxTime,
const void* pars,
rng::RngStream* rng,
bool adjZero)
{
// get array of next event rates
double nextTime = 0.0;
double transTime = 0.0;
double r = 0.0;
int nextEvent = 0;
int num_events = 0;
double dw = 0.0;
double rate = 0.0;
double unrate = 0.0;
int ret = 0;
const TransitionType* nextTrans;
// set time to EpiState
curState.time = time;
// calculate all rates
double totalRate = model->calculateTransRates(curState,transRates,trueRates);
double totalTrue = std::accumulate(trueRates.begin(),trueRates.end(),0.0);
while (ret <= 0) {
if (totalRate <= 0.0) {
time = maxTime;
ret = 4;
break;
}
// sample next event time
rng->uniform(1,&r);
nextTime = -1.0/totalRate * log(r);
if (time+nextTime < maxTime) {
// sample next event type and calculate rate of the event
nextEvent = model->chooseTransition(rng,transRates);
// get rate of chosen event (transRates is a cumulative array)
rate = transRates[nextEvent] - ((nextEvent>0) ? transRates[nextEvent-1] : 0.0);
// get the appropriate transition
nextTrans = model->getTType(nextEvent);
// - time interval
transTime = time+nextTime - lastEventTime();
// generate the transition
StateTransition st(transTime,*nextTrans,curState,pars,nextEvent,time+nextTime);
// get potential new state (without copying branch information)
EpiState newState;
newState.copyNoBranches(curState);
newState += st.trans;
// get probability of new state
newState.nextTime = nextTime;
dw = nextTrans->applyProb(newState,model->getPars());
if (dw > 0.0 || ! adjZero)
// check if the proposed state is feasible (prob > 0)
{
// if yes,
// add branch transformations
if (model->do_branches) {
curState.nextTime = nextTime;
nextTrans->applyBranch(curState,rng,st,pars);
dw *= exp(st.relprob);
}
// add transition to list
if (store_trans) transitions.push_back(st);
// adapt state
addTransition(st);
// multiply probability
updateProb(dw);
// updateLogProb(st.relprob);
// advance time
time += nextTime;
++num_events;
#ifdef DEBUG
if (ret < 0) {
cout << ascii::cyan << " legal event"
<< " (" << nextEvent << ")."
<< " w = " << dw << ", penalty = " << unrate
<< ", total rate = " << totalRate << "."
<< ascii::end << endl;
}
#endif
ret = 1; // exit loop
} else {
// if no,
// make this transition illegal in this particular step
#ifdef DEBUG
cout << ascii::cyan
<< " illegal event (" << nextEvent << ")."
<< " w = " << dw << ", penalty = " << rate
<< ", total rate = " << totalRate << "."
<< ascii::end << endl;
#endif
// adjust cumulative rates
for (size_t i = nextEvent; i < transRates.size(); ++i) {
transRates[i] -= rate;
}
totalRate = transRates.back();
// condition on not seeing this event
unrate += rate;
// redo loop with modified rates
ret = -1;
}
} else {
#ifdef DEBUG
if (ret < 0) {
cout << "No event. Total rate = " << totalRate << "." << endl;
}
#endif
nextTime = maxTime - time;
time = maxTime;
ret = 2; // exit loop
}
}
if (totalRate < 0.0) {
#ifdef DEBUG
cout << "\033[1;31m";
cout << "Negative rate (" << ret << "): " << totalRate
<< " ES = " << curState;
cout << "\033[0m" << endl;
#endif
return -1;
}
// condition on the illegal events ('prob' is in logarithmic scale)
// 'unrate' is the sum of all the rates that were conditioned on not
// happening
// if (unrate > 0.0)
{
// updateLogProb(-unrate*nextTime);
double dR = totalTrue - totalRate;
#ifdef DEBUG
if (dR != 0) {
cout << " difference in rates: " << totalTrue << " - " << totalRate << endl;
}
#endif
updateLogProb(-dR*nextTime);
}
return ret;
// return num_events;
// return (ret != 2) ? num_events : -2;
}
