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