filter.h

Go to the documentation of this file.

 
#ifndef INCLUDE_SRC_FILTER_H_
#define INCLUDE_SRC_FILTER_H_
 
#include <DataHandler/DataHandler.h>
#include <Eigen/Dense>
 
class Filter {
protected:
  static const unsigned short total_states = 3;
  static const unsigned short total_inputs = 2;
  static const unsigned short total_measurements = 2;
 
  /* Enums for easier accessing of variables in arrays and vectors. */
  enum { FORWARD_VELOCITY = 0, ANGULAR_VELOCITY = 1 };
  enum { RANGE = 0, BEARING = 1 };
  enum { X = 0, Y = 1, ORIENTATION = 2 };
 
  /* General Vector Types Definitions. */
  typedef Eigen::Matrix<double, 2, 1> vector2D_t;
  typedef Eigen::Matrix<double, total_states, 1> vector3D_t;
  typedef Eigen::Matrix<double, 2 * total_states, 1> vector6D_t;
 
  /* General Square Matrix Type Definitions. */
  typedef Eigen::Matrix<double, 2, 2> matrix2D_t;
  typedef Eigen::Matrix<double, total_states, total_states> matrix3D_t;
  typedef Eigen::Matrix<double, 2 * total_states, 2 * total_states> matrix6D_t;
 
  /* State Type Definitions. */
  typedef vector3D_t state_t;
  typedef vector6D_t augmentedState_t;
 
  /* Covariance Type Definitions. */
  typedef matrix3D_t covariance_t;
  typedef matrix6D_t augmentedCovariance_t;
 
  /* Information Type Definitions. */
  typedef vector3D_t information_t;
  typedef vector6D_t augmentedInformation_t;
 
  /* Precision Type Definitions. */
  typedef matrix3D_t precision_t;
  typedef matrix6D_t augmentedPrecision_t;
 
  /* Measurement Type Definitions. */
  typedef vector2D_t measurement_t;
  typedef matrix2D_t measurementCovariance_t;
 
  /* Measurement Type Definitions. */
  typedef Eigen::Matrix<double, 2 * total_states, total_measurements>
      kalmanGain_t;
 
  /* Motion Model Type Definitions. */
  typedef Eigen::Matrix<double, total_states, total_states> motionJacobian_t;
 
  typedef Eigen::Matrix<double, total_states, total_inputs> processJacobian_t;
 
  typedef Eigen::Matrix<double, total_measurements, 2 * total_states>
      measurementJacobian_t;
 
  typedef Eigen::Matrix<double, total_inputs, total_inputs> processCovariance_t;
 
  /* Huber Cost Function Type Definitions. */
  typedef Eigen::Matrix<double, total_measurements, 1>
      huberMeasurementThresholds_t;
 
  typedef Eigen::Matrix<double, total_measurements, total_measurements>
      huberMeasurementWeights_t;
 
  typedef Eigen::Matrix<double, 2 * total_states, 1> huberStateThresholds_t;
  typedef Eigen::Matrix<double, 2 * total_states, 2 * total_states>
      huberStateWeights_t;
 
  DataHandler &data_;
 
  struct EstimationParameters {
    unsigned short id;
    unsigned short barcode;
 
    state_t state_estimate = state_t::Zero();
 
    measurement_t measurement = measurement_t::Zero();
 
    measurement_t innovation = measurement_t::Zero();
 
    measurementCovariance_t innovation_covariance =
        measurementCovariance_t::Zero();
 
    augmentedState_t estimation_residual = augmentedState_t::Zero();
 
    covariance_t error_covariance = covariance_t::Identity() * 0.001;
 
    kalmanGain_t kalman_gain = kalmanGain_t::Zero();
 
    processCovariance_t process_noise = processCovariance_t::Zero();
 
    measurementCovariance_t measurement_noise = measurementCovariance_t::Zero();
 
    motionJacobian_t motion_jacobian = motionJacobian_t::Zero();
 
    processJacobian_t process_jacobian = processJacobian_t::Zero();
 
    measurementJacobian_t measurement_jacobian = measurementJacobian_t::Zero();
 
    state_t information_vector = state_t::Zero();
 
    precision_t precision_matrix = error_covariance.inverse();
  };
 
  std::vector<EstimationParameters> robot_parameters;
 
  std::vector<EstimationParameters> landmark_parameters;
 
  const huberMeasurementThresholds_t measurement_thresholds =
      (huberMeasurementThresholds_t() << 0.2, 0.01).finished();
 
  const huberStateThresholds_t state_thresholds =
      (huberStateThresholds_t() << 0.15, 0.154, 0.255, 0.0104, 0.0104, 0.0)
          .finished();
 
  /* Filter Functionality Functions */
  virtual void prediction(const Robot::Odometry &, EstimationParameters &) = 0;
 
  virtual void correction(EstimationParameters &,
                          const EstimationParameters &) = 0;
 
  /* Motion Model Functions. */
  void motionModel(const Robot::Odometry &, EstimationParameters &,
                   const double);
 
  void calculateMotionJacobian(const Robot::Odometry &, EstimationParameters &,
                               const double);
 
  void calculateProcessJacobian(EstimationParameters &, const double);
 
  /* Measurement Model Functions. */
  measurement_t measurementModel(EstimationParameters &,
                                 const EstimationParameters &);
 
  void calculateMeasurementJacobian(EstimationParameters &,
                                    const EstimationParameters &);
 
  /* Filter Helper Functions. */
  matrix3D_t marginalise(const matrix6D_t &);
 
  state_t marginalise(const vector6D_t &, const matrix6D_t &);
 
  augmentedInformation_t createAugmentedVector(const state_t &,
                                               const state_t &);
 
  augmentedCovariance_t createAugmentedMatrix(const covariance_t &,
                                              const covariance_t &);
 
  void normaliseAngle(double &);
 
  measurement_t calculateNormalisedInnovation(const EstimationParameters &);
 
  augmentedState_t
  calculateNormalisedEstimationResidual(const EstimationParameters &);
 
  /* Huber Cost Functions. */
  huberMeasurementWeights_t
  HuberMeasurement(const measurement_t &, const huberMeasurementThresholds_t &);
 
  huberStateWeights_t HuberState(const augmentedState_t &,
                                 const huberStateThresholds_t &);
 
  matrix3D_t computePseudoInverse(const matrix3D_t &);
  matrix6D_t computePseudoInverse(const matrix6D_t &);
 
public:
  explicit Filter(DataHandler &data);
  virtual ~Filter();
 
  void performInference();
};
 
#endif // INCLUDE_SRC_FILTER_H_