[Positioner] Add basic filtering

The new configuration option positioning.filter.max-speed allows to
specify the maximum speed at which the mobile terminals can travel; the
distance between two results for a given mobile will be limited to the
distance that it could have travelled, according to the two requests'
timestamps.

owlps-positioner/cfg/owlps-positioner.conf

@@ -250,6 +250,13 @@ mobile-csv-file = /usr/local/etc/owlps/mobiles.csv
 # The default is false.
 #ignore-cp-reference-points = false
 
+[positioning.filter]
+# This subsection contains filtering-related options.
+
+# Maximal speed at which the mobile terminal can move, in km/h.
+# This option also controls the activation of the filter (0 = disabled).
+#max-speed = 0
+
 [output]
 # The following options are related to the output of the results.
 

owlps-positioner/positioning.cc

@@ -128,17 +128,24 @@ void Positioning::loop()
 {
   vector<PositioningAlgorithm*>::const_iterator algo ;
 
+  float filter_max_speed =
+    Configuration::float_value("positioning.filter.max-speed") ;
+
   while (! input.eof() && owl_run)
     {
       const Request &request = input.get_next_request() ;
       if (! request)
         continue ;
 
+      const Mobile *const mobile = request.get_mobile() ;
+
       Point3D real_position ;
       bool compute_error = false ;
       ResultList results(&request) ;
       for (algo = algorithms.begin() ; algo != algorithms.end() ; ++algo)
         {
+          const string &algo_name = (*algo)->get_name() ;
+
           Result res ;
 
           try
@@ -149,14 +156,52 @@ void Positioning::loop()
             {
               cerr
                   << "Cannot compute with algorithm "
-                  << (*algo)->get_name() << ": \""
+                  << algo_name << ": \""
                   << e.what() << "\"\n" ;
               continue ;
             }
 
+          // Apply filter if the filter is enabled and that it is not the
+          // first result we compute for this mobile
+          if (filter_max_speed > 0
+              && ! mobile->get_last_results().empty()
+              && algo_name != "Real") // don't filter the real position
+            {
+              const ResultList &last_results = mobile->get_last_results() ;
+              const Result &prev_res =
+                last_results.get_result_for_algo(algo_name) ;
+
+              // Calculate the travel time (duration)
+              const Timestamp &timestamp =
+                res.get_request()->get_time_sent() ;
+              const Timestamp &prev_timestamp =
+                last_results.get_request()->get_time_sent() ;
+              Timestamp duration = prev_timestamp.elapsed(timestamp) ;
+
+              // Get the current and previous coordinates
+              const Point3D &pos = res.get_position() ;
+              const Point3D &prev_pos = prev_res.get_position() ;
+
+              // Recompute the result by interpolation
+              Point3D filtered_pos(
+                prev_pos.interpolate(pos, filter_max_speed, duration)) ;
+              if (filtered_pos != pos)
+                {
+                  if (Configuration::is_configured("verbose"))
+                    cerr
+                        << "Filtering (algorithm " << algo_name << "): "
+                        << "for request sent at " << timestamp
+                        << " by mobile " << mobile->get_mac_addr()
+                        << ", " << pos
+                        << " was replaced by " << filtered_pos
+                        << ".\n" ;
+                  res.set_position(filtered_pos) ;
+                }
+            }
+
           if (compute_error)
             res.compute_error(real_position) ;
-          else if ((*algo)->get_name() == "Real")
+          else if (algo_name == "Real")
             {
               compute_error = true ;
               real_position = res.get_position() ;
@@ -165,6 +210,11 @@ void Positioning::loop()
           results.add(res) ;
         }
 
+      // If filtering is enabled, update the last known position(s) of
+      // the current mobile
+      if (filter_max_speed > 0)
+        const_cast<Mobile*>(mobile)->set_last_results(results) ;
+
       output.write(results) ;
     }
 }

owlps-positioner/userinterface.cc

@@ -328,6 +328,10 @@ void UserInterface::fill_positioning_options()
    po::value<bool>()->default_value(false),
    "With the NSS algorithm, try to avoid selecting the reference"
    " points which are coordinates of a CP.")
+  ("positioning.filter.max-speed",
+   po::value<float>()->default_value(0),
+   "Maximal speed at which the mobiles can move, in km/h (0 = unlimited"
+   " speed).")
   ;
 
   file_options->add(options) ;