msf_core.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
 * You can contact the author at <slynen at ethz dot ch>
 * Copyright (C) 2011-2012 Stephan Weiss, ASL, ETH Zurich, Switzerland
 * You can contact the author at <stephan dot weiss at ieee dot org>
 *
 * 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 MSF_CORE_H_
#define MSF_CORE_H_

#include <vector>
#include <queue>

#include <Eigen/Eigen>

#include <msf_core/msf_sortedContainer.h>
#include <msf_core/msf_state.h>
#include <msf_core/msf_checkFuzzyTracking.h>

namespace msf_core {

template<typename EKFState_T>
class MSF_SensorManager;
template<typename EKFState_T>
class IMUHandler;

template<typename EKFState_T>
class MSF_Core {
  friend class MSF_MeasurementBase<EKFState_T> ;
  friend class IMUHandler<EKFState_T> ;
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  enum {
    nErrorStatesAtCompileTime = EKFState_T::nErrorStatesAtCompileTime,
    nStatesAtCompileTime = EKFState_T::nStatesAtCompileTime
  };

  typedef typename EKFState_T::StateDefinition_T StateDefinition_T;
  typedef typename EKFState_T::StateSequence_T StateSequence_T;
  typedef Eigen::Matrix<double, nErrorStatesAtCompileTime, 1> ErrorState;
  typedef Eigen::Matrix<double, nErrorStatesAtCompileTime,
      nErrorStatesAtCompileTime> ErrorStateCov;

  typedef msf_core::SortedContainer<EKFState_T> StateBuffer_T;
  typedef msf_core::SortedContainer<
      typename msf_core::MSF_MeasurementBase<EKFState_T>,
      typename msf_core::MSF_InvalidMeasurement<EKFState_T> > measurementBufferT;

  void addMeasurement(shared_ptr<MSF_MeasurementBase<EKFState_T> > measurement);

  void init(shared_ptr<MSF_MeasurementBase<EKFState_T> > measurement);

  shared_ptr<EKFState_T> getClosestState(double tstamp);

  void getAccumF_SC(
      const shared_ptr<EKFState_T>& state_old,
      const shared_ptr<EKFState_T>& state_new,
      Eigen::Matrix<double, nErrorStatesAtCompileTime, nErrorStatesAtCompileTime>& F);
  shared_ptr<msf_core::MSF_MeasurementBase<EKFState_T> > getPreviousMeasurement(
      double time, int sensorID);

  shared_ptr<EKFState_T> getStateAtTime(double tstamp);

  void predictProcessCovariance(shared_ptr<EKFState_T>& state_old,
                                shared_ptr<EKFState_T>& state_new);

  void propagateState(shared_ptr<EKFState_T>& state_old,
                      shared_ptr<EKFState_T>& state_new);

  void CleanUpBuffers();

  void setPCore(
      Eigen::Matrix<double, EKFState_T::nErrorStatesAtCompileTime,
          EKFState_T::nErrorStatesAtCompileTime>& P);

  MSF_Core(const MSF_SensorManager<EKFState_T>& usercalc);
  ~MSF_Core();

  const MSF_SensorManager<EKFState_T>& usercalc() const;

 private:

  enum {
    indexOfStateWithoutTemporalDrift = msf_tmp::IndexOfBestNonTemporalDriftingState<
        StateSequence_T>::value
  };

  typedef typename msf_tmp::getEnumStateType<StateSequence_T,
      indexOfStateWithoutTemporalDrift>::value nonDriftingStateType;

  // asc.
  StateBuffer_T stateBuffer_;
  measurementBufferT MeasurementBuffer_;
  std::queue<shared_ptr<MSF_MeasurementBase<EKFState_T> > > queueFutureMeasurements_;
  double time_P_propagated;
  typename StateBuffer_T::iterator_T it_last_IMU;
  Eigen::Matrix<double, 3, 1> g_;

  bool initialized_;
  bool predictionMade_;
  bool isfuzzyState_;
  // states.
  CheckFuzzyTracking<EKFState_T, nonDriftingStateType> fuzzyTracker_;
  const MSF_SensorManager<EKFState_T>& usercalc_;

  bool applyCorrection(shared_ptr<EKFState_T>& delaystate,
                       ErrorState & correction, double fuzzythres = 0.1);

  void propPToState(shared_ptr<EKFState_T>& state);

  //Internal state propagation:
  void process_imu(const msf_core::Vector3&linear_acceleration,
                   const msf_core::Vector3&angular_velocity,
                   const double& msg_stamp, size_t msg_seq);


  void process_extstate(const msf_core::Vector3& linear_acceleration,
                        const msf_core::Vector3& angular_velocity,
                        const msf_core::Vector3& p, const msf_core::Vector3& v,
                        const msf_core::Quaternion& q,
                        bool is_already_propagated, const double& msg_stamp,
                        size_t msg_seq);

  void propagatePOneStep();

  void handlePendingMeasurements();

};

}
;
// msf_core

#include <msf_core/implementation/msf_core.hpp>

#endif /* MSF_CORE_H_ */