Feature/bebop estimation

include/bebop_odometry_estimator.h

@@ -0,0 +1,78 @@
+/*
+ * Copyright 2015 Alex Millane, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef BEBOP_ODOMETRY_ESTIMATOR_H
+#define BEBOP_ODOMETRY_ESTIMATOR_H
+
+#include <iostream>
+#include <stdio.h>
+#include <math.h>
+
+#include <Eigen/Geometry>
+#include <ros/ros.h>
+#include <ros_vrpn_client/viconEstimator.h>
+
+#include "vicon_estimator.h"
+
+namespace vicon_estimator {
+
+class ViconOdometryEstimator {
+ public:
+  // Constructor
+  ViconOdometryEstimator(ros::NodeHandle& nh);
+
+  // Initialize the estimator parameters
+  void initializeParameters(ros::NodeHandle& nh);
+  // Reset the estimator
+  void reset();
+  // Publishing the intermediate results
+  void publishIntermediateResults(ros::Time timestamp);
+
+  // Calls the underlying estimator, updating the estimate with the latest
+  // measurement
+  void updateEstimate(const Eigen::Vector3d& position_measured_W,
+                      const Eigen::Quaterniond& orientation_measured_B_W);
+  // Getter methods for estimates values
+  Eigen::Vector3d getEstimatedPosition() const {
+    return vicon_estimator_.getEstimatedPosition();
+  }
+
+  Eigen::Vector3d getEstimatedVelocity() const {
+    return vicon_estimator_.getEstimatedVelocity();
+  }
+
+  Eigen::Quaterniond getEstimatedOrientation() const {
+    return vicon_estimator_.getEstimatedOrientation();
+  }
+
+  Eigen::Vector3d getEstimatedAngularVelocity() const {
+    return vicon_estimator_.getEstimatedAngularVelocity();
+  }
+
+ private:
+  // Underlying estimator
+  vicon_estimator::ViconEstimator vicon_estimator_;
+  // Publisher
+  ros::Publisher publisher_;
+};
+}
+
+#endif  // VICON_ODOMETRY_ESTIMATOR_H

include/vicon_estimator.h

@@ -39,6 +39,9 @@ enum class OutlierRejectionMethod {
 // values
 static const double kDefaultTranslationalKp = 1.0;
 static const double kDefaultTranslationalKv = 10.0;
+static const double kDefaultOutlierThresholdMeters = 0.5;
+static const int kDefaultMaximumOutlierCountTranslation = 10;
+
 static const Eigen::Vector3d kDefaultInitialPositionEstimate =
     Eigen::Vector3d::Zero();
 static const Eigen::Vector3d kDefaultInitialVelocityEstimate =
@@ -51,11 +54,15 @@ class TranslationalEstimatorParameters {
   TranslationalEstimatorParameters()
       : kp_(kDefaultTranslationalKp),
         kv_(kDefaultTranslationalKv),
+        outlier_threshold_meters_(kDefaultOutlierThresholdMeters),
+        maximum_outlier_count_(kDefaultMaximumOutlierCountTranslation),
         initial_position_estimate_(kDefaultInitialPositionEstimate),
         initial_velocity_estimate_(kDefaultInitialVelocityEstimate) {}
 
   double kp_;
   double kv_;
+  double outlier_threshold_meters_;
+  int maximum_outlier_count_;
   Eigen::Vector3d initial_position_estimate_;
   Eigen::Vector3d initial_velocity_estimate_;
 };
@@ -68,13 +75,15 @@ class TranslationalEstimatorResults {
       : position_measured_(Eigen::Vector3d::Zero()),
         position_old_(Eigen::Vector3d::Zero()),
         velocity_old_(Eigen::Vector3d::Zero()),
+        measurement_outlier_flag_(false),
         position_estimate_(Eigen::Vector3d::Zero()),
         velocity_estimate_(Eigen::Vector3d::Zero()) {}
 
   // Intermediate Estimator results
   Eigen::Vector3d position_measured_;
   Eigen::Vector3d position_old_;
   Eigen::Vector3d velocity_old_;
+  bool measurement_outlier_flag_;
   Eigen::Vector3d position_estimate_;
   Eigen::Vector3d velocity_estimate_;
 };
@@ -86,7 +95,7 @@ class TranslationalEstimator {
   // Constructor
   TranslationalEstimator();
   // Update estimated quantities with new measurement
-  EstimatorStatus updateEstimate(const Eigen::Vector3d& pos_measured,
+  EstimatorStatus updateEstimate(const Eigen::Vector3d& pos_measured_W,
                                  const double timestamp);
   // Reset the estimator
   void reset();
@@ -107,22 +116,26 @@ class TranslationalEstimator {
   TranslationalEstimatorParameters estimator_parameters_;
   TranslationalEstimatorResults estimator_results_;
 
+  // Last measurement
+  Eigen::Vector3d pos_measured_old_;
+  bool first_measurement_flag_;
+  int outlier_counter_;
+  double last_timestamp_;
+
   // Estimates
   Eigen::Vector3d position_estimate_W_;
   Eigen::Vector3d velocity_estimate_W_;
 
-  // Last measurement
-  double last_timestamp_;
-  bool first_measurement_flag_;
+  // Detects if the passed measurement is an outlier
+  bool detectMeasurementOutlierSubsequent(const Eigen::Vector3d& pos_measured);
 };
 
 // The parameter class for the translational estimator and parameter default
 // values
-static const double kDefaultLastTimestamp = -1.0;
-static const double kDefaultdOrientationEstimateInitialCovariance = 1.0;
-static const double kDefaultdRateEstimateInitialCovariance = 1.0;
+static const double kDefaultdOrientationEstimateInitialCovariance = 1;
+static const double kDefaultdRateEstimateInitialCovariance = 1;
 static const double kDefaultdOrientationProcessCovariance = 0.01;
-static const double kDefaultdRateProcessCovariance = 1.0;
+static const double kDefaultdRateProcessCovariance = 1;
 static const double kDefaultOrientationMeasurementCovariance = 0.0005;
 static const Eigen::Quaterniond kDefaultInitialOrientationEstimate =
     Eigen::Quaterniond::Identity();
@@ -138,7 +151,7 @@ static const OutlierRejectionMethod kDefaultOutlierRejectionMethod =
 static const double kDefaultOutlierRejectionMahalanobisThreshold = 5.0;
 
 static const double kDefaultOutlierRejectionSubsequentThresholdDegrees = 30.0;
-static const int kDefaultOutlierRejectionSubsequentMaximumCount = 10.0;
+static const int kDefaultOutlierRejectionSubsequentMaximumCountRotation = 10.0;
 
 static const bool kOutputMinimalQuaternions = false;
 
@@ -165,7 +178,7 @@ class RotationalEstimatorParameters {
         outlier_rejection_subsequent_threshold_degrees_(
             kDefaultOutlierRejectionSubsequentThresholdDegrees),
         outlier_rejection_subsequent_maximum_count_(
-            kDefaultOutlierRejectionSubsequentMaximumCount),
+            kDefaultOutlierRejectionSubsequentMaximumCountRotation),
         output_minimal_quaternions_(kOutputMinimalQuaternions){};
 
   double dorientation_estimate_initial_covariance_;

launch/asl_vicon.launch

@@ -1,15 +1,15 @@
 <launch>
-  <arg name="object_name" default="bluebird" />
+  <arg name="object_name" default="bebop" />
   <node ns="$(arg object_name)" name="vrpn_client" type="ros_vrpn_client" pkg="ros_vrpn_client" output="screen">
     <param name="vrpn_server_ip" value="vicon" />
     <param name="vrpn_coordinate_system" value="vicon" />
     <param name="timestamping_system" value="tracker" />
     <param name="object_name" value="$(arg mav_name)" />
     <param name="verbose" value="true" />
     <param name="translational_estimator/kp" value="0.5" />
-    <param name="translational_estimator/kv" value="0.5" />
-    <param name="rotational_estimator/orientation_estimate_initial_covariance" value="100.0" />
-    <param name="rotational_estimator/rate_estimate_initial_covariance" value="100.0" />
+    <param name="translational_estimator/kv" value="4.0" />
+    <param name="rotational_estimator/orientation_estimate_initial_covariance" value="10.0" />
+    <param name="rotational_estimator/rate_estimate_initial_covariance" value="10.0" />
     <param name="rotational_estimator/orientation_process_covariance" value="0.000002" />
     <param name="rotational_estimator/rate_process_covariance" value="10.0" />
     <param name="rotational_estimator/orientation_measurementCovariance" value="0.001" />

launch/bagfile_vicon.launch

@@ -0,0 +1,23 @@
+
+<launch>
+  <arg name="object_name" default="auk" />
+    <!-- Ros Vrpn Estimation node -->
+  <node ns="$(arg object_name)" name="vicon_estimator" type="vicon_estimation_node" pkg="ros_vrpn_client" output="screen">
+      <param name="object_name" value="$(arg object_name)" />
+      <param name="verbose" value="true" />
+      <param name="translational_estimator/kp" value="0.15" />
+      <param name="translational_estimator/kv" value="0.5" />
+      <param name="translational_estimator/outlier_threshold_meters" value="0.25" />
+      <param name="translational_estimator/maximum_outlier_count" value="30" />
+      <param name="rotational_estimator/orientation_estimate_initial_covariance" value="10.0" />
+      <param name="rotational_estimator/rate_estimate_initial_covariance" value="10.0" />
+      <param name="rotational_estimator/orientation_process_covariance" value="0.000002" />
+      <param name="rotational_estimator/rate_process_covariance" value="10.0" />
+      <param name="rotational_estimator/orientation_measurementCovariance" value="0.001" />
+      <param name="rotational_estimator/outlier_rejection_method" value="mahalanobis_distance" />
+      <param name="rotational_estimator/outlier_rejection_mahalanobis_threshold" value="4.0" />
+      <param name="rotational_estimator/outlier_rejection_subsequent_threshold_degrees" value="30.0" />
+      <param name="rotational_estimator/outlier_rejection_subsequent_maximum_count" value="50.0" />
+      <param name="rotational_estimator/output_minimal_quaternions" value="false" />
+    </node>
+</launch>