diff --git a/include/bebop_odometry_estimator.h b/include/bebop_odometry_estimator.h new file mode 100644 index 0000000..bc59a94 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +#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 diff --git a/include/vicon_estimator.h b/include/vicon_estimator.h index 0402741..b6bfac8 100644 --- a/include/vicon_estimator.h +++ b/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,6 +75,7 @@ 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()) {} @@ -75,6 +83,7 @@ class TranslationalEstimatorResults { 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_; diff --git a/launch/asl_vicon.launch b/launch/asl_vicon.launch index 38e0253..784dfaf 100644 --- a/launch/asl_vicon.launch +++ b/launch/asl_vicon.launch @@ -1,5 +1,5 @@ - + @@ -7,9 +7,9 @@ - - - + + + diff --git a/launch/bagfile_vicon.launch b/launch/bagfile_vicon.launch new file mode 100644 index 0000000..b560e58 --- /dev/null +++ b/launch/bagfile_vicon.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/library/vicon_estimator.cpp b/src/library/vicon_estimator.cpp index 3370119..6c8b330 100644 --- a/src/library/vicon_estimator.cpp +++ b/src/library/vicon_estimator.cpp @@ -96,6 +96,7 @@ EstimatorStatus TranslationalEstimator::updateEstimate( if (first_measurement_flag_) { first_measurement_flag_ = false; last_timestamp_ = timestamp; + pos_measured_old_ = pos_measured_W; position_estimate_W_ = pos_measured_W; return EstimatorStatus::OK; } @@ -103,38 +104,58 @@ EstimatorStatus TranslationalEstimator::updateEstimate( double dt = timestamp - last_timestamp_; last_timestamp_ = timestamp; - // Saving the measurement to the intermediate results - estimator_results_.position_measured_ = pos_measured_W; - // Saving the old state to the intermediate results - estimator_results_.position_old_ = position_estimate_W_; - estimator_results_.velocity_old_ = velocity_estimate_W_; - // Constructing the full state - Eigen::Matrix x_estimate; - x_estimate << position_estimate_W_, velocity_estimate_W_; - // Constructing the system matrix - Eigen::Matrix A; - A.setZero(); - A.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - A.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity(); - A.block<3, 3>(0, 3) = dt * Eigen::Matrix3d::Identity(); - // Constructing the measurement matrix - Eigen::Matrix C; - C.setZero(); - C.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - // Constructing the Luenberger gain matrix - Eigen::Matrix L_gain; - L_gain << estimator_parameters_.kp_ * Eigen::Matrix3d::Identity(), - estimator_parameters_.kv_ * Eigen::Matrix3d::Identity(); - // Correction using the Luenberger equations + gain - x_estimate = (A - L_gain * C * A) * x_estimate + L_gain * pos_measured_W; - // Extracting state components - position_estimate_W_ = x_estimate.block<3, 1>(0, 0); - velocity_estimate_W_ = x_estimate.block<3, 1>(3, 0); - // Saving estimate to intermediate results - estimator_results_.position_estimate_ = position_estimate_W_; - estimator_results_.velocity_estimate_ = velocity_estimate_W_; - // TODO(alexmillane): No outlier rejection yet implemented - return EstimatorStatus::OK; + // Detecting outlier measurements + bool measurement_update_flag; + if (detectMeasurementOutlierSubsequent(pos_measured_W)) { + measurement_update_flag = false; + } else { + measurement_update_flag = true; + } + + // If no outlier detected do measurement update + if (measurement_update_flag) { + // Saving the measurement to the intermediate results + estimator_results_.position_measured_ = pos_measured_W; + // Saving the old state to the intermediate results + estimator_results_.position_old_ = position_estimate_W_; + estimator_results_.velocity_old_ = velocity_estimate_W_; + // Constructing the full state + Eigen::Matrix x_estimate; + x_estimate << position_estimate_W_, velocity_estimate_W_; + // Constructing the system matrix + Eigen::Matrix A; + A.setZero(); + A.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + A.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity(); + A.block<3, 3>(0, 3) = dt * Eigen::Matrix3d::Identity(); + // Constructing the measurement matrix + Eigen::Matrix C; + C.setZero(); + C.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + // Constructing the Luenberger gain matrix + Eigen::Matrix L_gain; + L_gain << estimator_parameters_.kp_ * Eigen::Matrix3d::Identity(), + estimator_parameters_.kv_ * Eigen::Matrix3d::Identity(); + // Correction using the Luenberger equations + gain + x_estimate = (A - L_gain * C * A) * x_estimate + L_gain * pos_measured_W; + // Extracting state components + position_estimate_W_ = x_estimate.block<3, 1>(0, 0); + velocity_estimate_W_ = x_estimate.block<3, 1>(3, 0); + // Saving estimate to intermediate results + estimator_results_.position_estimate_ = position_estimate_W_; + estimator_results_.velocity_estimate_ = velocity_estimate_W_; + // Returning status + return EstimatorStatus::OK; + } + // If outlier detected just write priori estimate to posteriori + else { + // Global state correction (combining priori global state estimate with + // priori error state estimate) + estimator_results_.position_estimate_ = estimator_results_.position_old_; + estimator_results_.velocity_estimate_ = estimator_results_.velocity_old_; + // Return + return EstimatorStatus::OUTLIER; + } } void TranslationalEstimator::reset() { @@ -151,6 +172,43 @@ void TranslationalEstimator::setParameters( estimator_parameters_ = estimator_parameters; } +bool TranslationalEstimator::detectMeasurementOutlierSubsequent( + const Eigen::Vector3d& pos_measured) { + // Performing some initialization if this is the first measurement. + // Assuming first measurement valid, saving it and returning. + if (first_measurement_flag_) { + first_measurement_flag_ = false; + pos_measured_old_ = pos_measured; + return false; + } + + // Compare new measurement with old measuerement and constructing error vector + Eigen::Vector3d error = (pos_measured_old_ - pos_measured); + // Detecting if the measurement is an outlier. If the + bool measurement_outlier_flag = + (error.norm() >= estimator_parameters_.outlier_threshold_meters_); + + // After a certain number of measurements have been ignored in a row + // we assume we've made a mistake and accept the measurement as valid. + if (outlier_counter_ >= estimator_parameters_.maximum_outlier_count_) { + measurement_outlier_flag = false; + } + + // Saving the flag to the intermediate results structure + estimator_results_.measurement_outlier_flag_ = measurement_outlier_flag; + + // If rotation too great indicate that measurement is corrupted + if (measurement_outlier_flag) { + ++outlier_counter_; + return true; + } else { + // If measurement valid. Overwriting the old measurement. + pos_measured_old_ = pos_measured; + outlier_counter_ = 0; + return false; + } +} + /* * -------------------------------------------------------------------- * Rotational Estimator @@ -443,9 +501,11 @@ void RotationalEstimator::updateEstimateUpdateErrorEstimate( skewMatrix(orientation_estimate_priori_vector), -orientation_estimate_priori.vec().transpose(); H << Hdq, Eigen::Matrix::Zero(); + // Calculating the measured error quaternion Eigen::Quaterniond error_orientation = orientation_measured * orientation_estimate_priori.inverse(); + // Calculating the predicted measurement dependant on the sign of the measured // error quaternion Eigen::Quaterniond orientation_predicted; @@ -464,10 +524,12 @@ void RotationalEstimator::updateEstimateUpdateErrorEstimate( Eigen::Quaterniond(Hdq * dorientation_estimate_priori + orientation_estimate_priori.coeffs()); } + // Calculating the measurement residual Eigen::Vector4d measurement_residual; measurement_residual = orientation_measured.coeffs() - orientation_predicted.coeffs(); + // Computing the Kalman gain Eigen::Matrix S = H * covariance_priori * H.transpose() + measurement_covariance_; @@ -499,6 +561,7 @@ void RotationalEstimator::updateEstimateRecombineErrorGlobal( dx_measurement->block<3, 1>(0, 0); Eigen::Matrix drate_estimate_measurement = dx_measurement->block<3, 1>(3, 0); + // Completing the error quaternion // The sign real part of the full quaternion was calculated in the error // quaternion measurement update step @@ -514,6 +577,7 @@ void RotationalEstimator::updateEstimateRecombineErrorGlobal( dorientation_estimate_measurement.y(), dorientation_estimate_measurement.z()); } + // Using estimated error states to correct global estimate states Eigen::Quaterniond orientation_estimate_measurement = orientation_estimate_priori * diff --git a/src/ros_vrpn_client.cpp b/src/ros_vrpn_client.cpp index 3f8110a..11450d8 100644 --- a/src/ros_vrpn_client.cpp +++ b/src/ros_vrpn_client.cpp @@ -341,7 +341,7 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) { tf::quaternionEigenToMsg( orientation_estimate_B_W, target_state->estimated_odometry.pose.pose.orientation); - tf::vectorEigenToMsg(velocity_estimate_B, + tf::vectorEigenToMsg(velocity_estimate_W, target_state->estimated_odometry.twist.twist.linear); tf::vectorEigenToMsg(rate_estimate_B, target_state->estimated_odometry.twist.twist.angular); diff --git a/src/wrapper/vicon_odometry_estimator.cpp b/src/wrapper/vicon_odometry_estimator.cpp index 0f4f9a0..090c634 100644 --- a/src/wrapper/vicon_odometry_estimator.cpp +++ b/src/wrapper/vicon_odometry_estimator.cpp @@ -41,6 +41,11 @@ void ViconOdometryEstimator::initializeParameters(ros::NodeHandle& nh) { translationalEstimatorParameters.kp_); nh.getParam("translational_estimator/kv", translationalEstimatorParameters.kv_); + nh.getParam("translational_estimator/outlier_threshold_meters", + translationalEstimatorParameters.outlier_threshold_meters_); + nh.getParam("translational_estimator/maximum_outlier_count", + translationalEstimatorParameters.maximum_outlier_count_); + // Recovering rotational estimator parameters values from the parameter server vicon_estimator::RotationalEstimatorParameters rotationalEstimatorParameters; nh.getParam(