Exemple #1
0
void ftrl_proximal::update(SparseVector<float> x, float p, float y)
{
    for (SparseVector<float>::InnerIterator it(x); it; ++it)
    {
        int i = it.index();
        float x_i = it.value();
        float g = (p - y) * x_i;
        float sigma = (sqrt(n_(i) + g * g) - sqrt(n_(i))) / alpha_;
        z_(i) += g - sigma * w_(i);
        n_(i) += g * g;
    }
}
Exemple #2
0
float ftrl_proximal::predict(SparseVector<float> x)
{
    double wTx = 0.0;

    for (SparseVector<float>::InnerIterator it(x); it; ++it)
    {
        int i = it.index();
        int sign = 0;
        if(z_(i) < 0)
            sign =-1;
        else 
            sign = 1;

        if(z_(i) * sign <= lambda1_)
            w_(i) = 0.0;
        else 
            w_(i) = (sign * lambda1_ - z_(i)) / ((beta_ + sqrt(n_(i))) / alpha_ + lambda2_);

        wTx += w_(i) * it.value();
    }

    return 1.0 / (1.0 + exp(-max(min(wTx, 35.0), -35.0)));
}
Exemple #3
0
void threads::localization::update()
{
  
  bool new_datum_available = false;
  ///// BEGIN LOCKED SECTION
  {
    // pop Datum from queue
    std::lock_guard<std::mutex> lock(queue_mutex);
    //printf("is queue empty? size = %d\n", data_queue.size());
    if (!data_queue.empty())
    {
      new_datum_available = true;
      current_datum = data_queue.top();
      data_queue.pop();
      //printf("popped datum from queue, size = %d\n", data_queue.size());      
    }
    else 
    {
      //printf("no datum available\n");
    }
  }
  ///// END LOCKED SECTION

  //if there is a datum to process, continue, else return
  if (!new_datum_available) return;
  
  //printf("Processing a datum of type: %s", current_datum.type_string().c_str());
  //std::cout << " value = " << current_datum.value() << std::endl;
  
  // prediction step
  // calculate dt using current time and datum time stamp    
  double dt = utility::time_tools::dt(t, current_datum.timestamp());
  if (dt < 0)
  {
    printf("WARNING: localization timestep is negative, skipping sensor update\n");
    return;
  }
  
  predict(dt);
  t = current_datum.timestamp();
  
  // convert value std::vector into a column matrix
  std::vector<double> value = current_datum.value();
  if (current_datum.type() == SENSOR_TYPE::GPS)
  {
    containers.heartbeat_gps = 1;

    value.at(0) -= home_x; // use local frame
    value.at(1) -= home_y;
    
    // save gps information for use in gps velocity calculation
    tR = utility::time_tools::dt(t0, t);
    //printf("t = %f seconds\n", tR);
    gps_data_t.push_back(utility::time_tools::dt(t0, current_datum.timestamp()));
    gps_data_x.push_back(value.at(0));
    gps_data_y.push_back(value.at(1));
    // eliminate old gps data
    for (int i = gps_data_t.size()-1; i > -1; i--)
    {
      if (tR - gps_data_t.at(i) > GPS_HISTORY_TIME_WINDOW)
      {
        //printf("gps datum age = %f seconds, deleting it...\n", tR - gps_data_t.at(i));
        gps_data_t.erase(gps_data_t.begin() + i);
        gps_data_x.erase(gps_data_x.begin() + i);
        gps_data_y.erase(gps_data_y.begin() + i);
      }
    }
    // if there are enough gps data remaining, calculate dx/dt and dy/dt
    if (gps_data_t.size() >= GPS_HISTORY_REQUIRED_SIZE)
    {
      //printf("There are at least %d gps data available, calculating velocities...\n", GPS_HISTORY_REQUIRED_SIZE);
      double n, s_t, s_x, s_y, s_tx, s_ty, s_tt, dx_dt, dy_dt;
      n = (double)gps_data_t.size();
      s_t = std::accumulate(gps_data_t.begin(), gps_data_t.end(), 0.0);
      s_x = std::accumulate(gps_data_x.begin(), gps_data_x.end(), 0.0);
      s_y = std::accumulate(gps_data_y.begin(), gps_data_y.end(), 0.0);
      s_tt = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_t.begin(), 0.0);
      s_tx = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_x.begin(), 0.0);        
      s_ty = std::inner_product(gps_data_t.begin(), gps_data_t.end(), gps_data_y.begin(), 0.0);
      dx_dt = (n*s_tx - s_t*s_x)/(n*s_tt - s_t*s_t);
      dy_dt = (n*s_ty - s_t*s_y)/(n*s_tt - s_t*s_t);
      //printf("dx_dt = %f m/s,  dy_dt = %f m/s\n", dx_dt, dy_dt);        
      std::vector<double> velocities = {dx_dt, dy_dt};
      Eigen::MatrixXd covariance(2, 2);
      covariance = Eigen::MatrixXd::Identity(2, 2); 
      Datum datum(SENSOR_TYPE::GPS_VELOCITY, SENSOR_CATEGORY::LOCALIZATION, velocities, covariance);       
      new_sensor_update(datum); // put this gps_velocity datum into the queue
    }      
  }
  
  Eigen::Map<Eigen::MatrixXd> z_(value.data(), value.size(), 1);
  z = z_;
  R = current_datum.covariance();
  setH();
  
  K = P*H.transpose()*(H*P*H.transpose() + R).inverse();
  
  dz = z - H*state;
  if (current_datum.type() == SENSOR_TYPE::COMPASS)
  {
    dz(0, 0) = utility::angle_tools::minimum_difference(dz(0, 0)); // use true angular difference, not algebraic difference
  }
          
  S = H*P*H.transpose();
  
  if (S.determinant() < pow(10.0, -12.0))
  {
    printf("WARNING: innovation covariance is singular for sensor type: %s\n", current_datum.type_string().c_str());
    std::cout << "z = " << z << std::endl;
    std::cout << "H = " << H << std::endl;
    std::cout << "R = " << R << std::endl;
    std::cout << "P = " << P << std::endl;
    std::cout << "K = " << K << std::endl;
    std::cout << "S = " << S << std::endl;
    std::cout << "det(S) = " << S.determinant() << " vs. " << pow(10.0, -6.0) << std::endl;
    return;
  }
  
  double d = sqrt((dz.transpose()*S.inverse()*dz)(0, 0));
  //printf("Mahalonobis distance = %f\n", d);
  
  state += K*dz;
  P = (StateSizedSquareMatrix::Identity() - K*H)*P;        
  //std::cout << "Updated state = " << state.transpose() << std::endl;
  
  updateKB();
}