filter_base.h

Go to the documentation of this file.

/*
 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ROBOT_LOCALIZATION_FILTER_BASE_H
#define ROBOT_LOCALIZATION_FILTER_BASE_H

#include "robot_localization/filter_utilities.h"
#include "robot_localization/filter_common.h"

#include <Eigen/Dense>

#include <algorithm>
#include <limits>
#include <map>
#include <ostream>
#include <queue>
#include <set>
#include <string>
#include <vector>

#include <boost/shared_ptr.hpp>

namespace RobotLocalization
{

struct Measurement
{
  // The topic name for this measurement. Needed
  // for capturing previous state values for new
  // measurements.
  std::string topicName_;

  // The measurement and its associated covariance
  Eigen::VectorXd measurement_;
  Eigen::MatrixXd covariance_;

  // This defines which variables within this measurement
  // actually get passed into the filter. std::vector<bool>
  // is generally frowned upon, so we use ints.
  std::vector<int> updateVector_;

  // The real-valued time, in seconds, since some epoch
  // (presumably the start of execution, but any will do)
  double time_;

  // The Mahalanobis distance threshold in number of sigmas
  double mahalanobisThresh_;

  // The most recent control vector (needed for lagged data)
  Eigen::VectorXd latestControl_;

  // The time stamp of the most recent control term (needed for lagged data)
  double latestControlTime_;

  // We want earlier times to have greater priority
  bool operator()(const boost::shared_ptr<Measurement> &a, const boost::shared_ptr<Measurement> &b)
  {
    return (*this)(*(a.get()), *(b.get()));
  }

  bool operator()(const Measurement &a, const Measurement &b)
  {
    return a.time_ > b.time_;
  }

  Measurement() :
    topicName_(""),
    latestControl_(),
    latestControlTime_(0.0),
    time_(0.0),
    mahalanobisThresh_(std::numeric_limits<double>::max())
  {
  }
};
typedef boost::shared_ptr<Measurement> MeasurementPtr;

struct FilterState
{
  // The filter state vector
  Eigen::VectorXd state_;

  // The filter error covariance matrix
  Eigen::MatrixXd estimateErrorCovariance_;

  // The most recent control vector
  Eigen::VectorXd latestControl_;

  // The time stamp of the most recent measurement for the filter
  double lastMeasurementTime_;

  // The time stamp of the most recent control term
  double latestControlTime_;

  // We want the queue to be sorted from latest to earliest timestamps.
  bool operator()(const FilterState &a, const FilterState &b)
  {
    return a.lastMeasurementTime_ < b.lastMeasurementTime_;
  }

  FilterState() :
    state_(),
    estimateErrorCovariance_(),
    latestControl_(),
    lastMeasurementTime_(0.0),
    latestControlTime_(0.0)
  {}
};
typedef boost::shared_ptr<FilterState> FilterStatePtr;

class FilterBase
{
  public:
    FilterBase();

    virtual ~FilterBase();

    void reset();

    void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta);

    virtual void correct(const Measurement &measurement) = 0;

    const Eigen::VectorXd& getControl();

    double getControlTime();

    bool getDebug();

    const Eigen::MatrixXd& getEstimateErrorCovariance();

    bool getInitializedStatus();

    double getLastMeasurementTime();

    double getLastUpdateTime();

    const Eigen::VectorXd& getPredictedState();

    const Eigen::MatrixXd& getProcessNoiseCovariance();

    double getSensorTimeout();

    const Eigen::VectorXd& getState();

    virtual void predict(const double referenceTime, const double delta) = 0;

    virtual void processMeasurement(const Measurement &measurement);

    void setControl(const Eigen::VectorXd &control, const double controlTime);

    void setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
      const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
      const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains);

    void setDebug(const bool debug, std::ostream *outStream = NULL);

    void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance);

    void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance);

    void setLastMeasurementTime(const double lastMeasurementTime);

    void setLastUpdateTime(const double lastUpdateTime);

    void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance);

    void setSensorTimeout(const double sensorTimeout);

    void setState(const Eigen::VectorXd &state);

    void validateDelta(double &delta);

  protected:
    inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit,
      const double accelerationGain, const double decelerationLimit, const double decelerationGain)
    {
      FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");

      const double error = control - state;
      const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01);
      const double setPoint = (sameSign ? control : 0.0);
      const bool decelerating = ::fabs(setPoint) < ::fabs(state);
      double limit = accelerationLimit;
      double gain = accelerationGain;

      if (decelerating)
      {
        limit = decelerationLimit;
        gain = decelerationGain;
      }

      const double finalAccel = std::min(std::max(gain * error, -limit), limit);

      FB_DEBUG("Control value: " << control << "\n" <<
               "State value: " << state << "\n" <<
               "Error: " << error << "\n" <<
               "Same sign: " << (sameSign ? "true" : "false") << "\n" <<
               "Set point: " << setPoint << "\n" <<
               "Decelerating: " << (decelerating ? "true" : "false") << "\n" <<
               "Limit: " << limit << "\n" <<
               "Gain: " << gain << "\n" <<
               "Final is " << finalAccel << "\n");

      return finalAccel;
    }

    virtual void wrapStateAngles();

    virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
                                           const Eigen::MatrixXd &invCovariance,
                                           const double nsigmas);

    void prepareControl(const double referenceTime, const double predictionDelta);

    std::vector<double> accelerationGains_;

    std::vector<double> accelerationLimits_;

    Eigen::VectorXd controlAcceleration_;

    std::vector<double> decelerationGains_;

    std::vector<double> decelerationLimits_;

    Eigen::VectorXd latestControl_;

    std::vector<int> controlUpdateVector_;

    double controlTimeout_;

    Eigen::MatrixXd covarianceEpsilon_;

    std::ostream *debugStream_;

    Eigen::MatrixXd dynamicProcessNoiseCovariance_;

    Eigen::MatrixXd estimateErrorCovariance_;

    Eigen::MatrixXd identity_;

    bool initialized_;

    double lastMeasurementTime_;

    double lastUpdateTime_;

    double latestControlTime_;

    Eigen::VectorXd predictedState_;

    Eigen::MatrixXd processNoiseCovariance_;

    double sensorTimeout_;

    Eigen::VectorXd state_;

    Eigen::MatrixXd transferFunction_;

    Eigen::MatrixXd transferFunctionJacobian_;

    bool useControl_;

    bool useDynamicProcessNoiseCovariance_;

  private:
    bool debug_;
};

}  // namespace RobotLocalization

#endif  // ROBOT_LOCALIZATION_FILTER_BASE_H