예제 #1
0
/**
 * Dumps a serie of points into its own GPX file.
 */
void dump_points(char * input_fname, int gpx_index, points_result rs) {
  char out_f[255];
  int i;
  snprintf(out_f, 255, "%s_%02d.gpx", input_fname, gpx_index);
  printf("dumping trace into its own file (%s) ...\n", out_f);
  FILE * out_gpx = fopen(out_f, "w");

  fprintf(out_gpx, "<gpx version=\"1.0\" xmlns=\"%s\" creator=\"gpx_sanitizer\">\n  <trk>\n    <trkseg>\n", GPX_NS);
  for (i = 0; i < rs.size ; i++) {
    if (i > 0) {
      point p1 = rs.points[i-1], p2 = rs.points[i];
      float dist_between_pts = calculate_dist(p1.lat, p1.lon, p2.lat, p2.lon);
      // Cuts the segment in 2 if necessary
      if (dist_between_pts > DISTANCE_THRESHOLD) {
        fprintf(out_gpx, "    </trkseg>\n    <trkseg>\n");
      }
      fprintf(out_gpx, "      <trkpt lat=\"%f\" lon=\"%f\" />\n", p2.lat, p2.lon);
    }
  }
  fprintf(out_gpx, "      </trkseg>\n  </trk>\n</gpx>\n");
  fclose(out_gpx);
  return;
}
예제 #2
0
파일: FittedRMax.cpp 프로젝트: curranw/Core
void FittedRMax::update(QUpdate update)
{
    state_to_ind = VI->get_state_to_ind();
    static int it = 0;
    it++;
    if(update.reward == 0)
    {
        cout << "HI" << endl;
    }
    //cout << "Cur It: " << it << endl;
    //cout << "Cur State: " << update.state[0] << "," << update.state[1] << endl;//"," << update.state[2] << "," << update.state[3] << endl;
    //cout << "Cur Action: " << update.action << endl;
    for(map<QElement::State,int>::iterator state_it = state_to_ind->begin(); state_it != state_to_ind->end(); state_it++)
    {
        int cur_ind_update = state_it->second;
        QElement::State cur_state_update = state_it->first;
        //cout << "Cur Ind: " << cur_ind_update << endl;
        for(unsigned int a_update = 0; a_update < m_possible_actions.size(); a_update++)
        {
            int cur_sa_update = cur_ind_update * m_possible_actions.size() + a_update;
            double weight = calculate_dist(update.state, update.action, cur_state_update, a_update);
            if(weight < .001) continue;
            double alpha;
            if(weight < .00001 && N_estimate[cur_sa_update] == 0) continue;
            else alpha = weight/(N_estimate[cur_sa_update] + weight);
            //cout << "Alpha:" << alpha << endl;
            N_estimate[cur_sa_update] += weight;
            R_estimate[cur_sa_update] = (1 - alpha) * R_estimate[cur_sa_update] + alpha * update.reward;
            updated_states.insert(cur_ind_update);
            QElement::State action_effect = action_effect_function(update.state, update.next_state, cur_state_update);
            //action_effect = update.next_state;
            for(unsigned int af = 0; af < action_effect.size(); af++)
            {
                if(action_effect[af] > m_max_ranges[af] || action_effect[af] < m_min_ranges[af])
                {
                    action_effect = update.next_state;
                    break;
                }
            }
            //cout << "Action Effect: " << action_effect[0] << "," << action_effect[1] << endl;

            vector<QElement::State> coeffs_states;
            map<int, double> coeffs = calculate_mlinterp(action_effect);


            double sum = 0;

//            for(vector<QElement::State>::iterator it = coeffs_states.begin(); it != coeffs_states.end(); it++)
//            {
//                double weight2 = calculate_dist(*it, 1, action_effect, 1);
//                cout << weight2 << endl;
//            }
            double sum_offset = 0;
            int cur_sa = cur_ind_update * m_possible_actions.size() + update.action;
            for(map<int, double>::iterator it = coeffs.begin(); it != coeffs.end(); it++)
            {
                int ind = it->first;
                double coeff = it->second;
                if(coeff < 0.001) continue;
                sum += coeff;
//                for(map<QElement::State,int>::iterator blah = state_to_ind->begin(); blah != state_to_ind->end(); blah++)
//                {
//                    if(blah->second == ind)
//                    {
//                        cout << blah->first[0] << "," << blah->first[1] << endl;
//                    }
//                }
//                cout << ind << "," << coeff << endl;
                if(coeff > 1)
                {
                    cout << "Coeff > 1" << endl;
                    exit(1);
                }

                P_estimate(cur_sa, ind) = (1 - alpha) * P_estimate(cur_sa, ind) + alpha * coeff;
                double orig_val = P_estimate(cur_sa, ind);
//                cout << P_estimate(cur_sa, ind) << endl;
                P_estimate(cur_sa, ind) = 0.01 * round( P_estimate(cur_sa, ind) * 100.0 );
//                cout << P_estimate(cur_sa, ind) << endl;
                sum_offset += abs(orig_val - P_estimate(cur_sa, ind));
            }
            P_estimate.row(cur_sa) = P_estimate.row(cur_sa)/arma::sum(P_estimate.row(cur_sa));
            if(abs(sum - 1) > 0.01)
            {
                cout << "MLINTERP BROKE!!!" << endl;
                cout << sum << endl;
                cout << "Action Effect: " << endl;
                for(unsigned int i = 0; i < action_effect.size(); i++)
                {
                    cout << action_effect[i] << endl;
                }
                map<int, double> coeffs = calculate_mlinterp(action_effect);
                exit(1);
            }
            //P_estimate.print();
            //cout << endl;
        }
    }

    //VI->update_model(R_estimate, P_estimate);alpha = 0

    //VI->calculate_solution();

}