robot_localization_estimator.h

Go to the documentation of this file.

/*
 * Copyright (c) 2016, TNO IVS Helmond.
 * 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_ROBOT_LOCALIZATION_ESTIMATOR_H
#define ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H

#include <iostream>
#include <vector>
#include <boost/circular_buffer.hpp>
#include <Eigen/Dense>

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

namespace RobotLocalization
{

struct Twist
{
  Eigen::Vector3d linear;
  Eigen::Vector3d angular;
};

struct EstimatorState
{
  EstimatorState():
    time_stamp(0.0),
    state(STATE_SIZE),
    covariance(STATE_SIZE, STATE_SIZE)
  {
    state.setZero();
    covariance.setZero();
  }

  double time_stamp;

  Eigen::VectorXd state;

  Eigen::MatrixXd covariance;

  friend std::ostream& operator<<(std::ostream &os, const EstimatorState& state)
  {
    return os << "state:\n - time_stamp: " << state.time_stamp <<
                 "\n - state: \n" << state.state <<
                 " - covariance: \n" << state.covariance;
  }
};

namespace EstimatorResults
{
enum EstimatorResult
{
  ExtrapolationIntoFuture = 0,
  Interpolation,
  ExtrapolationIntoPast,
  Exact,
  EmptyBuffer,
  Failed
};
}  // namespace EstimatorResults
typedef EstimatorResults::EstimatorResult EstimatorResult;

namespace FilterTypes
{
enum FilterType
{
  EKF = 0,
  UKF,
  NotDefined
};
}  // namespace FilterTypes
typedef FilterTypes::FilterType FilterType;

class RobotLocalizationEstimator
{
public:
  explicit RobotLocalizationEstimator(unsigned int buffer_capacity,
                                      FilterType filter_type,
                                      const Eigen::MatrixXd& process_noise_covariance,
                                      const std::vector<double>& filter_args = std::vector<double>());

  virtual ~RobotLocalizationEstimator();

  void setState(const EstimatorState& state);

  EstimatorResult getState(const double time, EstimatorState &state) const;

  void clearBuffer();

  void setBufferCapacity(const int capacity);

  unsigned int getBufferCapacity() const;

  unsigned int getSize() const;

private:
  friend std::ostream& operator<<(std::ostream &os, const RobotLocalizationEstimator& rle)
  {
    for ( boost::circular_buffer<EstimatorState>::const_iterator it = rle.state_buffer_.begin();
          it != rle.state_buffer_.end(); ++it )
    {
      os << *it << "\n";
    }
    return os;
  }

  void extrapolate(const EstimatorState& boundary_state,
                   const double requested_time,
                   EstimatorState& state_at_req_time) const;

  void interpolate(const EstimatorState& given_state_1, const EstimatorState& given_state_2,
                   const double requested_time, EstimatorState& state_at_req_time) const;

  boost::circular_buffer<EstimatorState> state_buffer_;

  FilterBase* filter_;
};

}  // namespace RobotLocalization

#endif  // ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H