// ----------------------------------------------------------------------
void
LocalizationMinMaxModule::
work( void )
throw()
{
if ( owner().is_anchor() )
state_ = minmax_finished;
if ( state_ == minmax_finished )
return;
if ( state_ == minmax_wait )
state_ = minmax_work;
shawn::Vec est_pos;
NeighborInfoList neighbors;
collect_neighbors( neighborhood(), lat_anchors, neighbors );
if ( est_pos_min_max( neighbors, est_pos ) )
{
if ( !observer().check_residue() || check_residue( neighbors, est_pos, lat_anchors, observer().comm_range() ) )
node_w().set_est_position( est_pos );
}
state_ = minmax_finished;
}
// ----------------------------------------------------------------------
void
LocalizationDLSModule::
estimate_position( void ) throw(){
shawn::Vec* est_pos;
double distance_r1,distance_r2;
int count =0;
distance_r1 = owner().estimate_distance(node(), *linearization_tool_);
distance_r1 *=distance_r1;
NeighborInfoList neighbors;
collect_neighbors( neighborhood(), lat_anchors, neighbors );
for( std::vector<shawn::Node*>::iterator iter = beacons_->begin(); iter!=beacons_->end();iter++,count++){
for( ConstNeighborInfoListIterator iter1 = neighbors.begin(); iter1!=neighbors.end(); iter1++){
if((*iter)->id() == (*iter1)->node().id() ){
distance_r2 = (*iter1)->distance();
//distance_r2 = owner().estimate_distance(node(), **iter);
if(distance_r2 == std::numeric_limits<double>::max() ||distance_r2 == std::numeric_limits<double>::min())
vector_r_->at(count,0) = 0;
else
vector_r_->at(count,0) = 0.5*( distance_r1 - (distance_r2*distance_r2) + (vector_r_->at(count,0)));
break;
}
}
}
SimpleMatrix<double>* temp = new SimpleMatrix<double>(3,1);
*temp= *matrix_a_ * *vector_r_;
est_pos = new shawn::Vec(temp->at(0,0),temp->at(1,0),temp->at(2,0));
*est_pos += (linearization_tool_->has_est_position())?
(linearization_tool_->est_position()):(linearization_tool_->real_position());
node_w().set_est_position( *est_pos );
delete temp;
}
void lof_storage::update_row(const string& row, const common::sfv_t& diff) {
unordered_set<string> update_set;
{
common::sfv_t query;
nn_engine_->decode_row(row, query);
if (!query.empty()) {
collect_neighbors(row, update_set);
}
}
nn_engine_->update_row(row, diff);
collect_neighbors(row, update_set);
update_set.insert(row);
update_entries(update_set);
}
// ----------------------------------------------------------------------
void
LocalizationIterLaterationModule::
iter_lateration_step( void )
throw()
{
if ( state_ == il_finished || !sound_ )
return;
if ( iteration_cnt_ >= iteration_limit_ )
state_ = il_finished;
++iteration_cnt_;
neighborhood_w().reassign_twins( twin_measure_ * observer().comm_range() );
if ( neighborhood().confident_neighbor_cnt() < min_confident_nbrs_ )
return;
Vec est_pos;
NeighborInfoList neighbors;
collect_neighbors( neighborhood(), lat_confident, neighbors );
// try to update position. if lateration fails, confidence is set to 0,
// else position is updated and confidence is set to average of all
// neighbor confidences
if ( est_pos_lateration( neighbors, est_pos, lat_confident, false ) &&
est_pos_lateration( neighbors, est_pos, lat_confident, true ) )
{
set_confidence( neighborhood().avg_neighbor_confidence() );
// check validity of new position. if check fails, there is a chance
// of 'res_acceptance_', which is normally set to 0.1, to accept
// the position anyway. this is done to avoid being trapped in a
// local minimum. moreover, if the bad position is accepted, the
// confidence is reduced by 50%.
if ( !check_residue( neighbors, est_pos, lat_confident, observer().comm_range() ) )
{
if ( res_acceptance_ > uniform_random_0i_1i() )
set_confidence( observer().confidence() / 2 );
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
return;
}
}
// check, whether the new position is very close to the old one or
// not. if so, refinement is finished.
if ( node().has_est_position() )
{
double pos_diff = euclidean_distance( est_pos, node().est_position() );
if ( pos_diff < abort_pos_update_ * observer().comm_range() )
state_ = il_finished;
}
node_w().set_est_position( est_pos );
}
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
}
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
}
