SSF_Core.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <stephan dot weiss at ieee dot org>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef SSF_CORE_H_
00033 #define SSF_CORE_H_
00034 
00035 
00036 #include <Eigen/Eigen>
00037 
00038 #include <ros/ros.h>
00039 #include <dynamic_reconfigure/server.h>
00040 #include <ssf_core/SSF_CoreConfig.h>
00041 
00042 // message includes
00043 #include <sensor_fusion_comm/DoubleArrayStamped.h>
00044 #include <sensor_fusion_comm/ExtState.h>
00045 #include <sensor_fusion_comm/ExtEkf.h>
00046 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00047 #include <sensor_msgs/Imu.h>
00048 
00049 #include <vector>
00050 #include <ssf_core/state.h>
00051 
00052 #define N_STATE_BUFFER 256      ///< size of unsigned char, do not change!
00053 #define HLI_EKF_STATE_SIZE 16   ///< number of states exchanged with external propagation. Here: p,v,q,bw,bw=16
00054 
00055 namespace ssf_core{
00056 
00057 typedef dynamic_reconfigure::Server<ssf_core::SSF_CoreConfig> ReconfigureServer;
00058 
00059 class SSF_Core
00060 {
00061 
00062 public:
00063   typedef Eigen::Matrix<double, N_STATE, 1> ErrorState;
00064   typedef Eigen::Matrix<double, N_STATE, N_STATE> ErrorStateCov;
00065 
00067   void initialize(const Eigen::Matrix<double, 3, 1> & p, const Eigen::Matrix<double, 3, 1> & v,
00068                   const Eigen::Quaternion<double> & q, const Eigen::Matrix<double, 3, 1> & b_w,
00069                   const Eigen::Matrix<double, 3, 1> & b_a, const double & L, const Eigen::Quaternion<double> & q_wv,
00070                   const Eigen::Matrix<double, N_STATE, N_STATE> & P, const Eigen::Matrix<double, 3, 1> & w_m,
00071                   const Eigen::Matrix<double, 3, 1> & a_m, const Eigen::Matrix<double, 3, 1> & g,
00072                   const Eigen::Quaternion<double> & q_ci, const Eigen::Matrix<double, 3, 1> & p_ci);
00073 
00075   unsigned char getClosestState(State* timestate, ros::Time tstamp, double delay = 0.00);
00076 
00078   bool getStateAtIdx(State* timestate, unsigned char idx);
00079 
00080   SSF_Core();
00081   ~SSF_Core();
00082 
00083 private:
00084   const static int nFullState_ = 28; 
00085   const static int nBuff_ = 30; 
00086   const static int nMaxCorr_ = 50; 
00087   const static int QualityThres_ = 1e3;
00088 
00089   Eigen::Matrix<double, N_STATE, N_STATE> Fd_; 
00090   Eigen::Matrix<double, N_STATE, N_STATE> Qd_; 
00091 
00093   State StateBuffer_[N_STATE_BUFFER]; 
00094   unsigned char idx_state_; 
00095   unsigned char idx_P_; 
00096   unsigned char idx_time_; 
00097 
00098   Eigen::Matrix<double, 3, 1> g_; 
00099 
00101   int qvw_inittimer_;
00102   Eigen::Matrix<double, nBuff_, 4> qbuff_;
00103 
00105   Eigen::Matrix<double, N_STATE, 1> correction_;
00106 
00108   ssf_core::SSF_CoreConfig config_;
00109 
00110   Eigen::Matrix<double, 3, 3> R_IW_; 
00111   Eigen::Matrix<double, 3, 3> R_CI_; 
00112   Eigen::Matrix<double, 3, 3> R_WV_; 
00113 
00114   bool initialized_;
00115   bool predictionMade_;
00116 
00118 
00123   bool data_playback_;
00124 
00125   enum
00126   {
00127     NO_UP, GOOD_UP, FUZZY_UP
00128   };
00129 
00130   ros::Publisher pubState_; 
00131   sensor_fusion_comm::DoubleArrayStamped msgState_;
00132 
00133   ros::Publisher pubPose_; 
00134   geometry_msgs::PoseWithCovarianceStamped msgPose_;
00135 
00136   ros::Publisher pubPoseCrtl_; 
00137   sensor_fusion_comm::ExtState msgPoseCtrl_;
00138 
00139   ros::Publisher pubCorrect_; 
00140   sensor_fusion_comm::ExtEkf msgCorrect_;
00141 
00142   ros::Subscriber subState_; 
00143   ros::Subscriber subImu_; 
00144 
00145   sensor_fusion_comm::ExtEkf hl_state_buf_; 
00146 
00147   // dynamic reconfigure
00148   ReconfigureServer *reconfServer_;
00149   typedef boost::function<void(ssf_core::SSF_CoreConfig& config, uint32_t level)> CallbackType;
00150   std::vector<CallbackType> callbacks_;
00151 
00153   void propagateState(const double dt);
00154 
00156   void predictProcessCovariance(const double dt);
00157 
00159   bool applyCorrection(unsigned char idx_delaystate, const ErrorState & res_delayed, double fuzzythres = 0.1);
00160 
00162   void propPToIdx(unsigned char idx);
00163 
00165 
00171   void imuCallback(const sensor_msgs::ImuConstPtr & msg);
00172 
00174 
00180   void stateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg);
00181 
00183   void Config(ssf_core::SSF_CoreConfig &config, uint32_t level);
00184 
00186   void DynConfig(ssf_core::SSF_CoreConfig &config, uint32_t level);
00187 
00189   double getMedian(const Eigen::Matrix<double, nBuff_, 1> & data);
00190 
00191 public:
00192   // some header implementations
00193 
00195   template<class H_type, class Res_type, class R_type>
00196     bool applyMeasurement(unsigned char idx_delaystate, const Eigen::MatrixBase<H_type>& H_delayed,
00197                           const Eigen::MatrixBase<Res_type> & res_delayed, const Eigen::MatrixBase<R_type>& R_delayed,
00198                           double fuzzythres = 0.1)
00199     {
00200       EIGEN_STATIC_ASSERT_FIXED_SIZE(H_type);
00201       EIGEN_STATIC_ASSERT_FIXED_SIZE(R_type);
00202 
00203       // get measurements
00204       if (!predictionMade_)
00205         return false;
00206 
00207       // make sure we have correctly propagated cov until idx_delaystate
00208       propPToIdx(idx_delaystate);
00209 
00210       R_type S;
00211       Eigen::Matrix<double, N_STATE, R_type::RowsAtCompileTime> K;
00212       ErrorStateCov & P = StateBuffer_[idx_delaystate].P_;
00213 
00214       S = H_delayed * StateBuffer_[idx_delaystate].P_ * H_delayed.transpose() + R_delayed;
00215       K = P * H_delayed.transpose() * S.inverse();
00216 
00217       correction_ = K * res_delayed;
00218       const ErrorStateCov KH = (ErrorStateCov::Identity() - K * H_delayed);
00219       P = KH * P * KH.transpose() + K * R_delayed * K.transpose();
00220 
00221       // make sure P stays symmetric
00222       P = 0.5 * (P + P.transpose());
00223 
00224       return applyCorrection(idx_delaystate, correction_, fuzzythres);
00225     }
00226 
00228   template<class T>
00229     void registerCallback(void(T::*cb_func)(ssf_core::SSF_CoreConfig& config, uint32_t level), T* p_obj)
00230     {
00231       callbacks_.push_back(boost::bind(cb_func, p_obj, _1, _2));
00232     }
00233 };
00234 
00235 };// end namespace
00236 
00237 #endif /* SSF_CORE_H_ */


ssf_core
Author(s): Stephan Weiss, Markus Achtelik
autogenerated on Fri Jan 3 2014 11:21:29