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; } }
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))); }
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(); }