├── .gitignore ├── README.md ├── ethzasl_sensor_fusion ├── CMakeLists.txt └── package.xml ├── sensor_fusion_comm ├── CMakeLists.txt ├── mainpage.dox ├── msg │ ├── DoubleArrayStamped.msg │ ├── ExtEkf.msg │ └── ExtState.msg └── package.xml ├── ssf_core ├── CMakeLists.txt ├── IMPORTANT ├── cfg │ └── SSF_Core.cfg ├── include │ └── ssf_core │ │ ├── SSF_Core.h │ │ ├── eigen_conversions.h │ │ ├── eigen_utils.h │ │ ├── measurement.h │ │ └── state.h ├── mainpage.dox ├── msg │ ├── DoubleArrayStamped.msg │ ├── ext_ekf.msg │ ├── ext_imu.msg │ └── ext_state.msg ├── package.xml ├── scripts │ ├── plot_accbias │ ├── plot_attitude │ ├── plot_calib │ ├── plot_gyrbias │ ├── plot_position │ ├── plot_qvw │ ├── plot_relevant │ ├── plot_scale │ └── plot_velocity └── src │ ├── SSF_Core.cpp │ ├── calcQ.h │ ├── measurement.cpp │ └── state.cpp └── ssf_updates ├── CMakeLists.txt ├── launch ├── pose_sensor.launch └── position_sensor.launch ├── mainpage.dox ├── msg └── PositionWithCovarianceStamped.msg ├── package.xml ├── pose_sensor_fix.yaml ├── position_sensor_fix.yaml └── src ├── main.cpp ├── pose_measurements.h ├── pose_sensor.cpp ├── pose_sensor.h ├── position_measurements.h ├── position_sensor.cpp └── position_sensor.h /.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ethzasl_sensor_fusion 2 | ===================== 3 | 4 | time delay compensated single and multi sensor fusion framework based on an EKF 5 | -------------------------------------------------------------------------------- /ethzasl_sensor_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ethzasl_sensor_fusion) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ethzasl_sensor_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ethzasl_sensor_fusion 3 | 0.1.0 4 | Time delay compensated single and multi sensor fusion framework based on an EKF 5 | Maintained by Stephan Weiss 6 | Markus Achtelik 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/ethzasl_sensor_fusion 11 | http://github.com/ethz-asl/ethzasl_sensor_fusion/issues 12 | 13 | Maintained by Stephan Weiss 14 | Markus Achtelik 15 | 16 | 17 | catkin 18 | 19 | 20 | ssf_updates 21 | ssf_core 22 | sensor_fusion_comm 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /sensor_fusion_comm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sensor_fusion_comm) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation) 5 | 6 | # Set the build type. Options are: 7 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 8 | # Debug : w/ debug symbols, w/o optimization 9 | # Release : w/o debug symbols, w/ optimization 10 | # RelWithDebInfo : w/ debug symbols, w/ optimization 11 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 12 | #set(ROS_BUILD_TYPE RelWithDebInfo) 13 | 14 | 15 | #set the default path for built executables to the "bin" directory 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 17 | #set the default path for built libraries to the "lib" directory 18 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 19 | 20 | 21 | add_message_files( 22 | FILES 23 | DoubleArrayStamped.msg 24 | ExtEkf.msg 25 | ExtState.msg 26 | ) 27 | 28 | #uncomment if you have defined services 29 | #add_service_files( 30 | # FILES 31 | # TODO: List your msg files here 32 | #) 33 | 34 | generate_messages( 35 | DEPENDENCIES geometry_msgs std_msgs 36 | ) 37 | # TODO: fill in what other packages will need to use this package 38 | ## DEPENDS: system dependencies of this project that dependent projects also need 39 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 40 | ## INCLUDE_DIRS: 41 | ## LIBRARIES: libraries you create in this project that dependent projects also need 42 | catkin_package( 43 | DEPENDS 44 | CATKIN_DEPENDS geometry_msgs 45 | INCLUDE_DIRS # TODO include 46 | LIBRARIES # TODO 47 | ) 48 | -------------------------------------------------------------------------------- /sensor_fusion_comm/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sensor_fusion_comm is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /sensor_fusion_comm/msg/DoubleArrayStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] data -------------------------------------------------------------------------------- /sensor_fusion_comm/msg/ExtEkf.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 angular_velocity 3 | geometry_msgs/Vector3 linear_acceleration 4 | float32[] state 5 | int32 flag 6 | 7 | uint32 ignore_state = 0 8 | uint32 current_state = 1 9 | uint32 initialization = 2 10 | uint32 state_correction = 3 11 | -------------------------------------------------------------------------------- /sensor_fusion_comm/msg/ExtState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Vector3 velocity -------------------------------------------------------------------------------- /sensor_fusion_comm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | sensor_fusion_comm 3 | 0.1.0 4 | This package contains messages, services and action definitions needed by ethzasl_sensor_fusion and nodes communicating with it. 5 | Stephan Weiss 6 | Markus Achtelik 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/sensor_fusion_comm 11 | 12 | 13 | Stephan Weiss 14 | Markus Achtelik 15 | 16 | 17 | catkin 18 | 19 | 20 | geometry_msgs 21 | 22 | 23 | geometry_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /ssf_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ssf_core) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs dynamic_reconfigure cmake_modules sensor_fusion_comm) 5 | 6 | # Set the build type. Options are: 7 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 8 | # Debug : w/ debug symbols, w/o optimization 9 | # Release : w/o debug symbols, w/ optimization 10 | # RelWithDebInfo : w/ debug symbols, w/ optimization 11 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 12 | #set(ROS_BUILD_TYPE RelWithDebInfo) 13 | 14 | 15 | #set the default path for built executables to the "bin" directory 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 17 | #set the default path for built libraries to the "lib" directory 18 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 19 | 20 | # Generate dynamic parameters 21 | 22 | #add_message_files( 23 | # FILES 24 | # TODO: List your msg files here 25 | #) 26 | 27 | add_definitions (-Wall -O3) 28 | 29 | # get eigen 30 | find_package(Eigen REQUIRED) 31 | include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 32 | 33 | generate_dynamic_reconfigure_options(cfg/SSF_Core.cfg) 34 | 35 | catkin_package( 36 | DEPENDS 37 | CATKIN_DEPENDS roscpp sensor_msgs dynamic_reconfigure sensor_fusion_comm 38 | INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} 39 | LIBRARIES ssf_core 40 | ) 41 | 42 | add_library(ssf_core src/SSF_Core.cpp src/measurement.cpp src/state.cpp) 43 | add_dependencies(ssf_core ${PROJECT_NAME}_gencfg) 44 | target_link_libraries(ssf_core ${catkin_LIBRRIES}) 45 | 46 | -------------------------------------------------------------------------------- /ssf_core/IMPORTANT: -------------------------------------------------------------------------------- 1 | Note on ssf_core and pcl from ROS: 2 | - When pcl libraries used with ssf_core the EKF update yields NaNs 3 | 4 | possible reason: pcl libraries conflict with Eigen (overwriting symbols, 5 | definitions, functions,...??). This yields wierd values in Eigen 6 | calculations and eventually to NaNs, INFs etc 7 | 8 | -------------------------------------------------------------------------------- /ssf_core/cfg/SSF_Core.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | # You can contact the author at 5 | 6 | # All rights reserved. 7 | 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are met: 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of ETHZ-ASL nor the 16 | # names of its contributors may be used to endorse or promote products 17 | # derived from this software without specific prior written permission. 18 | 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | # DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | 31 | PACKAGE='ssf_core' 32 | 33 | #from driver_base.msg import SensorLevels 34 | from dynamic_reconfigure.parameter_generator_catkin import * 35 | 36 | gen = ParameterGenerator() 37 | 38 | # @todo Think about levels. Setting most of these to STOP to guarantee atomicity. 39 | # @todo Double check these ranges, defaults 40 | 41 | INIT_FILTER = gen.const("INIT_FILTER", int_t, 0x00000001, "init_filter") 42 | MISC = gen.const("MISC", int_t, 0x00000002, "misc") 43 | SET_HEIGHT = gen.const("SET_HEIGHT", int_t, 0x00000004, "set_height") 44 | 45 | # Name Type Reconfiguration level Description Default Min Max 46 | gen.add("init_filter", bool_t, INIT_FILTER["value"], "call filter init using defined scale", False) 47 | gen.add("scale_init", double_t, MISC["value"], "value for initial scale", 1, 0.001, 30) 48 | gen.add("fixed_scale", bool_t, MISC["value"], "fix scale", False) 49 | gen.add("fixed_bias", bool_t, MISC["value"], "fix biases", False) 50 | gen.add("fixed_calib", bool_t, MISC["value"], "fix calibration states", False) 51 | gen.add("noise_acc", double_t, MISC["value"], "noise accelerations (std. dev)", 0.0083, 1.0e-4, 0.5) 52 | gen.add("noise_accbias", double_t, MISC["value"], "noise acceleration bias (std. dev)", 0.00083, 1.0e-7, 0.1) 53 | gen.add("noise_gyr", double_t, MISC["value"], "noise gyros (std. dev)", 0.0013, 1.0e-4, 0.5) 54 | gen.add("noise_gyrbias", double_t, MISC["value"], "noise gyro biases (std. dev)", 0.00013, 1.0e-7, 0.1) 55 | gen.add("noise_scale", double_t, MISC["value"], "noise scale (std. dev)", 0.0, 0, 10.0) 56 | gen.add("noise_qwv", double_t, MISC["value"], "noise qwv (std. dev)", 0.0, 0, 10.0) 57 | gen.add("noise_qci", double_t, MISC["value"], "noise qci (std. dev)", 0.0, 0, 10.0) 58 | gen.add("noise_pic", double_t, MISC["value"], "noise pic (std. dev)", 0.0, 0, 10.0) 59 | gen.add("delay", double_t, MISC["value"], "fix delay in seconds", 0.03, -2.0, 2.0) 60 | gen.add("set_height", bool_t, SET_HEIGHT["value"], "call filter init using defined height", False) 61 | gen.add("height", double_t, MISC["value"], "height in m for init", 1, 0.1, 20) 62 | gen.add("meas_noise1", double_t, MISC["value"], "noise for measurement sensor (std. dev)", 0.01, 0, 10) 63 | gen.add("meas_noise2", double_t, MISC["value"], "noise for measurement sensor (std. dev)", 0.01, 0, 10) 64 | 65 | 66 | exit(gen.generate(PACKAGE, "Config", "SSF_Core")) 67 | -------------------------------------------------------------------------------- /ssf_core/include/ssf_core/SSF_Core.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef SSF_CORE_H_ 33 | #define SSF_CORE_H_ 34 | 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | // message includes 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | #define N_STATE_BUFFER 256 ///< size of unsigned char, do not change! 53 | #define HLI_EKF_STATE_SIZE 16 ///< number of states exchanged with external propagation. Here: p,v,q,bw,bw=16 54 | 55 | namespace ssf_core{ 56 | 57 | typedef dynamic_reconfigure::Server ReconfigureServer; 58 | 59 | class SSF_Core 60 | { 61 | 62 | public: 63 | typedef Eigen::Matrix ErrorState; 64 | typedef Eigen::Matrix ErrorStateCov; 65 | 66 | /// big init routine 67 | void initialize(const Eigen::Matrix & p, const Eigen::Matrix & v, 68 | const Eigen::Quaternion & q, const Eigen::Matrix & b_w, 69 | const Eigen::Matrix & b_a, const double & L, const Eigen::Quaternion & q_wv, 70 | const Eigen::Matrix & P, const Eigen::Matrix & w_m, 71 | const Eigen::Matrix & a_m, const Eigen::Matrix & g, 72 | const Eigen::Quaternion & q_ci, const Eigen::Matrix & p_ci); 73 | 74 | /// retreive all state information at time t. Used to build H, residual and noise matrix by update sensors 75 | unsigned char getClosestState(State* timestate, ros::Time tstamp, double delay = 0.00); 76 | 77 | /// get all state information at a given index in the ringbuffer 78 | bool getStateAtIdx(State* timestate, unsigned char idx); 79 | 80 | SSF_Core(); 81 | ~SSF_Core(); 82 | 83 | private: 84 | const static int nFullState_ = 28; ///< complete state 85 | const static int nBuff_ = 30; ///< buffer size for median q_vw 86 | const static int nMaxCorr_ = 50; ///< number of IMU measurements buffered for time correction actions 87 | const static int QualityThres_ = 1e3; 88 | 89 | Eigen::Matrix Fd_; ///< discrete state propagation matrix 90 | Eigen::Matrix Qd_; ///< discrete propagation noise matrix 91 | 92 | /// state variables 93 | State StateBuffer_[N_STATE_BUFFER]; ///< EKF ringbuffer containing pretty much all info needed at time t 94 | unsigned char idx_state_; ///< pointer to state buffer at most recent state 95 | unsigned char idx_P_; ///< pointer to state buffer at P latest propagated 96 | unsigned char idx_time_; ///< pointer to state buffer at a specific time 97 | 98 | Eigen::Matrix g_; ///< gravity vector 99 | 100 | /// vision-world drift watch dog to determine fuzzy tracking 101 | int qvw_inittimer_; 102 | Eigen::Matrix qbuff_; 103 | 104 | /// correction from EKF update 105 | Eigen::Matrix correction_; 106 | 107 | /// dynamic reconfigure config 108 | ssf_core::SSF_CoreConfig config_; 109 | 110 | Eigen::Matrix R_IW_; ///< Rot IMU->World 111 | Eigen::Matrix R_CI_; ///< Rot Camera->IMU 112 | Eigen::Matrix R_WV_; ///< Rot World->Vision 113 | 114 | bool initialized_; 115 | bool predictionMade_; 116 | 117 | /// enables internal state predictions for log replay 118 | /** 119 | * used to determine if internal states get overwritten by the external 120 | * state prediction (online) or internal state prediction is performed 121 | * for log replay, when the external prediction is not available. 122 | */ 123 | bool data_playback_; 124 | 125 | enum 126 | { 127 | NO_UP, GOOD_UP, FUZZY_UP 128 | }; 129 | 130 | ros::Publisher pubState_; ///< publishes all states of the filter 131 | sensor_fusion_comm::DoubleArrayStamped msgState_; 132 | 133 | ros::Publisher pubPose_; ///< publishes 6DoF pose output 134 | geometry_msgs::PoseWithCovarianceStamped msgPose_; 135 | 136 | ros::Publisher pubPoseCrtl_; ///< publishes 6DoF pose including velocity output 137 | sensor_fusion_comm::ExtState msgPoseCtrl_; 138 | 139 | ros::Publisher pubCorrect_; ///< publishes corrections for external state propagation 140 | sensor_fusion_comm::ExtEkf msgCorrect_; 141 | 142 | ros::Subscriber subState_; ///< subscriber to external state propagation 143 | ros::Subscriber subImu_; ///< subscriber to IMU readings 144 | 145 | sensor_fusion_comm::ExtEkf hl_state_buf_; ///< buffer to store external propagation data 146 | 147 | // dynamic reconfigure 148 | ReconfigureServer *reconfServer_; 149 | typedef boost::function CallbackType; 150 | std::vector callbacks_; 151 | 152 | /// propagates the state with given dt 153 | void propagateState(const double dt); 154 | 155 | /// propagets the error state covariance 156 | void predictProcessCovariance(const double dt); 157 | 158 | /// applies the correction 159 | bool applyCorrection(unsigned char idx_delaystate, const ErrorState & res_delayed, double fuzzythres = 0.1); 160 | 161 | /// propagate covariance to a given index in the ringbuffer 162 | void propPToIdx(unsigned char idx); 163 | 164 | /// internal state propagation 165 | /** 166 | * This function gets called on incoming imu messages an then performs 167 | * the state prediction internally. Only use this OR stateCallback by 168 | * remapping the topics accordingly. 169 | * \sa{stateCallback} 170 | */ 171 | void imuCallback(const sensor_msgs::ImuConstPtr & msg); 172 | 173 | /// external state propagation 174 | /** 175 | * This function gets called when state prediction is performed externally, 176 | * e.g. by asctec_mav_framework. Msg has to be the latest predicted state. 177 | * Only use this OR imuCallback by remapping the topics accordingly. 178 | * \sa{imuCallback} 179 | */ 180 | void stateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg); 181 | 182 | /// gets called by dynamic reconfigure and calls all registered callbacks in callbacks_ 183 | void Config(ssf_core::SSF_CoreConfig &config, uint32_t level); 184 | 185 | /// handles the dynamic reconfigure for ssf_core 186 | void DynConfig(ssf_core::SSF_CoreConfig &config, uint32_t level); 187 | 188 | /// computes the median of a given vector 189 | double getMedian(const Eigen::Matrix & data); 190 | 191 | public: 192 | // some header implementations 193 | 194 | /// main update routine called by a given sensor 195 | template 196 | bool applyMeasurement(unsigned char idx_delaystate, const Eigen::MatrixBase& H_delayed, 197 | const Eigen::MatrixBase & res_delayed, const Eigen::MatrixBase& R_delayed, 198 | double fuzzythres = 0.1) 199 | { 200 | EIGEN_STATIC_ASSERT_FIXED_SIZE(H_type); 201 | EIGEN_STATIC_ASSERT_FIXED_SIZE(R_type); 202 | 203 | // get measurements 204 | if (!predictionMade_) 205 | return false; 206 | 207 | // make sure we have correctly propagated cov until idx_delaystate 208 | propPToIdx(idx_delaystate); 209 | 210 | R_type S; 211 | Eigen::Matrix K; 212 | ErrorStateCov & P = StateBuffer_[idx_delaystate].P_; 213 | 214 | S = H_delayed * StateBuffer_[idx_delaystate].P_ * H_delayed.transpose() + R_delayed; 215 | K = P * H_delayed.transpose() * S.inverse(); 216 | 217 | correction_ = K * res_delayed; 218 | const ErrorStateCov KH = (ErrorStateCov::Identity() - K * H_delayed); 219 | P = KH * P * KH.transpose() + K * R_delayed * K.transpose(); 220 | 221 | // make sure P stays symmetric 222 | P = 0.5 * (P + P.transpose()); 223 | 224 | return applyCorrection(idx_delaystate, correction_, fuzzythres); 225 | } 226 | 227 | /// registers dynamic reconfigure callbacks 228 | template 229 | void registerCallback(void(T::*cb_func)(ssf_core::SSF_CoreConfig& config, uint32_t level), T* p_obj) 230 | { 231 | callbacks_.push_back(boost::bind(cb_func, p_obj, _1, _2)); 232 | } 233 | }; 234 | 235 | };// end namespace 236 | 237 | #endif /* SSF_CORE_H_ */ 238 | -------------------------------------------------------------------------------- /ssf_core/include/ssf_core/eigen_conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef EIGEN_CONVERSIONS_H_ 33 | #define EIGEN_CONVERSIONS_H_ 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | namespace eigen_conversions 42 | { 43 | 44 | /// copies eigen quaternion to geometry_msgs/quaternion 45 | template 46 | inline void quaternionToMsg(const Eigen::Quaternion & q_in, geometry_msgs::Quaternion & q_out) 47 | { 48 | q_out.w = q_in.w(); 49 | q_out.x = q_in.x(); 50 | q_out.y = q_in.y(); 51 | q_out.z = q_in.z(); 52 | } 53 | 54 | /// copies eigen quaternion to geometry_msgs/quaternion 55 | template 56 | inline geometry_msgs::Quaternion quaternionToMsg(const Eigen::Quaternion & q_in) 57 | { 58 | geometry_msgs::Quaternion q_out; 59 | quaternionToMsg(q_in, q_out); 60 | return q_out; 61 | } 62 | 63 | /// copies an eigen 3d vector to a 3d Point struct. point has to have members x,y,z! 64 | template 65 | inline void vector3dToPoint(const Eigen::MatrixBase & vec, Point & point) 66 | { 67 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 68 | point.x = vec[0]; 69 | point.y = vec[1]; 70 | point.z = vec[2]; 71 | } 72 | 73 | /// copies an eigen 3d vector to a 3d Point struct. point has to have members x,y,z! 74 | template 75 | inline Point vector3dToPoint(const Eigen::MatrixBase & vec) 76 | { 77 | Point point; 78 | vector3dToPoint(vec, point); 79 | return point; 80 | } 81 | 82 | } 83 | ; 84 | 85 | #endif /* EIGEN_CONVERSIONS_H_ */ 86 | -------------------------------------------------------------------------------- /ssf_core/include/ssf_core/eigen_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef EIGEN_UTILS_H_ 33 | #define EIGEN_UTILS_H_ 34 | 35 | #include 36 | #include 37 | 38 | /// returns the 3D cross product skew symmetric matrix of a given 3D vector 39 | template 40 | inline Eigen::Matrix skew(const Eigen::MatrixBase & vec) 41 | { 42 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 43 | return (Eigen::Matrix() << 0.0, -vec[2], vec[1], vec[2], 0.0, -vec[0], -vec[1], vec[0], 0.0).finished(); 44 | } 45 | 46 | /// returns a matrix with angular velocities used for quaternion derivatives/integration with the JPL notation 47 | /** 48 | The quaternion to be multiplied with this matrix has to be in the order x y z w !!! 49 | \param {3D vector with angular velocities} 50 | \return {4x4 matrix for multiplication with the quaternion} 51 | */ 52 | template 53 | inline Eigen::Matrix omegaMatJPL(const Eigen::MatrixBase & vec) 54 | { 55 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 56 | return ( 57 | Eigen::Matrix() << 58 | 0, vec[2], -vec[1], vec[0], 59 | -vec[2], 0, vec[0], vec[1], 60 | vec[1], -vec[0], 0, vec[2], 61 | -vec[0], -vec[1], -vec[2], 0 62 | ).finished(); 63 | } 64 | 65 | /// returns a matrix with angular velocities used for quaternion derivatives/integration with the Hamilton notation 66 | /** 67 | The quaternion to be multiplied with this matrix has to be in the order x y z w !!! 68 | \param {3D vector with angular velocities} 69 | \return {4x4 matrix for multiplication with the quaternion} 70 | */ 71 | template 72 | inline Eigen::Matrix omegaMatHamilton(const Eigen::MatrixBase & vec) 73 | { 74 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 75 | return ( 76 | Eigen::Matrix() << 77 | 0, -vec[2], vec[1], vec[0], 78 | vec[2], 0, -vec[0], vec[1], 79 | -vec[1], vec[0], 0, vec[2], 80 | -vec[0], -vec[1], -vec[2], 0 81 | ).finished(); 82 | } 83 | 84 | /// computes a quaternion from the 3-element small angle approximation theta 85 | template 86 | Eigen::Quaternion quaternionFromSmallAngle(const Eigen::MatrixBase & theta) 87 | { 88 | typedef typename Derived::Scalar Scalar; 89 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 90 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 91 | const Scalar q_squared = theta.squaredNorm() / 4.0; 92 | 93 | if ( q_squared < 1) 94 | { 95 | return Eigen::Quaternion(sqrt(1 - q_squared), theta[0] * 0.5, theta[1] * 0.5, theta[2] * 0.5); 96 | } 97 | else 98 | { 99 | const Scalar w = 1.0 / sqrt(1 + q_squared); 100 | const Scalar f = w*0.5; 101 | return Eigen::Quaternion(w, theta[0] * f, theta[1] * f, theta[2] * f); 102 | } 103 | } 104 | 105 | /// debug output to check misbehavior of Eigen 106 | template 107 | bool checkForNumeric(const T & vec, int size, const std::string & info) 108 | { 109 | for (int i = 0; i < size; i++) 110 | { 111 | if (isnan(vec[i])) 112 | { 113 | std::cerr << "=== ERROR === " << info << ": NAN at index " << i << std::endl; 114 | return false; 115 | } 116 | if (isinf(vec[i])) 117 | { 118 | std::cerr << "=== ERROR === " << info << ": INF at index " << i << std::endl; 119 | return false; 120 | } 121 | } 122 | return true; 123 | } 124 | 125 | #endif /* EIGEN_UTILS_H_ */ 126 | -------------------------------------------------------------------------------- /ssf_core/include/ssf_core/measurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef MEASUREMENT_H 33 | #define MEASUREMENT_H 34 | 35 | #include 36 | 37 | namespace ssf_core{ 38 | 39 | class MeasurementHandler; 40 | 41 | class Measurements 42 | { 43 | protected: 44 | typedef std::vector Handlers; 45 | Handlers handlers; 46 | 47 | ReconfigureServer *reconfServer_; 48 | 49 | void Config(ssf_core::SSF_CoreConfig &config, uint32_t level); 50 | virtual void init(double scale) = 0; 51 | 52 | public: 53 | 54 | // measurements 55 | Eigen::Quaternion q_cv_; 56 | Eigen::Matrix p_vc_; 57 | Eigen::Matrix v_vc_; 58 | SSF_Core ssf_core_; 59 | 60 | void addHandler(MeasurementHandler* handler) 61 | { 62 | handlers.push_back(handler); 63 | } 64 | Measurements(); 65 | virtual ~Measurements(); 66 | }; 67 | 68 | 69 | class MeasurementHandler 70 | { 71 | protected: 72 | Measurements* measurements; 73 | 74 | public: 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 76 | MeasurementHandler(Measurements* meas):measurements(meas){} 77 | 78 | virtual ~MeasurementHandler() {} 79 | }; 80 | 81 | }; // end namespace 82 | 83 | #endif /* MEASUREMENT_H */ 84 | -------------------------------------------------------------------------------- /ssf_core/include/ssf_core/state.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef STATE_H_ 33 | #define STATE_H_ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #define N_STATE 25 /// error state size 44 | 45 | namespace ssf_core 46 | { 47 | /** 48 | * This class defines the state, its associated error state covarinace and the 49 | * system inputs. The values in the braces determine the state's position in the 50 | * state vector / error state vector. 51 | */ 52 | class State 53 | { 54 | public: 55 | // states varying during propagation 56 | Eigen::Matrix p_; ///< position (IMU centered) (0-2 / 0-2) 57 | Eigen::Matrix v_; ///< velocity (3- 5 / 3- 5) 58 | Eigen::Quaternion q_; ///< attitude (6- 9 / 6- 8) 59 | Eigen::Matrix b_w_; ///< gyro biases (10-12 / 9-11) 60 | Eigen::Matrix b_a_; ///< acceleration biases (13-15 / 12-14) 61 | 62 | // states not varying during propagation 63 | double L_; ///< visual scale (16 / 15) 64 | Eigen::Quaternion q_wv_; ///< vision-world attitude drift (17-20 / 16-18) 65 | Eigen::Quaternion q_ci_; ///< camera-imu attitude calibration (21-24 / 19-21) 66 | Eigen::Matrix p_ci_; ///< camera-imu position calibration (25-27 / 22-24) 67 | 68 | // system inputs 69 | Eigen::Matrix w_m_; ///< angular velocity from IMU 70 | Eigen::Matrix a_m_; ///< acceleration from IMU 71 | 72 | Eigen::Quaternion q_int_; ///< this is the integrated ang. vel. no corrections applied, to use for delta rot in external algos... 73 | 74 | Eigen::Matrix P_;///< error state covariance 75 | 76 | double time_; ///< time of this state estimate 77 | 78 | /// resets the state 79 | /** 80 | * 3D vectors: 0; quaternion: unit quaternion; scale: 1; time:0; Error covariance: zeros 81 | */ 82 | void reset(); 83 | 84 | /// writes the covariance corresponding to position and attitude to cov 85 | void getPoseCovariance(geometry_msgs::PoseWithCovariance::_covariance_type & cov); 86 | 87 | /// assembles a PoseWithCovarianceStamped message from the state 88 | /** it does not set the header */ 89 | void toPoseMsg(geometry_msgs::PoseWithCovarianceStamped & pose); 90 | 91 | /// assembles an ExtState message from the state 92 | /** it does not set the header */ 93 | void toExtStateMsg(sensor_fusion_comm::ExtState & state); 94 | 95 | /// assembles a DoubleArrayStamped message from the state 96 | /** it does not set the header */ 97 | void toStateMsg(sensor_fusion_comm::DoubleArrayStamped & state); 98 | 99 | 100 | 101 | }; 102 | 103 | } 104 | 105 | #endif /* STATE_H_ */ 106 | -------------------------------------------------------------------------------- /ssf_core/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b ssf_core is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /ssf_core/msg/DoubleArrayStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] data -------------------------------------------------------------------------------- /ssf_core/msg/ext_ekf.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 angular_velocity 3 | geometry_msgs/Vector3 linear_acceleration 4 | float32[] state 5 | int32 flag 6 | 7 | uint32 ignore_state = 0 8 | uint32 current_state = 1 9 | uint32 initialization = 2 10 | uint32 state_correction = 3 11 | -------------------------------------------------------------------------------- /ssf_core/msg/ext_imu.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Quaternion orientation 3 | geometry_msgs/Vector3 angular_velocity 4 | geometry_msgs/Vector3 acceleration 5 | float64 height 6 | float64 differential_height -------------------------------------------------------------------------------- /ssf_core/msg/ext_state.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Vector3 velocity -------------------------------------------------------------------------------- /ssf_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ssf_core 3 | 0.1.0 4 | Single Sensor Fusion (SSF) framework containing the core filter functions including full state propagation and shell for update sensor modules 5 | Stephan Weiss 6 | Markus Achtelik 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/ethzasl_sensor_fusion/ssf_core 11 | http://github.com/ethz-asl/ethzasl_sensor_fusion/issues 12 | 13 | Stephan Weiss 14 | Markus Achtelik 15 | 16 | 17 | catkin 18 | 19 | 20 | roscpp 21 | sensor_msgs 22 | dynamic_reconfigure 23 | sensor_fusion_comm 24 | eigen 25 | cmake_modules 26 | 27 | 28 | roscpp 29 | sensor_msgs 30 | dynamic_reconfigure 31 | sensor_fusion_comm 32 | eigen 33 | 34 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_accbias: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting acceleration bias x y z [m/s^2] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[13]:data[14]:data[15] -b $T -t "acc bias" -l x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_attitude: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting attitude qw qx qy qz [quat] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[6]:data[7]:data[8]:data[9] -b $T -t "attitude" -l w,x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_calib: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting attitude q_ii w x y z [quat] ###" 11 | echo " ### plotting translation p_ic x y z [m] ###" 12 | echo " ### buffer = " $T "sec" 13 | 14 | rxplot ssf_core/state_out/data[21]:data[22]:data[23]:data[24] ssf_core/state_out/data[25]:data[26]:data[27] -b $T -t "attitude q_ic & translation p_ic" -l qw,qx,qy,qz,tx,ty,tz 15 | 16 | 17 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_gyrbias: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting gyro bias x y z [rad/s] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[10]:data[11]:data[12] -b $T -t "gyr bias" -l x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_position: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting position x y z [m] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[0]:data[1]:data[2] -b $T -t "position" -l x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_qvw: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting vision-world drift qw qx qy qz [quat] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[17]:data[18]:data[19]:data[20] -b $T -t "q_vw" -l w,x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_relevant: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting position x y z [m] ###" 11 | echo " ### plotting velocities x y z [m/s] ###" 12 | echo " ### plotting acceleration bias x y z [m/s^2] ###" 13 | echo " ### plotting scale ###" 14 | echo " ### buffer = " $T "sec" 15 | 16 | rxplot ssf_core/state_out/data[0]:data[1]:data[2] ssf_core/state_out/data[3]:data[4]:data[5] -b $T -t "position & velocity" -l px,py,pz,vx,vy,vz & 17 | rxplot ssf_core/state_out/data[13]:data[14]:data[15] ssf_core/state_out/data[16] -b $T -t "acc bias & scale" -l x,y,z,L 18 | 19 | 20 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_scale: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting scale ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[16] -b $T -t "scale" -l L 14 | -------------------------------------------------------------------------------- /ssf_core/scripts/plot_velocity: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -n "$1" ] 4 | then 5 | T=$1 6 | else 7 | T=20 8 | fi 9 | 10 | echo " ### plotting velocities x y z [m/s] ###" 11 | echo " ### buffer = " $T "sec" 12 | 13 | rxplot ssf_core/state_out/data[3]:data[4]:data[5] -b $T -t "velocity" -l x,y,z 14 | -------------------------------------------------------------------------------- /ssf_core/src/SSF_Core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | #include "calcQ.h" 34 | #include 35 | 36 | namespace ssf_core 37 | { 38 | 39 | SSF_Core::SSF_Core() 40 | { 41 | initialized_ = false; 42 | predictionMade_ = false; 43 | 44 | /// ros stuff 45 | ros::NodeHandle nh("ssf_core"); 46 | ros::NodeHandle pnh("~"); 47 | 48 | pubState_ = nh.advertise ("state_out", 1); 49 | pubCorrect_ = nh.advertise ("correction", 1); 50 | pubPose_ = nh.advertise ("pose", 1); 51 | pubPoseCrtl_ = nh.advertise ("ext_state", 1); 52 | msgState_.data.resize(nFullState_, 0); 53 | 54 | subImu_ = nh.subscribe("imu_state_input", 1 /*N_STATE_BUFFER*/, &SSF_Core::imuCallback, this); 55 | subState_ = nh.subscribe("hl_state_input", 1 /*N_STATE_BUFFER*/, &SSF_Core::stateCallback, this); 56 | 57 | msgCorrect_.state.resize(HLI_EKF_STATE_SIZE, 0); 58 | hl_state_buf_.state.resize(HLI_EKF_STATE_SIZE, 0); 59 | 60 | qvw_inittimer_ = 1; 61 | 62 | pnh.param("data_playback", data_playback_, false); 63 | reconfServer_ = new ReconfigureServer(ros::NodeHandle("~")); 64 | ReconfigureServer::CallbackType f = boost::bind(&SSF_Core::Config, this, _1, _2); 65 | reconfServer_->setCallback(f); 66 | //register dyn config list 67 | registerCallback(&SSF_Core::DynConfig, this); 68 | } 69 | 70 | SSF_Core::~SSF_Core() 71 | { 72 | delete reconfServer_; 73 | } 74 | 75 | void SSF_Core::initialize(const Eigen::Matrix & p, const Eigen::Matrix & v, 76 | const Eigen::Quaternion & q, const Eigen::Matrix & b_w, 77 | const Eigen::Matrix & b_a, const double & L, 78 | const Eigen::Quaternion & q_wv, const Eigen::Matrix & P, 79 | const Eigen::Matrix & w_m, const Eigen::Matrix & a_m, 80 | const Eigen::Matrix & g, const Eigen::Quaternion & q_ci, 81 | const Eigen::Matrix & p_ci) 82 | { 83 | initialized_ = false; 84 | predictionMade_ = false; 85 | qvw_inittimer_ = 1; 86 | 87 | // init state buffer 88 | for (int i = 0; i < N_STATE_BUFFER; i++) 89 | { 90 | StateBuffer_[i].reset(); 91 | } 92 | 93 | idx_state_ = 0; 94 | idx_P_ = 0; 95 | idx_time_ = 0; 96 | 97 | State & state = StateBuffer_[idx_state_]; 98 | state.p_ = p; 99 | state.v_ = v; 100 | state.q_ = q; 101 | state.b_w_ = b_w; 102 | state.b_a_ = b_a; 103 | state.L_ = L; 104 | state.q_wv_ = q_wv; 105 | state.q_ci_ = q_ci; 106 | state.p_ci_ = p_ci; 107 | state.w_m_ = w_m; 108 | state.q_int_ = state.q_wv_; 109 | state.a_m_ = a_m; 110 | state.time_ = ros::Time::now().toSec(); 111 | 112 | if (P.maxCoeff() == 0 && P.minCoeff() == 0) 113 | StateBuffer_[idx_P_].P_ << 114 | 0.016580786012789, 0.012199934386656, -0.001458808893504, 0.021111179657363, 0.007427567799788, 0.000037801439852, 0.001171469788518, -0.001169015812942, 0.000103349776558, -0.000003813309102, 0.000015542937454, -0.000004252270155, -0.000344432741256, -0.000188322508425, -0.000003798930056, 0.002878474013131, 0.000479648737527, 0.000160244196007, 0.000012449379372, -0.000025211583296, -0.000029240408089, -0.000001069329869, -0.001271299967766, -0.000133670678392, -0.003059838896447 115 | , 0.012906597122666, 0.050841902184280, -0.001973897835999, 0.017928487134657, 0.043154792703685, 0.000622902345606, 0.002031938336114, 0.000401913571459, -0.000231214341523, -0.000016591523613, 0.000011431341737, 0.000007932426867, 0.000311267088246, -0.000201092426841, 0.000004838759439, 0.008371265702599, -0.000186683528686, 0.000139783403254, 0.000070116051011, -0.000021128179249, -0.000028597234778, -0.000006006222525, -0.002966959059502, 0.000313165520973, 0.003179854597069 116 | , -0.001345477564898, -0.000886479514041, 0.014171550800995, -0.002720150074738, 0.005673098074032, 0.007935105430084, 0.000687618072508, 0.000684952051662, 0.000022000355078, -0.000008608300759, -0.000000799656033, 0.000001107610267, -0.000106383032603, -0.000356814673233, -0.000068763009837, -0.000051146093497, -0.000091362447823, 0.000293945574578, -0.000256092019589, 0.000042269002771, -0.000009567778418, -0.000017167287470, 0.004592386869817, -0.001581055638926, 0.000227387610329 117 | , 0.020963436713918, 0.016241565921214, -0.002606622877434, 0.043695944809847, 0.008282523689966, -0.001656117837207, 0.001638402584126, -0.002060006975745, -0.001362992588971, -0.000001331527123, 0.000032032914797, 0.000004134961242, 0.000341541553429, -0.000100600014193, 0.000025055557965, 0.003723777310732, -0.000161259841873, 0.000175908029926, -0.000010843973378, -0.000001022919132, -0.000020982262562, -0.000009716850289, -0.002231080476166, -0.001033766890345, -0.003628168927273 118 | , 0.009314922877817, 0.046059780658109, 0.003565024589881, 0.015262116382857, 0.065035219304194, -0.001635353752413, 0.002492076189539, 0.001255538625264, -0.000034886338628, -0.000029672138211, 0.000006695719137, 0.000006779584634, 0.000273857318856, 0.000241559075524, 0.000026819562998, 0.007341077421410, -0.000245364703147, -0.000214640089519, 0.000072765069578, -0.000031941424035, 0.000014164172022, -0.000014177340183, -0.000530959567309, 0.000080230949640, 0.003376885297505 119 | , -0.000029025742686, 0.000535037190485, 0.007958782884182, -0.001871298319530, -0.002083832757411, 0.012983170487598, 0.000132746916981, 0.000083483650298, 0.000020140288935, -0.000001280987614, 0.000000838029756, -0.000000023238638, -0.000309256650920, 0.000094250769772, -0.000143135502707, 0.000262797080980, 0.000133734202454, 0.000025809353285, 0.000051787574678, 0.000002954414724, -0.000012648552708, -0.000004097271489, 0.002381975267107, -0.001036906319084, 0.000115868771739 120 | , 0.001237915701080, 0.002441754382058, 0.000642141528976, 0.001714303831639, 0.003652445463202, 0.000133021899909, 0.000491964329936, 0.000029132708361, 0.000054571029310, -0.000003531797659, 0.000002108308557, -0.000000655503604, -0.000036221301269, -0.000080404390258, -0.000002011184920, 0.000409618760249, 0.000006455600111, 0.000037893047554, 0.000004332215700, -0.000003727533693, 0.000000308858737, -0.000004128771100, 0.000121407327690, -0.000116077155506, -0.000044599164311 121 | , -0.001129210933568, 0.000810737713225, 0.000687013243217, -0.002320565048774, 0.001923423915051, 0.000083505758388, 0.000045906211371, 0.000464144924949, -0.000074174151652, -0.000001593433385, -0.000002820148135, 0.000001999456261, 0.000068256370057, -0.000050158974131, -0.000000228078959, 0.000046796063511, -0.000043197112362, 0.000007902785285, 0.000000020609692, 0.000001805172252, 0.000002146994103, 0.000005750401157, 0.000309103513087, 0.000176510147723, 0.000423690330719 122 | , 0.000118011626188, -0.000151939328593, -0.000003895302246, -0.001370909458095, 0.000050912424428, 0.000014452281684, 0.000048567151385, -0.000077773340951, 0.000550829253488, -0.000001499983629, -0.000001785224358, -0.000005364537487, 0.000036601273545, 0.000003384325422, -0.000000535444414, -0.000032994187143, -0.000004973649389, -0.000005428744590, 0.000002850997192, -0.000006378420798, -0.000000001181394, -0.000014301726522, 0.000038455607205, 0.000110350938971, -0.000142528866262 123 | , -0.000005270401860, -0.000021814853820, -0.000010366987197, -0.000002004330853, -0.000038399333509, -0.000001674413901, -0.000004404646641, -0.000002139516677, -0.000001756665835, 0.000002030485308, -0.000000003944807, 0.000000005740984, 0.000000210906625, 0.000000302650227, 0.000000014520529, -0.000003266286919, -0.000000158321546, -0.000000508006293, -0.000000000135721, -0.000000498539464, 0.000000163904942, 0.000000129053161, -0.000003222034988, 0.000000064481380, -0.000001109329693 124 | , 0.000016356223202, 0.000012074093112, -0.000001861055809, 0.000034349032581, 0.000006058258467, 0.000000706161071, 0.000001988651054, -0.000003017460220, -0.000001874017262, -0.000000012182671, 0.000002030455681, -0.000000019800818, 0.000000488355222, 0.000001489016879, 0.000000028100385, 0.000002786101595, -0.000000046249993, 0.000000097139883, 0.000000389735880, -0.000000195417410, -0.000000460262829, 0.000000210319469, -0.000002235134510, -0.000002851445699, -0.000002883729469 125 | , -0.000003154072126, 0.000010432789869, 0.000002047297121, 0.000005626984656, 0.000009913025254, 0.000000398401049, -0.000000326490919, 0.000002058769308, -0.000005291111547, 0.000000001086789, 0.000000001772501, 0.000002006545689, 0.000000044716134, 0.000000414518295, -0.000000135444520, 0.000001531318739, -0.000000211673436, 0.000000405677050, -0.000000796855836, -0.000000266538355, -0.000000133632439, -0.000000338622240, -0.000000150597295, -0.000000563086699, 0.000003088758497 126 | , -0.000348907202366, 0.000314489658858, -0.000097981489533, 0.000332751125893, 0.000276947396796, -0.000311267592250, -0.000035302086269, 0.000070545012901, 0.000036626247889, 0.000000400828580, 0.000000087733422, 0.000000120709451, 0.001026573886639, 0.000013867120528, 0.000031828760993, 0.000009746783802, -0.000458840039830, -0.000019468671558, -0.000043520866307, 0.000007245947338, 0.000003901799711, -0.000004201599512, -0.000047176373840, 0.000119567188660, 0.000003684858444 127 | , -0.000190283000907, -0.000192352300127, -0.000359131551235, -0.000107453347870, 0.000258576553615, 0.000091496162086, -0.000081280254994, -0.000048304910474, 0.000002800928601, 0.000000908905402, 0.000001125333299, 0.000000471832044, 0.000019874619416, 0.001029579153516, 0.000011053406779, 0.000021449316681, 0.000016006639334, -0.000412772225495, 0.000006993477540, 0.000002648721730, 0.000004792699830, -0.000004141354722, -0.000083992422256, 0.000015935718681, -0.000000338251253 128 | , -0.000004368584055, 0.000003124910665, -0.000067807653083, 0.000024474336501, 0.000022105549875, -0.000144033820704, -0.000002164571960, -0.000000083713348, -0.000000674226005, 0.000000019237635, 0.000000025526504, -0.000000057252892, 0.000032366581999, 0.000010736184803, 0.000111095066893, 0.000000615680626, -0.000015341510438, -0.000007700695237, -0.000023026256094, 0.000000638926195, 0.000000960343604, 0.000000817586113, -0.000026575050709, 0.000013993827719, -0.000002316938385 129 | , 0.002973222331656, 0.008292388147295, -0.000211655385599, 0.003951267473552, 0.006718811356807, 0.000277369882917, 0.000349425829596, -0.000014812000602, -0.000045952715508, -0.000002513020002, 0.000002692914948, 0.000001078825296, 0.000009897987444, 0.000020034595279, 0.000000809851157, 0.001554211174363, 0.000023959770856, -0.000037670361809, -0.000009320812655, -0.000004598853139, -0.000006284196194, -0.000000693801636, -0.000469324632849, 0.000014818785588, 0.000277219840791 130 | , 0.000476557664133, -0.000191539372645, -0.000089666716294, -0.000163721235917, -0.000235017605089, 0.000134712473215, 0.000007671308678, -0.000041648250772, -0.000005375975547, 0.000000156986772, 0.000000504340505, -0.000000198574002, -0.000458130878121, 0.000014584188938, -0.000015616513739, 0.000023678958593, 0.000535136781135, -0.000016449781236, 0.000040831795426, -0.000013702650244, -0.000000627377616, -0.000004196881223, 0.000002230529685, -0.000050724631819, -0.000004714535751 131 | , 0.000162219848991, 0.000116427796874, 0.000292562152669, 0.000173404902614, -0.000249216364740, 0.000026816594889, 0.000036367682776, 0.000005763510102, -0.000005320926337, -0.000000071291000, -0.000000112152457, 0.000000334342568, -0.000022684595881, -0.000410859858969, -0.000007890929454, -0.000040454023111, -0.000011131820455, 0.000458907544194, -0.000005285694195, 0.000002246982110, -0.000002222041169, 0.000001951461640, 0.000047488638766, -0.000029510929794, 0.000005816436594 132 | , 0.000010794825884, 0.000058045653749, -0.000260506684499, -0.000007544850373, 0.000048451414581, 0.000048500128303, 0.000002555777025, -0.000001118968589, 0.000001725856751, 0.000000113523451, 0.000000356160739, -0.000000287211392, -0.000041197824317, 0.000004749859562, -0.000021745597805, -0.000011794173035, 0.000040317421040, -0.000001104681255, 0.000325476240984, 0.000006084247746, -0.000006253095726, -0.000005627495374, 0.000013663440542, -0.000012536337446, 0.000000477230568 133 | , -0.000028222744852, -0.000029726624789, 0.000042365440829, -0.000004529013669, -0.000041974513687, 0.000002547416367, -0.000004149622895, 0.000001656132079, -0.000006464228083, -0.000000593440587, -0.000000063566120, -0.000000230872869, 0.000007212338790, 0.000002222629345, 0.000000642817161, -0.000006111733946, -0.000013813495990, 0.000002643879751, 0.000005887006479, 0.000020142991502, -0.000000692093175, -0.000000188761575, 0.000017519903352, -0.000002456326732, 0.000001576856355 134 | , -0.000026132063406, -0.000024675067133, -0.000008452766004, -0.000014350608058, 0.000014404004024, -0.000011620075371, 0.000000539065468, 0.000001829895964, -0.000000462890431, 0.000000223093202, -0.000000499925964, -0.000000094710754, 0.000003954308159, 0.000004249241909, 0.000000876422290, -0.000005419924437, -0.000001021458192, -0.000002052781175, -0.000007397128908, -0.000000347703730, 0.000021540076832, 0.000001455562847, 0.000005351749933, 0.000020079632692, 0.000006997090317 135 | , 0.000001606076924, 0.000001031428045, -0.000015843471685, -0.000005357648114, -0.000007152430254, -0.000003359339850, -0.000003466742259, 0.000005980188844, -0.000014512044407, 0.000000136766387, 0.000000188396487, -0.000000299050190, -0.000004280062694, -0.000005018186182, 0.000000751147421, 0.000000382366121, -0.000004319412270, 0.000002858658354, -0.000005774838189, -0.000000199234914, 0.000001477444848, 0.000021955531390, -0.000005912741153, 0.000006848954650, 0.000000718992109 136 | , -0.001250410021685, -0.002465752118803, 0.004640769479530, -0.002397333962665, 0.000543954908379, 0.002370095810071, 0.000159513911164, 0.000327435894035, 0.000051354259180, -0.000002658607585, -0.000001766738193, -0.000000182288648, -0.000049404478395, -0.000084546262756, -0.000026628375388, -0.000398670523051, 0.000000139079122, 0.000048715190023, 0.000014902392001, 0.000017378375266, 0.000005675773452, -0.000005943594846, 0.013030218966816, 0.002362333360404, 0.000426396397327 137 | , -0.000130856879780, 0.000387010914370, -0.001570485481930, -0.001207751008090, 0.000021063199750, -0.001030927710933, -0.000109925957135, 0.000181001368406, 0.000107869867108, 0.000000177851848, -0.000002935702240, -0.000000493441232, 0.000119019560571, 0.000014103264454, 0.000013824858652, 0.000027253599949, -0.000051452899775, -0.000028435304764, -0.000013422029969, -0.000002043413021, 0.000020290127027, 0.000006914337519, 0.002362694187196, 0.016561843614191, 0.000974154946980 138 | , -0.002974278550351, 0.003344054784873, 0.000125156378167, -0.003468124255435, 0.003442635413150, 0.000109148337164, -0.000076026755915, 0.000385370025486, -0.000148952839125, -0.000000760036995, -0.000002603545684, 0.000003064524894, 0.000001812974918, -0.000002381321630, -0.000002469614200, 0.000309057481545, -0.000004492645187, 0.000007689077401, 0.000001009062356, 0.000001877737433, 0.000007317725714, 0.000000467906597, 0.000372138697091, 0.000966188804360, 0.011550623767300; 139 | else 140 | StateBuffer_[idx_P_].P_ = P; 141 | 142 | // constants 143 | g_ = g; 144 | 145 | // buffer for vision failure check 146 | qvw_inittimer_ = 1; 147 | qbuff_ = Eigen::Matrix::Constant(0); 148 | 149 | // init external propagation 150 | msgCorrect_.header.stamp = ros::Time::now(); 151 | msgCorrect_.header.seq = 0; 152 | msgCorrect_.angular_velocity.x = 0; 153 | msgCorrect_.angular_velocity.y = 0; 154 | msgCorrect_.angular_velocity.z = 0; 155 | msgCorrect_.linear_acceleration.x = 0; 156 | msgCorrect_.linear_acceleration.y = 0; 157 | msgCorrect_.linear_acceleration.z = 0; 158 | msgCorrect_.state[0] = state.p_[0]; 159 | msgCorrect_.state[1] = state.p_[1]; 160 | msgCorrect_.state[2] = state.p_[2]; 161 | msgCorrect_.state[3] = state.v_[0]; 162 | msgCorrect_.state[4] = state.v_[1]; 163 | msgCorrect_.state[5] = state.v_[2]; 164 | msgCorrect_.state[6] = state.q_.w(); 165 | msgCorrect_.state[7] = state.q_.x(); 166 | msgCorrect_.state[8] = state.q_.y(); 167 | msgCorrect_.state[9] = state.q_.z(); 168 | msgCorrect_.state[10] = state.b_w_[0]; 169 | msgCorrect_.state[11] = state.b_w_[1]; 170 | msgCorrect_.state[12] = state.b_w_[2]; 171 | msgCorrect_.state[13] = state.b_a_[0]; 172 | msgCorrect_.state[14] = state.b_a_[1]; 173 | msgCorrect_.state[15] = state.b_a_[2]; 174 | msgCorrect_.flag = sensor_fusion_comm::ExtEkf::initialization; 175 | pubCorrect_.publish(msgCorrect_); 176 | 177 | // increase state pointers 178 | idx_state_++; 179 | idx_P_++; 180 | 181 | initialized_ = true; 182 | } 183 | 184 | 185 | void SSF_Core::imuCallback(const sensor_msgs::ImuConstPtr & msg) 186 | { 187 | 188 | if (!initialized_) 189 | return; // // early abort // // 190 | 191 | StateBuffer_[idx_state_].time_ = msg->header.stamp.toSec(); 192 | 193 | static int seq = 0; 194 | 195 | // get inputs 196 | StateBuffer_[idx_state_].a_m_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; 197 | StateBuffer_[idx_state_].w_m_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; 198 | 199 | // remove acc spikes (TODO: find a cleaner way to do this) 200 | static Eigen::Matrix last_am = Eigen::Matrix(0, 0, 0); 201 | if (StateBuffer_[idx_state_].a_m_.norm() > 50) 202 | StateBuffer_[idx_state_].a_m_ = last_am; 203 | else 204 | last_am = StateBuffer_[idx_state_].a_m_; 205 | 206 | if (!predictionMade_) 207 | { 208 | if (fabs(StateBuffer_[(unsigned char)(idx_state_)].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_) > 5) 209 | { 210 | ROS_WARN_STREAM_THROTTLE(2, "large time-gap re-initializing to last state\n"); 211 | StateBuffer_[(unsigned char)(idx_state_ - 1)].time_ = StateBuffer_[(idx_state_)].time_; 212 | return; // // early abort // // (if timegap too big) 213 | } 214 | } 215 | 216 | propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_); 217 | predictProcessCovariance(StateBuffer_[idx_P_].time_ - StateBuffer_[(unsigned char)(idx_P_ - 1)].time_); 218 | 219 | checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].p_[0]), 3, "prediction p"); 220 | 221 | predictionMade_ = true; 222 | 223 | msgPose_.header.stamp = msg->header.stamp; 224 | msgPose_.header.seq = msg->header.seq; 225 | 226 | StateBuffer_[(unsigned char)(idx_state_ - 1)].toPoseMsg(msgPose_); 227 | pubPose_.publish(msgPose_); 228 | 229 | msgPoseCtrl_.header = msgPose_.header; 230 | StateBuffer_[(unsigned char)(idx_state_ - 1)].toExtStateMsg(msgPoseCtrl_); 231 | pubPoseCrtl_.publish(msgPoseCtrl_); 232 | 233 | seq++; 234 | } 235 | 236 | 237 | void SSF_Core::stateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg) 238 | { 239 | 240 | if (!initialized_) 241 | return; // // early abort // // 242 | 243 | StateBuffer_[idx_state_].time_ = msg->header.stamp.toSec(); 244 | 245 | static int seq = 0; 246 | 247 | // get inputs 248 | StateBuffer_[idx_state_].a_m_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; 249 | StateBuffer_[idx_state_].w_m_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; 250 | 251 | // remove acc spikes (TODO: find a cleaner way to do this) 252 | static Eigen::Matrix last_am = Eigen::Matrix(0, 0, 0); 253 | if (StateBuffer_[idx_state_].a_m_.norm() > 50) 254 | StateBuffer_[idx_state_].a_m_ = last_am; 255 | else 256 | last_am = StateBuffer_[idx_state_].a_m_; 257 | 258 | if (!predictionMade_) 259 | { 260 | if (fabs(StateBuffer_[(idx_state_)].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_) > 5) 261 | { 262 | ROS_WARN_STREAM_THROTTLE(2, "large time-gap re-initializing to last state\n"); 263 | StateBuffer_[(unsigned char)(idx_state_ - 1)].time_ = StateBuffer_[(idx_state_)].time_; 264 | StateBuffer_[(unsigned char)(idx_state_)].time_ = 0; 265 | return; // // early abort // // (if timegap too big) 266 | } 267 | } 268 | 269 | int32_t flag = msg->flag; 270 | if (data_playback_) 271 | flag = sensor_fusion_comm::ExtEkf::ignore_state; 272 | 273 | bool isnumeric = true; 274 | if (flag == sensor_fusion_comm::ExtEkf::current_state) 275 | isnumeric = checkForNumeric(&msg->state[0], 10, "before prediction p,v,q"); 276 | 277 | isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_].p_[0]), 3, "before prediction p"); 278 | 279 | if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) // state propagation is made externally, so we read the actual state 280 | { 281 | StateBuffer_[idx_state_].p_ = Eigen::Matrix(msg->state[0], msg->state[1], msg->state[2]); 282 | StateBuffer_[idx_state_].v_ = Eigen::Matrix(msg->state[3], msg->state[4], msg->state[5]); 283 | StateBuffer_[idx_state_].q_ = Eigen::Quaternion(msg->state[6], msg->state[7], msg->state[8], msg->state[9]); 284 | StateBuffer_[idx_state_].q_.normalize(); 285 | 286 | // zero props: 287 | StateBuffer_[idx_state_].b_w_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].b_w_; 288 | StateBuffer_[idx_state_].b_a_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].b_a_; 289 | StateBuffer_[idx_state_].L_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].L_; 290 | StateBuffer_[idx_state_].q_wv_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].q_wv_; 291 | StateBuffer_[idx_state_].q_ci_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].q_ci_; 292 | StateBuffer_[idx_state_].p_ci_ = StateBuffer_[(unsigned char)(idx_state_ - 1)].p_ci_; 293 | idx_state_++; 294 | 295 | hl_state_buf_ = *msg; 296 | } 297 | else if (flag == sensor_fusion_comm::ExtEkf::ignore_state || !isnumeric) // otherwise let's do the state prop. here 298 | propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_); 299 | 300 | predictProcessCovariance(StateBuffer_[idx_P_].time_ - StateBuffer_[(unsigned char)(idx_P_ - 1)].time_); 301 | 302 | isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].p_[0]), 3, "prediction p"); 303 | isnumeric = checkForNumeric((double*)(&StateBuffer_[idx_state_ - 1].P_(0)), N_STATE * N_STATE, "prediction done P"); 304 | 305 | predictionMade_ = true; 306 | 307 | msgPose_.header.stamp = msg->header.stamp; 308 | msgPose_.header.seq = msg->header.seq; 309 | 310 | StateBuffer_[(unsigned char)(idx_state_ - 1)].toPoseMsg(msgPose_); 311 | pubPose_.publish(msgPose_); 312 | 313 | msgPoseCtrl_.header = msgPose_.header; 314 | StateBuffer_[(unsigned char)(idx_state_ - 1)].toExtStateMsg(msgPoseCtrl_); 315 | pubPoseCrtl_.publish(msgPoseCtrl_); 316 | 317 | seq++; 318 | } 319 | 320 | 321 | void SSF_Core::propagateState(const double dt) 322 | { 323 | typedef const Eigen::Matrix ConstMatrix4; 324 | typedef const Eigen::Matrix ConstVector3; 325 | typedef Eigen::Matrix Matrix4; 326 | 327 | // get references to current and previous state 328 | State & cur_state = StateBuffer_[idx_state_]; 329 | State & prev_state = StateBuffer_[(unsigned char)(idx_state_ - 1)]; 330 | 331 | // zero props: 332 | cur_state.b_w_ = prev_state.b_w_; 333 | cur_state.b_a_ = prev_state.b_a_; 334 | cur_state.L_ = prev_state.L_; 335 | cur_state.q_wv_ = prev_state.q_wv_; 336 | cur_state.q_ci_ = prev_state.q_ci_; 337 | cur_state.p_ci_ = prev_state.p_ci_; 338 | 339 | // Eigen::Quaternion dq; 340 | Eigen::Matrix dv; 341 | ConstVector3 ew = cur_state.w_m_ - cur_state.b_w_; 342 | ConstVector3 ewold = prev_state.w_m_ - prev_state.b_w_; 343 | ConstVector3 ea = cur_state.a_m_ - cur_state.b_a_; 344 | ConstVector3 eaold = prev_state.a_m_ - prev_state.b_a_; 345 | ConstMatrix4 Omega = omegaMatJPL(ew); 346 | ConstMatrix4 OmegaOld = omegaMatJPL(ewold); 347 | Matrix4 OmegaMean = omegaMatJPL((ew + ewold) / 2); 348 | 349 | // zero order quaternion integration 350 | // cur_state.q_ = (Eigen::Matrix::Identity() + 0.5*Omega*dt)*StateBuffer_[(unsigned char)(idx_state_-1)].q_.coeffs(); 351 | 352 | // first order quaternion integration, this is kind of costly and may not add a lot to the quality of propagation... 353 | int div = 1; 354 | Matrix4 MatExp; 355 | MatExp.setIdentity(); 356 | OmegaMean *= 0.5 * dt; 357 | for (int i = 1; i < 5; i++) 358 | { 359 | div *= i; 360 | MatExp = MatExp + OmegaMean / div; 361 | OmegaMean *= OmegaMean; 362 | } 363 | 364 | // first oder quat integration matrix 365 | ConstMatrix4 quat_int = MatExp + 1.0 / 48.0 * (Omega * OmegaOld - OmegaOld * Omega) * dt * dt; 366 | 367 | // first oder quaternion integration 368 | cur_state.q_.coeffs() = quat_int * prev_state.q_.coeffs(); 369 | cur_state.q_.normalize(); 370 | 371 | // first oder quaternion integration 372 | cur_state.q_int_.coeffs() = quat_int * prev_state.q_int_.coeffs(); 373 | cur_state.q_int_.normalize(); 374 | 375 | dv = (cur_state.q_.toRotationMatrix() * ea + prev_state.q_.toRotationMatrix() * eaold) / 2; 376 | cur_state.v_ = prev_state.v_ + (dv - g_) * dt; 377 | cur_state.p_ = prev_state.p_ + ((cur_state.v_ + prev_state.v_) / 2 * dt); 378 | idx_state_++; 379 | } 380 | 381 | 382 | void SSF_Core::predictProcessCovariance(const double dt) 383 | { 384 | 385 | typedef const Eigen::Matrix ConstMatrix3; 386 | typedef const Eigen::Matrix ConstVector3; 387 | typedef Eigen::Vector3d Vector3; 388 | 389 | // noises 390 | ConstVector3 nav = Vector3::Constant(config_.noise_acc /* / sqrt(dt) */); 391 | ConstVector3 nbav = Vector3::Constant(config_.noise_accbias /* * sqrt(dt) */); 392 | 393 | ConstVector3 nwv = Vector3::Constant(config_.noise_gyr /* / sqrt(dt) */); 394 | ConstVector3 nbwv = Vector3::Constant(config_.noise_gyrbias /* * sqrt(dt) */); 395 | 396 | ConstVector3 nqwvv = Eigen::Vector3d::Constant(config_.noise_qwv); 397 | ConstVector3 nqciv = Eigen::Vector3d::Constant(config_.noise_qci); 398 | ConstVector3 npicv = Eigen::Vector3d::Constant(config_.noise_pic); 399 | 400 | // bias corrected IMU readings 401 | ConstVector3 ew = StateBuffer_[idx_P_].w_m_ - StateBuffer_[idx_P_].b_w_; 402 | ConstVector3 ea = StateBuffer_[idx_P_].a_m_ - StateBuffer_[idx_P_].b_a_; 403 | 404 | ConstMatrix3 a_sk = skew(ea); 405 | ConstMatrix3 w_sk = skew(ew); 406 | ConstMatrix3 eye3 = Eigen::Matrix::Identity(); 407 | 408 | ConstMatrix3 C_eq = StateBuffer_[idx_P_].q_.toRotationMatrix(); 409 | 410 | const double dt_p2_2 = dt * dt * 0.5; // dt^2 / 2 411 | const double dt_p3_6 = dt_p2_2 * dt / 3.0; // dt^3 / 6 412 | const double dt_p4_24 = dt_p3_6 * dt * 0.25; // dt^4 / 24 413 | const double dt_p5_120 = dt_p4_24 * dt * 0.2; // dt^5 / 120 414 | 415 | ConstMatrix3 Ca3 = C_eq * a_sk; 416 | ConstMatrix3 A = Ca3 * (-dt_p2_2 * eye3 + dt_p3_6 * w_sk - dt_p4_24 * w_sk * w_sk); 417 | ConstMatrix3 B = Ca3 * (dt_p3_6 * eye3 - dt_p4_24 * w_sk + dt_p5_120 * w_sk * w_sk); 418 | ConstMatrix3 D = -A; 419 | ConstMatrix3 E = eye3 - dt * w_sk + dt_p2_2 * w_sk * w_sk; 420 | ConstMatrix3 F = -dt * eye3 + dt_p2_2 * w_sk - dt_p3_6 * (w_sk * w_sk); 421 | ConstMatrix3 C = Ca3 * F; 422 | 423 | // discrete error state propagation Matrix Fd according to: 424 | // Stephan Weiss and Roland Siegwart. 425 | // Real-Time Metric State Estimation for Modular Vision-Inertial Systems. 426 | // IEEE International Conference on Robotics and Automation. Shanghai, China, 2011 427 | Fd_.setIdentity(); 428 | Fd_.block<3, 3> (0, 3) = dt * eye3; 429 | Fd_.block<3, 3> (0, 6) = A; 430 | Fd_.block<3, 3> (0, 9) = B; 431 | Fd_.block<3, 3> (0, 12) = -C_eq * dt_p2_2; 432 | 433 | Fd_.block<3, 3> (3, 6) = C; 434 | Fd_.block<3, 3> (3, 9) = D; 435 | Fd_.block<3, 3> (3, 12) = -C_eq * dt; 436 | 437 | Fd_.block<3, 3> (6, 6) = E; 438 | Fd_.block<3, 3> (6, 9) = F; 439 | 440 | calc_Q(dt, StateBuffer_[idx_P_].q_, ew, ea, nav, nbav, nwv, nbwv, config_.noise_scale, nqwvv, nqciv, npicv, Qd_); 441 | StateBuffer_[idx_P_].P_ = Fd_ * StateBuffer_[(unsigned char)(idx_P_ - 1)].P_ * Fd_.transpose() + Qd_; 442 | 443 | idx_P_++; 444 | } 445 | 446 | 447 | bool SSF_Core::getStateAtIdx(State* timestate, unsigned char idx) 448 | { 449 | if (!predictionMade_) 450 | { 451 | timestate->time_ = -1; 452 | return false; 453 | } 454 | 455 | *timestate = StateBuffer_[idx]; 456 | 457 | return true; 458 | } 459 | 460 | unsigned char SSF_Core::getClosestState(State* timestate, ros::Time tstamp, double delay) 461 | { 462 | if (!predictionMade_) 463 | { 464 | timestate->time_ = -1; 465 | return false; 466 | } 467 | 468 | unsigned char idx = (unsigned char)(idx_state_ - 1); 469 | double timedist = 1e100; 470 | double timenow = tstamp.toSec() - delay - config_.delay; 471 | 472 | while (fabs(timenow - StateBuffer_[idx].time_) < timedist) // timedist decreases continuously until best point reached... then rises again 473 | { 474 | timedist = fabs(timenow - StateBuffer_[idx].time_); 475 | idx--; 476 | } 477 | idx++; // we subtracted one too much before.... 478 | 479 | static bool started = false; 480 | if (idx == 1 && !started) 481 | idx = 2; 482 | started = true; 483 | 484 | if (StateBuffer_[idx].time_ == 0) 485 | return false; // // early abort // // not enough predictions made yet to apply measurement (too far in past) 486 | 487 | propPToIdx(idx); // catch up with covariance propagation if necessary 488 | 489 | *timestate = StateBuffer_[idx]; 490 | 491 | return idx; 492 | } 493 | 494 | void SSF_Core::propPToIdx(unsigned char idx) 495 | { 496 | // propagate cov matrix until idx 497 | if (idxidx_state_)) //need to propagate some covs 498 | while (idx!=(unsigned char)(idx_P_-1)) 499 | predictProcessCovariance(StateBuffer_[idx_P_].time_-StateBuffer_[(unsigned char)(idx_P_-1)].time_); 500 | } 501 | 502 | bool SSF_Core::applyCorrection(unsigned char idx_delaystate, const ErrorState & res_delayed, double fuzzythres) 503 | { 504 | static int seq_m = 0; 505 | if (config_.fixed_scale) 506 | { 507 | correction_(15) = 0; //scale 508 | } 509 | 510 | if (config_.fixed_bias) 511 | { 512 | correction_(9) = 0; //acc bias x 513 | correction_(10) = 0; //acc bias y 514 | correction_(11) = 0; //acc bias z 515 | correction_(12) = 0; //gyro bias x 516 | correction_(13) = 0; //gyro bias y 517 | correction_(14) = 0; //gyro bias z 518 | } 519 | 520 | if (config_.fixed_calib) 521 | { 522 | correction_(19) = 0; //q_ic roll 523 | correction_(20) = 0; //q_ic pitch 524 | correction_(21) = 0; //q_ic yaw 525 | correction_(22) = 0; //p_ci x 526 | correction_(23) = 0; //p_ci y 527 | correction_(24) = 0; //p_ci z 528 | } 529 | 530 | // state update: 531 | 532 | // store old values in case of fuzzy tracking 533 | // TODO: what to do with attitude? augment measurement noise? 534 | 535 | State & delaystate = StateBuffer_[idx_delaystate]; 536 | 537 | const Eigen::Matrix buff_bw = delaystate.b_w_; 538 | const Eigen::Matrix buff_ba = delaystate.b_a_; 539 | const double buff_L = delaystate.L_; 540 | const Eigen::Quaternion buff_qwv = delaystate.q_wv_; 541 | const Eigen::Quaternion buff_qci = delaystate.q_ci_; 542 | const Eigen::Matrix buff_pic = delaystate.p_ci_; 543 | 544 | delaystate.p_ = delaystate.p_ + correction_.block<3, 1> (0, 0); 545 | delaystate.v_ = delaystate.v_ + correction_.block<3, 1> (3, 0); 546 | delaystate.b_w_ = delaystate.b_w_ + correction_.block<3, 1> (9, 0); 547 | delaystate.b_a_ = delaystate.b_a_ + correction_.block<3, 1> (12, 0); 548 | delaystate.L_ = delaystate.L_ + correction_(15); 549 | if (delaystate.L_ < 0) 550 | { 551 | ROS_WARN_STREAM_THROTTLE(1,"Negative scale detected: " << delaystate.L_ << ". Correcting to 0.1"); 552 | delaystate.L_ = 0.1; 553 | } 554 | 555 | Eigen::Quaternion qbuff_q = quaternionFromSmallAngle(correction_.block<3, 1> (6, 0)); 556 | delaystate.q_ = delaystate.q_ * qbuff_q; 557 | delaystate.q_.normalize(); 558 | 559 | Eigen::Quaternion qbuff_qwv = quaternionFromSmallAngle(correction_.block<3, 1> (16, 0)); 560 | delaystate.q_wv_ = delaystate.q_wv_ * qbuff_qwv; 561 | delaystate.q_wv_.normalize(); 562 | 563 | Eigen::Quaternion qbuff_qci = quaternionFromSmallAngle(correction_.block<3, 1> (19, 0)); 564 | delaystate.q_ci_ = delaystate.q_ci_ * qbuff_qci; 565 | delaystate.q_ci_.normalize(); 566 | 567 | delaystate.p_ci_ = delaystate.p_ci_ + correction_.block<3, 1> (22, 0); 568 | 569 | // update qbuff_ and check for fuzzy tracking 570 | if (qvw_inittimer_ > nBuff_) 571 | { 572 | // should be unit quaternion if no error 573 | Eigen::Quaternion errq = delaystate.q_wv_.conjugate() * 574 | Eigen::Quaternion( 575 | getMedian(qbuff_.block (0, 3)), 576 | getMedian(qbuff_.block (0, 0)), 577 | getMedian(qbuff_.block (0, 1)), 578 | getMedian(qbuff_.block (0, 2)) 579 | ); 580 | 581 | if (std::max(errq.vec().maxCoeff(), -errq.vec().minCoeff()) / fabs(errq.w()) * 2 > fuzzythres) // fuzzy tracking (small angle approx) 582 | { 583 | ROS_WARN_STREAM_THROTTLE(1,"fuzzy tracking triggered: " << std::max(errq.vec().maxCoeff(), -errq.vec().minCoeff())/fabs(errq.w())*2 << " limit: " << fuzzythres <<"\n"); 584 | 585 | //state_.q_ = buff_q; 586 | delaystate.b_w_ = buff_bw; 587 | delaystate.b_a_ = buff_ba; 588 | delaystate.L_ = buff_L; 589 | delaystate.q_wv_ = buff_qwv; 590 | delaystate.q_ci_ = buff_qci; 591 | delaystate.p_ci_ = buff_pic; 592 | correction_.block<16, 1> (9, 0) = Eigen::Matrix::Zero(); 593 | qbuff_q.setIdentity(); 594 | qbuff_qwv.setIdentity(); 595 | qbuff_qci.setIdentity(); 596 | } 597 | else // if tracking ok: update mean and 3sigma of past N q_vw's 598 | { 599 | qbuff_.block<1, 4> (qvw_inittimer_ - nBuff_ - 1, 0) = Eigen::Matrix(delaystate.q_wv_.coeffs()); 600 | qvw_inittimer_ = (qvw_inittimer_) % nBuff_ + nBuff_ + 1; 601 | } 602 | } 603 | else // at beginning get mean and 3sigma of past N q_vw's 604 | { 605 | qbuff_.block<1, 4> (qvw_inittimer_ - 1, 0) = Eigen::Matrix(delaystate.q_wv_.coeffs()); 606 | qvw_inittimer_++; 607 | } 608 | 609 | // idx fiddeling to ensure correct update until now from the past 610 | idx_time_ = idx_state_; 611 | idx_state_ = idx_delaystate + 1; 612 | idx_P_ = idx_delaystate + 1; 613 | 614 | // propagate state matrix until now 615 | while (idx_state_ != idx_time_) 616 | propagateState(StateBuffer_[idx_state_].time_ - StateBuffer_[(unsigned char)(idx_state_ - 1)].time_); 617 | 618 | checkForNumeric(&correction_[0], HLI_EKF_STATE_SIZE, "update"); 619 | 620 | // publish correction for external propagation 621 | msgCorrect_.header.stamp = ros::Time::now(); 622 | msgCorrect_.header.seq = seq_m; 623 | msgCorrect_.angular_velocity.x = 0; 624 | msgCorrect_.angular_velocity.y = 0; 625 | msgCorrect_.angular_velocity.z = 0; 626 | msgCorrect_.linear_acceleration.x = 0; 627 | msgCorrect_.linear_acceleration.y = 0; 628 | msgCorrect_.linear_acceleration.z = 0; 629 | 630 | const unsigned char idx = (unsigned char)(idx_state_ - 1); 631 | msgCorrect_.state[0] = StateBuffer_[idx].p_[0] - hl_state_buf_.state[0]; 632 | msgCorrect_.state[1] = StateBuffer_[idx].p_[1] - hl_state_buf_.state[1]; 633 | msgCorrect_.state[2] = StateBuffer_[idx].p_[2] - hl_state_buf_.state[2]; 634 | msgCorrect_.state[3] = StateBuffer_[idx].v_[0] - hl_state_buf_.state[3]; 635 | msgCorrect_.state[4] = StateBuffer_[idx].v_[1] - hl_state_buf_.state[4]; 636 | msgCorrect_.state[5] = StateBuffer_[idx].v_[2] - hl_state_buf_.state[5]; 637 | 638 | Eigen::Quaterniond hl_q(hl_state_buf_.state[6], hl_state_buf_.state[7], hl_state_buf_.state[8], hl_state_buf_.state[9]); 639 | qbuff_q = hl_q.inverse() * StateBuffer_[idx].q_; 640 | msgCorrect_.state[6] = qbuff_q.w(); 641 | msgCorrect_.state[7] = qbuff_q.x(); 642 | msgCorrect_.state[8] = qbuff_q.y(); 643 | msgCorrect_.state[9] = qbuff_q.z(); 644 | 645 | msgCorrect_.state[10] = StateBuffer_[idx].b_w_[0] - hl_state_buf_.state[10]; 646 | msgCorrect_.state[11] = StateBuffer_[idx].b_w_[1] - hl_state_buf_.state[11]; 647 | msgCorrect_.state[12] = StateBuffer_[idx].b_w_[2] - hl_state_buf_.state[12]; 648 | msgCorrect_.state[13] = StateBuffer_[idx].b_a_[0] - hl_state_buf_.state[13]; 649 | msgCorrect_.state[14] = StateBuffer_[idx].b_a_[1] - hl_state_buf_.state[14]; 650 | msgCorrect_.state[15] = StateBuffer_[idx].b_a_[2] - hl_state_buf_.state[15]; 651 | 652 | msgCorrect_.flag = sensor_fusion_comm::ExtEkf::state_correction; 653 | pubCorrect_.publish(msgCorrect_); 654 | 655 | // publish state 656 | msgState_.header = msgCorrect_.header; 657 | StateBuffer_[idx].toStateMsg(msgState_); 658 | pubState_.publish(msgState_); 659 | seq_m++; 660 | 661 | return 1; 662 | } 663 | 664 | void SSF_Core::Config(ssf_core::SSF_CoreConfig& config, uint32_t level) 665 | { 666 | for (std::vector::iterator it = callbacks_.begin(); it != callbacks_.end(); it++) 667 | (*it)(config, level); 668 | } 669 | 670 | void SSF_Core::DynConfig(ssf_core::SSF_CoreConfig& config, uint32_t level) 671 | { 672 | config_ = config; 673 | } 674 | 675 | double SSF_Core::getMedian(const Eigen::Matrix & data) 676 | { 677 | std::vector mediandistvec; 678 | mediandistvec.reserve(nBuff_); 679 | for (int i = 0; i < nBuff_; ++i) 680 | mediandistvec.push_back(data(i)); 681 | 682 | if (mediandistvec.size() > 0) 683 | { 684 | std::vector::iterator first = mediandistvec.begin(); 685 | std::vector::iterator last = mediandistvec.end(); 686 | std::vector::iterator middle = first + std::floor((last - first) / 2); 687 | std::nth_element(first, middle, last); // can specify comparator as optional 4th arg 688 | return *middle; 689 | } 690 | else 691 | return 0; 692 | } 693 | 694 | }; // end namespace 695 | -------------------------------------------------------------------------------- /ssf_core/src/calcQ.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef CALCQ_H_ 33 | #define CALCQ_H_ 34 | 35 | 36 | #include 37 | 38 | 39 | 40 | template void calc_Q( 41 | double dt, 42 | const Eigen::Quaternion & q, 43 | const Eigen::MatrixBase & ew, 44 | const Eigen::MatrixBase & ea, 45 | const Eigen::MatrixBase & n_a, 46 | const Eigen::MatrixBase & n_ba, 47 | const Eigen::MatrixBase & n_w, 48 | const Eigen::MatrixBase & n_bw, 49 | double n_L, 50 | const Eigen::MatrixBase & n_qvw, 51 | const Eigen::MatrixBase & n_qci, 52 | const Eigen::MatrixBase & n_pic, 53 | Eigen::MatrixBase & Qd) 54 | { 55 | 56 | double q1=q.w(), q2=q.x(), q3=q.y(), q4=q.z(); 57 | double ew1=ew(0), ew2=ew(1), ew3=ew(2); 58 | double ea1=ea(0), ea2=ea(1), ea3=ea(2); 59 | double n_a1=n_a(0), n_a2=n_a(1), n_a3=n_a(2); 60 | double n_ba1=n_ba(0), n_ba2=n_ba(1), n_ba3=n_ba(2); 61 | double n_w1=n_w(0), n_w2=n_w(1), n_w3=n_w(2); 62 | double n_bw1=n_bw(0), n_bw2=n_bw(1), n_bw3=n_bw(2); 63 | double n_qvw1=n_qvw(0), n_qvw2=n_qvw(1), n_qvw3=n_qvw(2); 64 | 65 | double t343 = dt*dt; 66 | double t348 = q1*q4*2.0; 67 | double t349 = q2*q3*2.0; 68 | double t344 = t348-t349; 69 | double t356 = q1*q3*2.0; 70 | double t357 = q2*q4*2.0; 71 | double t345 = t356+t357; 72 | double t350 = q1*q1; 73 | double t351 = q2*q2; 74 | double t352 = q3*q3; 75 | double t353 = q4*q4; 76 | double t346 = t350+t351-t352-t353; 77 | double t347 = n_a1*n_a1; 78 | double t354 = n_a2*n_a2; 79 | double t355 = n_a3*n_a3; 80 | double t358 = q1*q2*2.0; 81 | double t359 = t344*t344; 82 | double t360 = t345*t345; 83 | double t361 = t346*t346; 84 | double t363 = ea2*t345; 85 | double t364 = ea3*t344; 86 | double t362 = t363+t364; 87 | double t365 = t362*t362; 88 | double t366 = t348+t349; 89 | double t367 = t350-t351+t352-t353; 90 | double t368 = q3*q4*2.0; 91 | double t369 = t356-t357; 92 | double t370 = t350-t351-t352+t353; 93 | double t371 = n_w3*n_w3; 94 | double t372 = t358+t368; 95 | double t373 = n_w2*n_w2; 96 | double t374 = n_w1*n_w1; 97 | double t375 = dt*t343*t346*t347*t366*(1.0/3.0); 98 | double t376 = t358-t368; 99 | double t377 = t343*t346*t347*t366*(1.0/2.0); 100 | double t378 = t366*t366; 101 | double t379 = t376*t376; 102 | double t380 = ea1*t367; 103 | double t391 = ea2*t366; 104 | double t381 = t380-t391; 105 | double t382 = ea3*t367; 106 | double t383 = ea2*t376; 107 | double t384 = t382+t383; 108 | double t385 = t367*t367; 109 | double t386 = ea1*t376; 110 | double t387 = ea3*t366; 111 | double t388 = t386+t387; 112 | double t389 = ea2*t370; 113 | double t407 = ea3*t372; 114 | double t390 = t389-t407; 115 | double t392 = ea1*t372; 116 | double t393 = ea2*t369; 117 | double t394 = t392+t393; 118 | double t395 = ea1*t370; 119 | double t396 = ea3*t369; 120 | double t397 = t395+t396; 121 | double t398 = n_ba1*n_ba1; 122 | double t399 = n_ba2*n_ba2; 123 | double t400 = n_ba3*n_ba3; 124 | double t401 = dt*t343*t345*t355*t370*(1.0/3.0); 125 | double t402 = t401-dt*t343*t346*t347*t369*(1.0/3.0)-dt*t343*t344*t354*t372*(1.0/3.0); 126 | double t403 = dt*t343*t354*t367*t372*(1.0/3.0); 127 | double t404 = t403-dt*t343*t347*t366*t369*(1.0/3.0)-dt*t343*t355*t370*t376*(1.0/3.0); 128 | double t405 = t343*t345*t355*t370*(1.0/2.0); 129 | double t406 = dt*t343*t362*t373*t397*(1.0/6.0); 130 | double t421 = t343*t346*t347*t369*(1.0/2.0); 131 | double t422 = dt*t343*t362*t371*t394*(1.0/6.0); 132 | double t423 = t343*t344*t354*t372*(1.0/2.0); 133 | double t424 = dt*t343*t362*t374*t390*(1.0/6.0); 134 | double t408 = t405+t406-t421-t422-t423-t424; 135 | double t409 = t343*t354*t367*t372*(1.0/2.0); 136 | double t410 = dt*t343*t374*t384*t390*(1.0/6.0); 137 | double t411 = dt*t343*t373*t388*t397*(1.0/6.0); 138 | double t463 = t343*t355*t370*t376*(1.0/2.0); 139 | double t464 = t343*t347*t366*t369*(1.0/2.0); 140 | double t465 = dt*t343*t371*t381*t394*(1.0/6.0); 141 | double t412 = t409+t410+t411-t463-t464-t465; 142 | double t413 = t369*t369; 143 | double t414 = t372*t372; 144 | double t415 = t370*t370; 145 | double t416 = t343*t354*t359*(1.0/2.0); 146 | double t417 = t343*t355*t360*(1.0/2.0); 147 | double t418 = t343*t347*t361*(1.0/2.0); 148 | double t419 = t416+t417+t418-dt*t343*t365*t371*(1.0/6.0)-dt*t343*t365*t373*(1.0/6.0)-dt*t343*t365*t374*(1.0/6.0); 149 | double t453 = t343*t344*t354*t367*(1.0/2.0); 150 | double t454 = t343*t345*t355*t376*(1.0/2.0); 151 | double t420 = t377-t453-t454; 152 | double t426 = ew2*t362; 153 | double t427 = ew3*t362; 154 | double t425 = t426-t427; 155 | double t428 = dt*t365; 156 | double t429 = ew1*ew1; 157 | double t430 = ew2*ew2; 158 | double t431 = ew3*ew3; 159 | double t432 = t430+t431; 160 | double t433 = t362*t432; 161 | double t434 = ew1*t343*t365; 162 | double t435 = t429+t431; 163 | double t436 = t362*t435; 164 | double t443 = ew2*ew3*t362; 165 | double t437 = t433+t436-t443; 166 | double t438 = ew1*t362*t394; 167 | double t511 = ew1*t362*t397; 168 | double t439 = t438-t511; 169 | double t440 = t343*t439*(1.0/2.0); 170 | double t441 = t429+t430; 171 | double t442 = t362*t441; 172 | double t444 = t390*t432; 173 | double t445 = ew2*t394; 174 | double t446 = ew3*t397; 175 | double t447 = t445+t446; 176 | double t448 = ew1*ew2*t362; 177 | double t449 = ew1*ew3*t362; 178 | double t450 = ew1*ew3*t362*(1.0/2.0); 179 | double t451 = dt*t362; 180 | double t452 = ew1*t343*t362*(1.0/2.0); 181 | double t455 = dt*t343*t362*t374*t384*(1.0/6.0); 182 | double t456 = t343*t347*t378*(1.0/2.0); 183 | double t457 = t343*t355*t379*(1.0/2.0); 184 | double t458 = t381*t381; 185 | double t459 = t384*t384; 186 | double t460 = t343*t354*t385*(1.0/2.0); 187 | double t461 = t388*t388; 188 | double t462 = t456+t457+t460-dt*t343*t371*t458*(1.0/6.0)-dt*t343*t374*t459*(1.0/6.0)-dt*t343*t373*t461*(1.0/6.0); 189 | double t466 = t433+t442-t443; 190 | double t467 = ew1*t362*t388; 191 | double t468 = ew1*t362*t381; 192 | double t469 = t467+t468; 193 | double t470 = t343*t469*(1.0/2.0); 194 | double t471 = t384*t432; 195 | double t472 = ew2*t381; 196 | double t479 = ew3*t388; 197 | double t473 = t472-t479; 198 | double t474 = -t433+t448+t449; 199 | double t475 = dt*t343*t346*t366*t398*(1.0/3.0); 200 | double t476 = dt*t346*t347*t366; 201 | double t477 = ew2*ew3*t381; 202 | double t492 = t388*t435; 203 | double t478 = t471+t477-t492; 204 | double t480 = t472-t479; 205 | double t481 = ew1*ew3*t381; 206 | double t482 = ew1*ew2*t388; 207 | double t483 = t471+t481+t482; 208 | double t484 = ew2*ew3*t388; 209 | double t486 = t381*t441; 210 | double t485 = t471+t484-t486; 211 | double t487 = t394*t441; 212 | double t488 = ew2*ew3*t397; 213 | double t489 = t444+t487+t488; 214 | double t490 = t397*t435; 215 | double t491 = ew2*ew3*t394; 216 | double t493 = ew1*t381*t397; 217 | double t541 = ew1*t388*t394; 218 | double t494 = t493-t541; 219 | double t495 = t343*t494*(1.0/2.0); 220 | double t496 = ew1*ew2*t397; 221 | double t527 = ew1*ew3*t394; 222 | double t497 = t444+t496-t527; 223 | double t498 = ew2*ew3*t381*(1.0/2.0); 224 | double t499 = ew1*t343*t381*(1.0/2.0); 225 | double t500 = t384*t432*(1.0/2.0); 226 | double t501 = ew2*ew3*t388*(1.0/2.0); 227 | double t502 = n_bw1*n_bw1; 228 | double t503 = n_bw3*n_bw3; 229 | double t504 = t343*t347*t413*(1.0/2.0); 230 | double t505 = t343*t354*t414*(1.0/2.0); 231 | double t506 = t397*t397; 232 | double t507 = t390*t390; 233 | double t508 = t343*t355*t415*(1.0/2.0); 234 | double t509 = t394*t394; 235 | double t510 = t504+t505+t508-dt*t343*t373*t506*(1.0/6.0)-dt*t343*t371*t509*(1.0/6.0)-dt*t343*t374*t507*(1.0/6.0); 236 | double t512 = -t444+t490+t491; 237 | double t513 = t397*t437*(1.0/2.0); 238 | double t514 = t362*t394*t429; 239 | double t515 = dt*t362*t397; 240 | double t516 = t362*t489*(1.0/2.0); 241 | double t517 = t394*t466*(1.0/2.0); 242 | double t518 = t362*t397*t429; 243 | double t519 = t516+t517+t518; 244 | double t520 = dt*t362*t394; 245 | double t521 = t440+t520-dt*t343*t519*(1.0/3.0); 246 | double t522 = t371*t521; 247 | double t523 = t362*t447; 248 | double t524 = t390*t425; 249 | double t525 = t523+t524; 250 | double t526 = t343*t525*(1.0/2.0); 251 | double t528 = t425*t447; 252 | double t529 = t390*t474*(1.0/2.0); 253 | double t530 = t528+t529-t362*t497*(1.0/2.0); 254 | double t531 = dt*t343*t530*(1.0/3.0); 255 | double t532 = dt*t362*t390; 256 | double t533 = t526+t531+t532; 257 | double t534 = t374*t533; 258 | double t535 = dt*t343*t345*t370*t400*(1.0/3.0); 259 | double t536 = dt*t345*t355*t370; 260 | double t537 = t381*t489*(1.0/2.0); 261 | double t538 = t388*t397*t429; 262 | double t539 = t537+t538-t394*t485*(1.0/2.0); 263 | double t540 = dt*t343*t539*(1.0/3.0); 264 | double t542 = t495+t540-dt*t381*t394; 265 | double t543 = t388*t512*(1.0/2.0); 266 | double t544 = t381*t394*t429; 267 | double t545 = t543+t544-t397*t478*(1.0/2.0); 268 | double t546 = dt*t343*t545*(1.0/3.0); 269 | double t547 = t495+t546-dt*t388*t397; 270 | double t548 = t373*t547; 271 | double t549 = t384*t447; 272 | double t550 = t549-t390*t473; 273 | double t551 = t343*t550*(1.0/2.0); 274 | double t552 = t384*t497*(1.0/2.0); 275 | double t553 = t390*t483*(1.0/2.0); 276 | double t554 = t447*t473; 277 | double t555 = t552+t553+t554; 278 | double t556 = dt*t384*t390; 279 | double t557 = t551+t556-dt*t343*t555*(1.0/3.0); 280 | double t558 = dt*t343*t367*t372*t399*(1.0/3.0); 281 | double t559 = dt*t354*t367*t372; 282 | double t560 = t548+t558+t559-t371*t542-t374*t557-dt*t347*t366*t369-dt*t355*t370*t376-dt*t343*t366*t369*t398*(1.0/3.0)-dt*t343*t370*t376*t400*(1.0/3.0); 283 | double t561 = ew1*t343*t394*t397; 284 | double t562 = ew1*t343*t397*(1.0/2.0); 285 | double t563 = n_bw2*n_bw2; 286 | double t564 = dt*t343*t362*t374*(1.0/6.0); 287 | double t565 = dt*t343*t374*t390*(1.0/6.0); 288 | double t566 = ew1*ew2*t362*(1.0/2.0); 289 | double t567 = -t433+t450+t566; 290 | double t568 = dt*t343*t567*(1.0/3.0); 291 | double t569 = t343*t425*(1.0/2.0); 292 | double t570 = t451+t568+t569; 293 | double t571 = dt*t343*t362*t373*t432*(1.0/6.0); 294 | double t572 = dt*t343*t362*t371*t432*(1.0/6.0); 295 | double t573 = t571+t572-t374*t570; 296 | double t574 = ew1*ew2*t397*(1.0/2.0); 297 | double t575 = t444+t574-ew1*ew3*t394*(1.0/2.0); 298 | double t576 = t343*t447*(1.0/2.0); 299 | double t577 = dt*t390; 300 | double t578 = t576+t577-dt*t343*t575*(1.0/3.0); 301 | double t579 = dt*t343*t371*t394*t432*(1.0/6.0); 302 | double t580 = t579-t374*t578-dt*t343*t373*t397*t432*(1.0/6.0); 303 | double t581 = dt*t343*t373*t388*(1.0/6.0); 304 | double t582 = t362*t432*(1.0/2.0); 305 | double t583 = ew2*ew3*t362*(1.0/2.0); 306 | double t584 = t362*t429; 307 | double t585 = t583+t584; 308 | double t586 = ew3*t473; 309 | double t587 = ew1*ew2*t384*(1.0/2.0); 310 | double t588 = t586+t587; 311 | double t589 = dt*t343*t588*(1.0/3.0); 312 | double t590 = t374*(t589-ew3*t343*t384*(1.0/2.0)); 313 | double t591 = t388*t429; 314 | double t592 = t498+t591; 315 | double t593 = dt*t343*t592*(1.0/3.0); 316 | double t594 = t499+t593; 317 | double t595 = -t492+t498+t500; 318 | double t596 = dt*t343*t595*(1.0/3.0); 319 | double t597 = dt*t388; 320 | double t598 = -t499+t596+t597; 321 | double t599 = t590-t371*t594-t373*t598; 322 | double t600 = t397*t429; 323 | double t601 = ew2*ew3*t394*(1.0/2.0); 324 | double t602 = ew1*t343*t394*(1.0/2.0); 325 | double t603 = ew3*t447; 326 | double t604 = t603-ew1*ew2*t390*(1.0/2.0); 327 | double t605 = dt*t343*t604*(1.0/3.0); 328 | double t606 = ew3*t343*t390*(1.0/2.0); 329 | double t607 = t605+t606; 330 | double t608 = t374*t607; 331 | double t609 = t390*t432*(1.0/2.0); 332 | double t610 = dt*t397; 333 | double t611 = t430*(1.0/2.0); 334 | double t612 = t431*(1.0/2.0); 335 | double t613 = t611+t612; 336 | double t614 = ew1*t343*(1.0/2.0); 337 | double t615 = dt*t343*t362*t371*(1.0/6.0); 338 | double t616 = dt*t343*t371*t381*(1.0/6.0); 339 | double t617 = dt*t343*t371*t394*(1.0/6.0); 340 | double t618 = ew2*t425; 341 | double t619 = t450+t618; 342 | double t620 = dt*t343*t619*(1.0/3.0); 343 | double t621 = ew2*t343*t362*(1.0/2.0); 344 | double t622 = t620+t621; 345 | double t623 = dt*t343*t585*(1.0/3.0); 346 | double t624 = t381*t429; 347 | double t625 = t501+t624; 348 | double t626 = dt*t343*t625*(1.0/3.0); 349 | double t627 = ew1*t343*t388*(1.0/2.0); 350 | double t628 = ew2*t473; 351 | double t629 = t628-ew1*ew3*t384*(1.0/2.0); 352 | double t630 = dt*t343*t629*(1.0/3.0); 353 | double t631 = t630-ew2*t343*t384*(1.0/2.0); 354 | double t632 = -t486+t500+t501; 355 | double t633 = dt*t343*t632*(1.0/3.0); 356 | double t634 = dt*t381; 357 | double t635 = t627+t633+t634; 358 | double t636 = ew2*t447; 359 | double t637 = ew1*ew3*t390*(1.0/2.0); 360 | double t638 = t636+t637; 361 | double t639 = dt*t343*t638*(1.0/3.0); 362 | double t640 = ew2*t343*t390*(1.0/2.0); 363 | double t641 = t639+t640; 364 | double t642 = t394*t429; 365 | double t643 = ew2*ew3*t397*(1.0/2.0); 366 | double t644 = t487+t609+t643; 367 | double t645 = dt*t343*t644*(1.0/3.0); 368 | double t646 = t562+t645-dt*t394; 369 | double t647 = t371*t646; 370 | double t648 = ew2*t343*(1.0/2.0); 371 | double t649 = dt*ew1*ew3*t343*(1.0/6.0); 372 | double t650 = t648+t649; 373 | double t651 = t374*t650; 374 | double t652 = t651-dt*t343*t371*t613*(1.0/3.0); 375 | double t653 = dt*ew2*ew3*t343*(1.0/6.0); 376 | double t654 = t614+t653; 377 | double t655 = t371*t654; 378 | double t656 = dt*t343*t397*t563*(1.0/6.0); 379 | double t657 = dt*ew1*t343*t563*(1.0/6.0); 380 | double t658 = dt*t343*t369*t398*(1.0/6.0); 381 | double t659 = t343*t369*t398*(1.0/2.0); 382 | double t660 = dt*t343*t344*t399*(1.0/6.0); 383 | double t661 = t343*t344*t399*(1.0/2.0); 384 | double t662 = dt*t343*t376*t400*(1.0/6.0); 385 | double t663 = t343*t376*t400*(1.0/2.0); 386 | Qd(0,0) = dt*t343*t347*t361*(1.0/3.0)+dt*t343*t354*t359*(1.0/3.0)+dt*t343*t355*t360*(1.0/3.0); 387 | Qd(0,1) = t375-dt*t343*t345*t355*(t358-q3*q4*2.0)*(1.0/3.0)-dt*t343*t344*t354*t367*(1.0/3.0); 388 | Qd(0,2) = t402; 389 | Qd(0,3) = t419; 390 | Qd(0,4) = t420; 391 | Qd(0,5) = t408; 392 | Qd(0,6) = t564; 393 | Qd(0,8) = t615; 394 | Qd(0,12) = dt*t343*t346*t398*(-1.0/6.0); 395 | Qd(0,13) = t660; 396 | Qd(0,14) = dt*t343*t345*t400*(-1.0/6.0); 397 | Qd(1,0) = t375-dt*t343*t344*t354*t367*(1.0/3.0)-dt*t343*t345*t355*t376*(1.0/3.0); 398 | Qd(1,1) = dt*t343*t347*t378*(1.0/3.0)+dt*t343*t355*t379*(1.0/3.0)+dt*t343*t354*t385*(1.0/3.0); 399 | Qd(1,2) = t404; 400 | Qd(1,3) = t377+t455-t343*t344*t354*t367*(1.0/2.0)-t343*t345*t355*t376*(1.0/2.0)-dt*t343*t362*t371*t381*(1.0/6.0)-dt*t343*t362*t373*t388*(1.0/6.0); 401 | Qd(1,4) = t462; 402 | Qd(1,5) = t412; 403 | Qd(1,6) = dt*t343*t374*t384*(-1.0/6.0); 404 | Qd(1,7) = t581; 405 | Qd(1,8) = t616; 406 | Qd(1,12) = dt*t343*t366*t398*(-1.0/6.0); 407 | Qd(1,13) = dt*t343*t367*t399*(-1.0/6.0); 408 | Qd(1,14) = t662; 409 | Qd(2,0) = t402; 410 | Qd(2,1) = t404; 411 | Qd(2,2) = dt*t343*t347*t413*(1.0/3.0)+dt*t343*t354*t414*(1.0/3.0)+dt*t343*t355*t415*(1.0/3.0); 412 | Qd(2,3) = t408; 413 | Qd(2,4) = t412; 414 | Qd(2,5) = t510; 415 | Qd(2,6) = t565; 416 | Qd(2,7) = dt*t343*t373*t397*(-1.0/6.0); 417 | Qd(2,8) = t617; 418 | Qd(2,12) = t658; 419 | Qd(2,13) = dt*t343*t372*t399*(-1.0/6.0); 420 | Qd(2,14) = dt*t343*t370*t400*(-1.0/6.0); 421 | Qd(3,0) = t419; 422 | Qd(3,1) = t420; 423 | Qd(3,2) = t408; 424 | Qd(3,3) = t374*(t428+t343*t362*t425+dt*t343*(t362*(t448+t449-t362*t432)+t425*t425)*(1.0/3.0))+t373*(t428-t434+dt*t343*(t365*t429-t362*t437)*(1.0/3.0))+t371*(t428+t434+dt*t343*(t365*t429-t362*(t433+t442-ew2*ew3*t362))*(1.0/3.0))+dt*t347*t361+dt*t354*t359+dt*t355*t360+dt*t343*t359*t399*(1.0/3.0)+dt*t343*t361*t398*(1.0/3.0)+dt*t343*t360*t400*(1.0/3.0); 425 | Qd(3,4) = t475+t476-dt*t344*t354*t367-dt*t345*t355*t376-dt*t343*t344*t367*t399*(1.0/3.0)-dt*t343*t345*t376*t400*(1.0/3.0); 426 | Qd(3,5) = t522+t534+t535+t536-t373*(t440+t515-dt*t343*(t513+t514+t362*(t490+t491-t390*t432)*(1.0/2.0))*(1.0/3.0))-dt*t346*t347*t369-dt*t344*t354*t372-dt*t343*t346*t369*t398*(1.0/3.0)-dt*t343*t344*t372*t399*(1.0/3.0); 427 | Qd(3,6) = t573; 428 | Qd(3,8) = -t371*(t451+t452-dt*t343*(t442+t582-ew2*ew3*t362*(1.0/2.0))*(1.0/3.0))-t374*t622+t373*(t452-dt*t343*t585*(1.0/3.0)); 429 | Qd(3,9) = dt*t343*t362*t502*(-1.0/6.0); 430 | Qd(3,11) = dt*t343*t362*t503*(-1.0/6.0); 431 | Qd(3,12) = t343*t346*t398*(-1.0/2.0); 432 | Qd(3,13) = t661; 433 | Qd(3,14) = t343*t345*t400*(-1.0/2.0); 434 | Qd(4,0) = t377-t453-t454+t455-dt*t343*t362*t371*t381*(1.0/6.0)-dt*t343*t362*t373*t388*(1.0/6.0); 435 | Qd(4,1) = t462; 436 | Qd(4,2) = t412; 437 | Qd(4,3) = t475+t476-t374*(t343*(t384*t425-t362*t473)*(1.0/2.0)-dt*t343*(t362*t483*(1.0/2.0)-t384*t474*(1.0/2.0)+t425*t473)*(1.0/3.0)+dt*t362*t384)+t371*(t470+dt*t362*t381+dt*t343*(t362*t485*(1.0/2.0)-t381*t466*(1.0/2.0)+t362*t388*t429)*(1.0/3.0))+t373*(-t470+dt*t362*t388+dt*t343*(t388*t437*(-1.0/2.0)+t362*t478*(1.0/2.0)+t362*t381*t429)*(1.0/3.0))-dt*t344*t354*t367-dt*t345*t355*t376-dt*t343*t344*t367*t399*(1.0/3.0)-dt*t343*t345*t376*t400*(1.0/3.0); 438 | Qd(4,4) = -t374*(-dt*t459+dt*t343*(t384*t483-t480*t480)*(1.0/3.0)+t343*t384*t473)+t373*(dt*t461+dt*t343*(t388*t478+t429*t458)*(1.0/3.0)-ew1*t343*t381*t388)+t371*(dt*t458+dt*t343*(t381*t485+t429*t461)*(1.0/3.0)+ew1*t343*t381*t388)+dt*t347*t378+dt*t355*t379+dt*t354*t385+dt*t343*t378*t398*(1.0/3.0)+dt*t343*t385*t399*(1.0/3.0)+dt*t343*t379*t400*(1.0/3.0); 439 | Qd(4,5) = t560; 440 | Qd(4,6) = -t374*(-dt*t384+t343*t473*(1.0/2.0)+dt*t343*(t471+ew1*ew2*t388*(1.0/2.0)+ew1*ew3*t381*(1.0/2.0))*(1.0/3.0))+dt*t343*t371*t381*t432*(1.0/6.0)+dt*t343*t373*t388*t432*(1.0/6.0); 441 | Qd(4,7) = t599; 442 | Qd(4,8) = -t374*t631-t371*t635-t373*(t626-ew1*t343*t388*(1.0/2.0)); 443 | Qd(4,9) = dt*t343*t384*t502*(1.0/6.0); 444 | Qd(4,10) = dt*t343*t388*t563*(-1.0/6.0); 445 | Qd(4,11) = dt*t343*t381*t503*(-1.0/6.0); 446 | Qd(4,12) = t343*t366*t398*(-1.0/2.0); 447 | Qd(4,13) = t343*t367*t399*(-1.0/2.0); 448 | Qd(4,14) = t663; 449 | Qd(5,0) = t408; 450 | Qd(5,1) = t412; 451 | Qd(5,2) = t510; 452 | Qd(5,3) = t522+t534+t535+t536-t373*(t440+t515-dt*t343*(t513+t514+t362*t512*(1.0/2.0))*(1.0/3.0))-dt*t346*t347*t369-dt*t344*t354*t372-dt*t343*t346*t369*t398*(1.0/3.0)-dt*t343*t344*t372*t399*(1.0/3.0); 453 | Qd(5,4) = t560; 454 | Qd(5,5) = -t371*(t561-dt*t509+dt*t343*(t394*t489-t429*t506)*(1.0/3.0))+t373*(t561+dt*t506-dt*t343*(t397*t512-t429*t509)*(1.0/3.0))+t374*(dt*t507-dt*t343*(t390*t497-t447*t447)*(1.0/3.0)+t343*t390*t447)+dt*t347*t413+dt*t354*t414+dt*t355*t415+dt*t343*t398*t413*(1.0/3.0)+dt*t343*t399*t414*(1.0/3.0)+dt*t343*t400*t415*(1.0/3.0); 455 | Qd(5,6) = t580; 456 | Qd(5,7) = t608+t371*(dt*t343*(t600-ew2*ew3*t394*(1.0/2.0))*(1.0/3.0)-ew1*t343*t394*(1.0/2.0))+t373*(t602+t610-dt*t343*(t490+t601-t390*t432*(1.0/2.0))*(1.0/3.0)); 457 | Qd(5,8) = t647-t374*t641-t373*(t562+dt*t343*(t642-ew2*ew3*t397*(1.0/2.0))*(1.0/3.0)); 458 | Qd(5,9) = dt*t343*t390*t502*(-1.0/6.0); 459 | Qd(5,10) = t656; 460 | Qd(5,11) = dt*t343*t394*t503*(-1.0/6.0); 461 | Qd(5,12) = t659; 462 | Qd(5,13) = t343*t372*t399*(-1.0/2.0); 463 | Qd(5,14) = t343*t370*t400*(-1.0/2.0); 464 | Qd(6,0) = t564; 465 | Qd(6,2) = t565; 466 | Qd(6,3) = t573; 467 | Qd(6,5) = t580; 468 | Qd(6,6) = t374*(dt-dt*t343*t432*(1.0/3.0))+dt*t343*t502*(1.0/3.0); 469 | Qd(6,8) = t652; 470 | Qd(6,9) = t343*t502*(-1.0/2.0); 471 | Qd(7,0) = dt*t343*t362*t373*(1.0/6.0); 472 | Qd(7,1) = t581; 473 | Qd(7,2) = dt*t343*t373*t397*(-1.0/6.0); 474 | Qd(7,3) = -t371*(t452+t623)-t374*(dt*t343*(t566-ew3*t425)*(1.0/3.0)-ew3*t343*t362*(1.0/2.0))+t373*(-t451+t452+dt*t343*(t436+t582-t583)*(1.0/3.0)); 475 | Qd(7,4) = t599; 476 | Qd(7,5) = t608+t373*(t602+t610-dt*t343*(t490+t601-t609)*(1.0/3.0))-t371*(t602-dt*t343*(t600-t601)*(1.0/3.0)); 477 | Qd(7,6) = -t374*(ew3*t343*(1.0/2.0)-dt*ew1*ew2*t343*(1.0/6.0))-dt*t343*t373*t613*(1.0/3.0); 478 | Qd(7,7) = t373*(dt-dt*t343*t435*(1.0/3.0))+dt*t343*t563*(1.0/3.0)+dt*t343*t371*t429*(1.0/3.0)+dt*t343*t374*t431*(1.0/3.0); 479 | Qd(7,8) = t655-t373*(t614-dt*ew2*ew3*t343*(1.0/6.0))-dt*ew2*ew3*t343*t374*(1.0/3.0); 480 | Qd(7,9) = dt*ew3*t343*t502*(1.0/6.0); 481 | Qd(7,10) = t343*t563*(-1.0/2.0); 482 | Qd(7,11) = dt*ew1*t343*t503*(-1.0/6.0); 483 | Qd(8,0) = t615; 484 | Qd(8,1) = t616; 485 | Qd(8,2) = t617; 486 | Qd(8,3) = -t374*t622-t371*(t451+t452-dt*t343*(t442+t582-t583)*(1.0/3.0))+t373*(t452-t623); 487 | Qd(8,4) = -t374*t631-t371*t635-t373*(t626-t627); 488 | Qd(8,5) = t647-t374*t641-t373*(t562+dt*t343*(t642-t643)*(1.0/3.0)); 489 | Qd(8,6) = t652; 490 | Qd(8,7) = t655-t373*(t614-t653)-dt*ew2*ew3*t343*t374*(1.0/3.0); 491 | Qd(8,8) = t371*(dt-dt*t343*t441*(1.0/3.0))+dt*t343*t503*(1.0/3.0)+dt*t343*t373*t429*(1.0/3.0)+dt*t343*t374*t430*(1.0/3.0); 492 | Qd(8,9) = dt*ew2*t343*t502*(-1.0/6.0); 493 | Qd(8,10) = t657; 494 | Qd(8,11) = t343*t503*(-1.0/2.0); 495 | Qd(9,3) = dt*t343*t362*t502*(-1.0/6.0); 496 | Qd(9,5) = dt*t343*t390*t502*(-1.0/6.0); 497 | Qd(9,6) = t343*t502*(-1.0/2.0); 498 | Qd(9,8) = dt*ew2*t343*t502*(-1.0/6.0); 499 | Qd(9,9) = dt*t502; 500 | Qd(10,3) = dt*t343*t362*t563*(-1.0/6.0); 501 | Qd(10,4) = dt*t343*t388*t563*(-1.0/6.0); 502 | Qd(10,5) = t656; 503 | Qd(10,7) = t343*t563*(-1.0/2.0); 504 | Qd(10,8) = t657; 505 | Qd(10,10) = dt*t563; 506 | Qd(11,3) = dt*t343*t362*t503*(-1.0/6.0); 507 | Qd(11,4) = dt*t343*t381*t503*(-1.0/6.0); 508 | Qd(11,5) = dt*t343*t394*t503*(-1.0/6.0); 509 | Qd(11,7) = dt*ew1*t343*t503*(-1.0/6.0); 510 | Qd(11,8) = t343*t503*(-1.0/2.0); 511 | Qd(11,11) = dt*t503; 512 | Qd(12,0) = dt*t343*t346*t398*(-1.0/6.0); 513 | Qd(12,1) = dt*t343*t366*t398*(-1.0/6.0); 514 | Qd(12,2) = t658; 515 | Qd(12,3) = t343*t346*t398*(-1.0/2.0); 516 | Qd(12,4) = t343*t366*t398*(-1.0/2.0); 517 | Qd(12,5) = t659; 518 | Qd(12,12) = dt*t398; 519 | Qd(13,0) = t660; 520 | Qd(13,1) = dt*t343*t367*t399*(-1.0/6.0); 521 | Qd(13,2) = dt*t343*t372*t399*(-1.0/6.0); 522 | Qd(13,3) = t661; 523 | Qd(13,4) = t343*t367*t399*(-1.0/2.0); 524 | Qd(13,5) = t343*t372*t399*(-1.0/2.0); 525 | Qd(13,13) = dt*t399; 526 | Qd(14,0) = dt*t343*t345*t400*(-1.0/6.0); 527 | Qd(14,1) = t662; 528 | Qd(14,2) = dt*t343*t370*t400*(-1.0/6.0); 529 | Qd(14,3) = t343*t345*t400*(-1.0/2.0); 530 | Qd(14,4) = t663; 531 | Qd(14,5) = t343*t370*t400*(-1.0/2.0); 532 | Qd(14,14) = dt*t400; 533 | Qd(15,15) = dt*(n_L*n_L); 534 | Qd(16,16) = dt*(n_qvw1*n_qvw1); 535 | Qd(17,17) = dt*(n_qvw2*n_qvw2); 536 | Qd(18,18) = dt*(n_qvw3*n_qvw3); 537 | Qd(19,19) = dt*(n_qci[0]*n_qci[0]); 538 | Qd(20,20) = dt*(n_qci[1]*n_qci[1]); 539 | Qd(21,21) = dt*(n_qci[2]*n_qci[2]); 540 | Qd(22,22) = dt*(n_pic[0]*n_pic[0]); 541 | Qd(23,23) = dt*(n_pic[1]*n_pic[1]); 542 | Qd(24,24) = dt*(n_pic[2]*n_pic[2]); 543 | }; 544 | 545 | #endif /* CALCQ_H_ */ 546 | -------------------------------------------------------------------------------- /ssf_core/src/measurement.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | 34 | namespace ssf_core{ 35 | 36 | Measurements::Measurements() 37 | { 38 | // setup: initial pos, att, of measurement sensor 39 | 40 | p_vc_ = Eigen::Matrix::Constant(0); 41 | q_cv_ = Eigen::Quaternion(1, 0, 0, 0); 42 | 43 | ssf_core_.registerCallback(&Measurements::Config,this); 44 | } 45 | 46 | Measurements::~Measurements() 47 | { 48 | for (Handlers::iterator it(handlers.begin()); it != handlers.end(); ++it) 49 | delete *it; 50 | 51 | delete reconfServer_; 52 | return; 53 | } 54 | 55 | 56 | void Measurements::Config(ssf_core::SSF_CoreConfig& config, uint32_t level){ 57 | if(level & ssf_core::SSF_Core_INIT_FILTER){ 58 | init(config.scale_init); 59 | config.init_filter = false; 60 | } 61 | else if(level & ssf_core::SSF_Core_SET_HEIGHT){ 62 | if(p_vc_.norm()==0) 63 | { 64 | ROS_WARN_STREAM("No measurements received yet to initialize position - using scale factor " << config.scale_init << " for init"); 65 | init(config.scale_init); 66 | } 67 | else 68 | { 69 | init(p_vc_[2]/config.height); 70 | ROS_WARN_STREAM("init filter (set scale to: " << p_vc_[2]/config.height << ")"); 71 | } 72 | config.set_height = false; 73 | } 74 | } 75 | 76 | }; // end namespace 77 | -------------------------------------------------------------------------------- /ssf_core/src/state.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include 33 | 34 | namespace ssf_core 35 | { 36 | 37 | void State::reset(){ 38 | // states varying during propagation 39 | p_.setZero(); 40 | v_.setZero(); 41 | q_.setIdentity(); 42 | b_w_.setZero(); 43 | b_a_.setZero(); 44 | 45 | L_ = 1.0; 46 | q_wv_.setIdentity(); 47 | q_ci_.setIdentity(); 48 | p_ci_.setZero(); 49 | 50 | w_m_.setZero(); 51 | a_m_.setZero(); 52 | 53 | q_int_.setIdentity(); 54 | 55 | P_.setZero(); 56 | time_ = 0; 57 | } 58 | 59 | void State::getPoseCovariance(geometry_msgs::PoseWithCovariance::_covariance_type & cov) 60 | { 61 | assert(cov.size() == 36); 62 | 63 | for (int i = 0; i < 9; i++) 64 | cov[i / 3 * 6 + i % 3] = P_(i / 3 * N_STATE + i % 3); 65 | 66 | for (int i = 0; i < 9; i++) 67 | cov[i / 3 * 6 + (i % 3 + 3)] = P_(i / 3 * N_STATE + (i % 3 + 6)); 68 | 69 | for (int i = 0; i < 9; i++) 70 | cov[(i / 3 + 3) * 6 + i % 3] = P_((i / 3 + 6) * N_STATE + i % 3); 71 | 72 | for (int i = 0; i < 9; i++) 73 | cov[(i / 3 + 3) * 6 + (i % 3 + 3)] = P_((i / 3 + 6) * N_STATE + (i % 3 + 6)); 74 | } 75 | 76 | void State::toPoseMsg(geometry_msgs::PoseWithCovarianceStamped & pose) 77 | { 78 | eigen_conversions::vector3dToPoint(p_, pose.pose.pose.position); 79 | eigen_conversions::quaternionToMsg(q_, pose.pose.pose.orientation); 80 | getPoseCovariance(pose.pose.covariance); 81 | } 82 | 83 | void State::toExtStateMsg(sensor_fusion_comm::ExtState & state) 84 | { 85 | eigen_conversions::vector3dToPoint(p_, state.pose.position); 86 | eigen_conversions::quaternionToMsg(q_, state.pose.orientation); 87 | eigen_conversions::vector3dToPoint(v_, state.velocity); 88 | } 89 | 90 | void State::toStateMsg(sensor_fusion_comm::DoubleArrayStamped & state) 91 | { 92 | state.data[0] = p_[0]; 93 | state.data[1] = p_[1]; 94 | state.data[2] = p_[2]; 95 | state.data[3] = v_[0]; 96 | state.data[4] = v_[1]; 97 | state.data[5] = v_[2]; 98 | state.data[6] = q_.w(); 99 | state.data[7] = q_.x(); 100 | state.data[8] = q_.y(); 101 | state.data[9] = q_.z(); 102 | state.data[10] = b_w_[0]; 103 | state.data[11] = b_w_[1]; 104 | state.data[12] = b_w_[2]; 105 | state.data[13] = b_a_[0]; 106 | state.data[14] = b_a_[1]; 107 | state.data[15] = b_a_[2]; 108 | state.data[16] = L_; 109 | state.data[17] = q_wv_.w(); 110 | state.data[18] = q_wv_.x(); 111 | state.data[19] = q_wv_.y(); 112 | state.data[20] = q_wv_.z(); 113 | state.data[21] = q_ci_.w(); 114 | state.data[22] = q_ci_.x(); 115 | state.data[23] = q_ci_.y(); 116 | state.data[24] = q_ci_.z(); 117 | state.data[25] = p_ci_[0]; 118 | state.data[26] = p_ci_[1]; 119 | state.data[27] = p_ci_[2]; 120 | } 121 | 122 | }; // end namespace ssf_core 123 | -------------------------------------------------------------------------------- /ssf_updates/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ssf_updates) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp ssf_core geometry_msgs message_generation) 5 | 6 | # Set the build type. Options are: 7 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 8 | # Debug : w/ debug symbols, w/o optimization 9 | # Release : w/o debug symbols, w/ optimization 10 | # RelWithDebInfo : w/ debug symbols, w/ optimization 11 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 12 | #set(ROS_BUILD_TYPE RelWithDebInfo) 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | add_definitions (-Wall -O3) 20 | 21 | include_directories(include ${catkin_INCLUDE_DIRS}) 22 | 23 | add_message_files(FILES PositionWithCovarianceStamped.msg) 24 | 25 | generate_messages(DEPENDENCIES geometry_msgs std_msgs) 26 | 27 | catkin_package(CATKIN_DEPENDS roscpp ssf_core geometry_msgs) 28 | 29 | add_executable(pose_sensor src/main.cpp src/pose_sensor.cpp) 30 | set_property(TARGET pose_sensor PROPERTY COMPILE_DEFINITIONS POSE_MEAS) 31 | 32 | target_link_libraries(pose_sensor ${catkin_LIBRARIES}) 33 | 34 | add_executable(position_sensor src/main.cpp src/position_sensor.cpp) 35 | set_property(TARGET position_sensor PROPERTY COMPILE_DEFINITIONS POSITION_MEAS) 36 | 37 | target_link_libraries(position_sensor ${catkin_LIBRARIES}) 38 | 39 | -------------------------------------------------------------------------------- /ssf_updates/launch/pose_sensor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ssf_updates/launch/position_sensor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ssf_updates/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b ssf_updates is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /ssf_updates/msg/PositionWithCovarianceStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point position 3 | float64[9] covariance -------------------------------------------------------------------------------- /ssf_updates/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ssf_updates 3 | 0.1.0 4 | Single Sensor Fusion (SSF) framework containing the update sensor modules 5 | Stephan Weiss 6 | Markus Achtelik 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/ethzasl_sensor_fusion/ssf_updates 11 | http://github.com/ethz-asl/ethzasl_sensor_fusion/issues 12 | 13 | Stephan Weiss 14 | Markus Achtelik 15 | 16 | 17 | catkin 18 | 19 | 20 | roscpp 21 | ssf_core 22 | geometry_msgs 23 | 24 | 25 | roscpp 26 | ssf_core 27 | geometry_msgs 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ssf_updates/pose_sensor_fix.yaml: -------------------------------------------------------------------------------- 1 | scale_init: 1 2 | fixed_scale: 0 3 | fixed_bias: 0 4 | noise_acc: 0.083 5 | noise_accbias: 0.0083 6 | noise_gyr: 0.0013 7 | noise_gyrbias: 0.00013 8 | noise_scale: 0.0 9 | noise_qwv: 0.0 10 | noise_qci: 0.0 11 | noise_pic: 0.0 12 | delay: 0.02 13 | meas_noise1: 0.01 14 | meas_noise2: 0.02 15 | 16 | data_playback: False 17 | 18 | # initialization of camera w.r.t. IMU 19 | init/q_ci/w: 1.0 20 | init/q_ci/x: 0.0 21 | init/q_ci/y: 0.0 22 | init/q_ci/z: 0.0 23 | 24 | init/p_ci/x: 0.0 25 | init/p_ci/y: 0.0 26 | init/p_ci/z: 0.0 27 | 28 | # initialization of world w.r.t. vision 29 | init/q_wv/w: 1.0 30 | init/q_wv/x: 0.0 31 | init/q_wv/y: 0.0 32 | init/q_wv/z: 0.0 33 | 34 | use_fixed_covariance: false 35 | measurement_world_sensor: true # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam) 36 | -------------------------------------------------------------------------------- /ssf_updates/position_sensor_fix.yaml: -------------------------------------------------------------------------------- 1 | scale_init: 1 2 | fixed_scale: 0 3 | fixed_bias: 0 4 | noise_acc: 0.083 5 | noise_accbias: 0.0083 6 | noise_gyr: 0.0013 7 | noise_gyrbias: 0.00013 8 | noise_scale: 0.0 9 | noise_qwv: 0.0 10 | noise_qci: 0.0 11 | noise_pic: 0.0 12 | delay: 0.02 13 | meas_noise1: 0.005 14 | meas_noise2: 0.0 15 | 16 | data_playback: False 17 | 18 | # initialization of camera w.r.t. IMU 19 | init/q_ci/w: 1.0 20 | init/q_ci/x: 0.0 21 | init/q_ci/y: 0.0 22 | init/q_ci/z: 0.0 23 | 24 | init/p_ci/x: 0.0 25 | init/p_ci/y: 0.0 26 | init/p_ci/z: 0.0 27 | 28 | # initialization of world w.r.t. vision 29 | init/q_wv/w: 1.0 30 | init/q_wv/x: 0.0 31 | init/q_wv/y: 0.0 32 | init/q_wv/z: 0.0 33 | 34 | use_fixed_covariance: true 35 | measurement_world_sensor: true # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam) 36 | -------------------------------------------------------------------------------- /ssf_updates/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifdef POSE_MEAS 33 | #include "pose_measurements.h" 34 | #endif 35 | #ifdef POSITION_MEAS 36 | #include "position_measurements.h" 37 | #endif 38 | 39 | int main(int argc, char** argv) 40 | { 41 | ros::init(argc, argv, "ssf_core"); 42 | #ifdef POSE_MEAS 43 | PoseMeasurements PoseMeas; 44 | ROS_INFO_STREAM("Filter type: pose_sensor"); 45 | #endif 46 | 47 | #ifdef POSITION_MEAS 48 | PositionMeasurements PositionMeas; 49 | ROS_INFO_STREAM("Filter type: position_sensor"); 50 | #endif 51 | 52 | 53 | // print published/subscribed topics 54 | ros::V_string topics; 55 | ros::this_node::getSubscribedTopics(topics); 56 | std::string nodeName = ros::this_node::getName(); 57 | std::string topicsStr = nodeName + ":\n\tsubscribed to topics:\n"; 58 | for(unsigned int i=0; i 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef POSE_MEASUREMENTS_H 33 | #define POSE_MEASUREMENTS_H 34 | 35 | #include 36 | #include 37 | #include "pose_sensor.h" 38 | 39 | class PoseMeasurements : public ssf_core::Measurements 40 | { 41 | public: 42 | PoseMeasurements() 43 | { 44 | addHandler(new PoseSensorHandler(this)); 45 | 46 | ros::NodeHandle pnh("~"); 47 | pnh.param("init/p_ci/x", p_ci_[0], 0.0); 48 | pnh.param("init/p_ci/y", p_ci_[1], 0.0); 49 | pnh.param("init/p_ci/z", p_ci_[2], 0.0); 50 | 51 | pnh.param("init/q_ci/w", q_ci_.w(), 1.0); 52 | pnh.param("init/q_ci/x", q_ci_.x(), 0.0); 53 | pnh.param("init/q_ci/y", q_ci_.y(), 0.0); 54 | pnh.param("init/q_ci/z", q_ci_.z(), 0.0); 55 | q_ci_.normalize(); 56 | 57 | pnh.param("init/q_wv/w", q_wv_.w(), 1.0); 58 | pnh.param("init/q_wv/x", q_wv_.x(), 0.0); 59 | pnh.param("init/q_wv/y", q_wv_.y(), 0.0); 60 | pnh.param("init/q_wv/z", q_wv_.z(), 0.0); 61 | q_wv_.normalize(); 62 | } 63 | 64 | private: 65 | 66 | Eigen::Matrix p_ci_; ///< initial distance camera-IMU 67 | Eigen::Quaternion q_ci_; ///< initial rotation camera-IMU 68 | Eigen::Quaternion q_wv_; ///< initial rotation wolrd-vision 69 | 70 | void init(double scale) 71 | { 72 | Eigen::Matrix p, v, b_w, b_a, g, w_m, a_m; 73 | Eigen::Quaternion q; 74 | ssf_core::SSF_Core::ErrorStateCov P; 75 | 76 | // init values 77 | g << 0, 0, 9.81; /// gravity 78 | b_w << 0,0,0; /// bias gyroscopes 79 | b_a << 0,0,0; /// bias accelerometer 80 | 81 | v << 0,0,0; /// robot velocity (IMU centered) 82 | w_m << 0,0,0; /// initial angular velocity 83 | a_m =g; /// initial acceleration 84 | 85 | P.setZero(); // error state covariance; if zero, a default initialization in ssf_core is used 86 | 87 | // check if we have already input from the measurement sensor 88 | if (p_vc_.norm() == 0) 89 | ROS_WARN_STREAM("No measurements received yet to initialize position - using [0 0 0]"); 90 | if ((q_cv_.norm() == 1) & (q_cv_.w() == 1)) 91 | ROS_WARN_STREAM("No measurements received yet to initialize attitude - using [1 0 0 0]"); 92 | 93 | // calculate initial attitude and position based on sensor measurements 94 | q = (q_ci_ * q_cv_.conjugate() * q_wv_).conjugate(); 95 | q.normalize(); 96 | p = q_wv_.conjugate().toRotationMatrix() * p_vc_ / scale - q.toRotationMatrix() * p_ci_; 97 | 98 | // call initialization in core 99 | ssf_core_.initialize(p, v, q, b_w, b_a, scale, q_wv_, P, w_m, a_m, g, q_ci_, p_ci_); 100 | 101 | ROS_INFO_STREAM("filter initialized to: \n" << 102 | "position: [" << p[0] << ", " << p[1] << ", " << p[2] << "]" << std::endl << 103 | "scale:" << scale << std::endl << 104 | "attitude (w,x,y,z): [" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << std::endl << 105 | "p_ci: [" << p_ci_[0] << ", " << p_ci_[1] << ", " << p_ci_[2] << std::endl << 106 | "q_ci: (w,x,y,z): [" << q_ci_.w() << ", " << q_ci_.x() << ", " << q_ci_.y() << ", " << q_ci_.z() << "]"); 107 | } 108 | }; 109 | 110 | #endif /* POSE_MEASUREMENTS_H */ 111 | -------------------------------------------------------------------------------- /ssf_updates/src/pose_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "pose_sensor.h" 33 | #include 34 | 35 | #define N_MEAS 7 // measurement size 36 | PoseSensorHandler::PoseSensorHandler(ssf_core::Measurements* meas) : 37 | MeasurementHandler(meas) 38 | { 39 | ros::NodeHandle pnh("~"); 40 | pnh.param("measurement_world_sensor", measurement_world_sensor_, true); 41 | pnh.param("use_fixed_covariance", use_fixed_covariance_, false); 42 | 43 | ROS_INFO_COND(measurement_world_sensor_, "interpreting measurement as sensor w.r.t. world"); 44 | ROS_INFO_COND(!measurement_world_sensor_, "interpreting measurement as world w.r.t. sensor (e.g. ethzasl_ptam)"); 45 | 46 | ROS_INFO_COND(use_fixed_covariance_, "using fixed covariance"); 47 | ROS_INFO_COND(!use_fixed_covariance_, "using covariance from sensor"); 48 | 49 | subscribe(); 50 | } 51 | 52 | void PoseSensorHandler::subscribe() 53 | { 54 | ros::NodeHandle nh("ssf_core"); 55 | subMeasurement_ = nh.subscribe("pose_measurement", 1, &PoseSensorHandler::measurementCallback, this); 56 | 57 | measurements->ssf_core_.registerCallback(&PoseSensorHandler::noiseConfig, this); 58 | 59 | nh.param("meas_noise1", n_zp_, 0.01); // default position noise is for ethzasl_ptam 60 | nh.param("meas_noise2", n_zq_, 0.02); // default attitude noise is for ethzasl_ptam 61 | } 62 | 63 | void PoseSensorHandler::noiseConfig(ssf_core::SSF_CoreConfig& config, uint32_t level) 64 | { 65 | // if(level & ssf_core::SSF_Core_MISC) 66 | // { 67 | this->n_zp_ = config.meas_noise1; 68 | this->n_zq_ = config.meas_noise2; 69 | // } 70 | } 71 | 72 | void PoseSensorHandler::measurementCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) 73 | { 74 | // ROS_INFO_STREAM("measurement received \n" 75 | // << "type is: " << typeid(msg).name()); 76 | 77 | // init variables 78 | ssf_core::State state_old; 79 | ros::Time time_old = msg->header.stamp; 80 | Eigen::Matrix H_old; 81 | Eigen::Matrix r_old; 82 | Eigen::Matrix R; 83 | 84 | H_old.setZero(); 85 | R.setZero(); 86 | 87 | // get measurements 88 | z_p_ = Eigen::Matrix(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); 89 | z_q_ = Eigen::Quaternion(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); 90 | 91 | // take covariance from sensor 92 | R.block<6, 6> (0, 0) = Eigen::Matrix(&msg->pose.covariance[0]); 93 | //clear cross-correlations between q and p 94 | R.block<3, 3> (0, 3) = Eigen::Matrix::Zero(); 95 | R.block<3, 3> (3, 0) = Eigen::Matrix::Zero(); 96 | R(6, 6) = 1e-6; // q_vw yaw-measurement noise 97 | 98 | /*************************************************************************************/ 99 | // use this if your pose sensor is ethzasl_ptam (www.ros.org/wiki/ethzasl_ptam) 100 | // ethzasl_ptam publishes the camera pose as the world seen from the camera 101 | if (!measurement_world_sensor_) 102 | { 103 | Eigen::Matrix C_zq = z_q_.toRotationMatrix(); 104 | z_q_ = z_q_.conjugate(); 105 | z_p_ = -C_zq.transpose() * z_p_; 106 | 107 | Eigen::Matrix C_cov(Eigen::Matrix::Zero()); 108 | C_cov.block<3, 3> (0, 0) = C_zq; 109 | C_cov.block<3, 3> (3, 3) = C_zq; 110 | 111 | R.block<6, 6> (0, 0) = C_cov.transpose() * R.block<6, 6> (0, 0) * C_cov; 112 | } 113 | /*************************************************************************************/ 114 | 115 | // alternatively take fix covariance from reconfigure GUI 116 | if (use_fixed_covariance_) 117 | { 118 | const double s_zp = n_zp_ * n_zp_; 119 | const double s_zq = n_zq_ * n_zq_; 120 | R = (Eigen::Matrix() << s_zp, s_zp, s_zp, s_zq, s_zq, s_zq, 1e-6).finished().asDiagonal(); 121 | } 122 | 123 | // feedback for init case 124 | measurements->p_vc_ = z_p_; 125 | measurements->q_cv_ = z_q_; 126 | 127 | unsigned char idx = measurements->ssf_core_.getClosestState(&state_old, time_old); 128 | if (state_old.time_ == -1) 129 | return; // // early abort // // 130 | 131 | // get rotation matrices 132 | Eigen::Matrix C_wv = state_old.q_wv_.conjugate().toRotationMatrix(); 133 | Eigen::Matrix C_q = state_old.q_.conjugate().toRotationMatrix(); 134 | Eigen::Matrix C_ci = state_old.q_ci_.conjugate().toRotationMatrix(); 135 | 136 | // preprocess for elements in H matrix 137 | Eigen::Matrix vecold; 138 | vecold = (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_; 139 | Eigen::Matrix skewold = skew(vecold); 140 | 141 | Eigen::Matrix pci_sk = skew(state_old.p_ci_); 142 | 143 | // construct H matrix using H-blockx :-) 144 | // position: 145 | H_old.block<3, 3> (0, 0) = C_wv.transpose() * state_old.L_; // p 146 | H_old.block<3, 3> (0, 6) = -C_wv.transpose() * C_q.transpose() * pci_sk * state_old.L_; // q 147 | H_old.block<3, 1> (0, 15) = C_wv.transpose() * C_q.transpose() * state_old.p_ci_ + C_wv.transpose() * state_old.p_; // L 148 | H_old.block<3, 3> (0, 16) = -C_wv.transpose() * skewold; // q_wv 149 | H_old.block<3, 3> (0, 22) = C_wv.transpose() * C_q.transpose() * state_old.L_; //p_ci 150 | 151 | // attitude 152 | H_old.block<3, 3> (3, 6) = C_ci; // q 153 | H_old.block<3, 3> (3, 16) = C_ci * C_q; // q_wv 154 | H_old.block<3, 3> (3, 19) = Eigen::Matrix::Identity(); //q_ci 155 | H_old(6, 18) = 1.0; // fix vision world yaw drift because unobservable otherwise (see PhD Thesis) 156 | 157 | // construct residuals 158 | // position 159 | r_old.block<3, 1> (0, 0) = z_p_ - C_wv.transpose() * (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_; 160 | // attitude 161 | Eigen::Quaternion q_err; 162 | q_err = (state_old.q_wv_ * state_old.q_ * state_old.q_ci_).conjugate() * z_q_; 163 | r_old.block<3, 1> (3, 0) = q_err.vec() / q_err.w() * 2; 164 | // vision world yaw drift 165 | q_err = state_old.q_wv_; 166 | r_old(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y()) / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z())); 167 | 168 | // call update step in core class 169 | measurements->ssf_core_.applyMeasurement(idx, H_old, r_old, R); 170 | } 171 | -------------------------------------------------------------------------------- /ssf_updates/src/pose_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef POSE_SENSOR_H 33 | #define POSE_SENSOR_H 34 | 35 | #include 36 | 37 | class PoseSensorHandler : public ssf_core::MeasurementHandler 38 | { 39 | private: 40 | // measurements 41 | Eigen::Quaternion z_q_; /// attitude measurement camera seen from world 42 | Eigen::Matrix z_p_; /// position measurement camera seen from world 43 | double n_zp_, n_zq_; /// position and attitude measurement noise 44 | 45 | ros::Subscriber subMeasurement_; 46 | 47 | bool measurement_world_sensor_; ///< defines if the pose of the sensor is measured in world coordinates (true, default) or vice versa (false, e.g. PTAM) 48 | bool use_fixed_covariance_; ///< use fixed covariance set by dynamic reconfigure 49 | 50 | void subscribe(); 51 | void measurementCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); 52 | void noiseConfig(ssf_core::SSF_CoreConfig& config, uint32_t level); 53 | 54 | public: 55 | PoseSensorHandler(); 56 | PoseSensorHandler(ssf_core::Measurements* meas); 57 | }; 58 | 59 | #endif /* POSE_SENSOR_H */ 60 | -------------------------------------------------------------------------------- /ssf_updates/src/position_measurements.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef POSITION_MEASUREMENTS_H 33 | #define POSITION_MEASUREMENTS_H 34 | 35 | #include 36 | #include 37 | #include "position_sensor.h" 38 | 39 | class PositionMeasurements: public ssf_core::Measurements 40 | { 41 | public: 42 | PositionMeasurements() 43 | { 44 | addHandler(new PositionSensorHandler(this)); 45 | 46 | ros::NodeHandle pnh("~"); 47 | pnh.param("init/p_ci/x", p_ci_[0], 0.0); 48 | pnh.param("init/p_ci/y", p_ci_[1], 0.0); 49 | pnh.param("init/p_ci/z", p_ci_[2], 0.0); 50 | 51 | pnh.param("init/q_ci/w", q_ci_.w(), 1.0); 52 | pnh.param("init/q_ci/x", q_ci_.x(), 0.0); 53 | pnh.param("init/q_ci/y", q_ci_.y(), 0.0); 54 | pnh.param("init/q_ci/z", q_ci_.z(), 0.0); 55 | q_ci_.normalize(); 56 | 57 | pnh.param("init/q_wv/w", q_wv_.w(), 1.0); 58 | pnh.param("init/q_wv/x", q_wv_.x(), 0.0); 59 | pnh.param("init/q_wv/y", q_wv_.y(), 0.0); 60 | pnh.param("init/q_wv/z", q_wv_.z(), 0.0); 61 | q_wv_.normalize(); 62 | } 63 | 64 | private: 65 | 66 | Eigen::Matrix p_ci_; ///< initial distance camera-IMU 67 | Eigen::Quaternion q_ci_; ///< initial rotation camera-IMU 68 | Eigen::Quaternion q_wv_; ///< initial rotation wolrd-vision 69 | 70 | void init(double scale) 71 | { 72 | Eigen::Matrix p, v, b_w, b_a, g, w_m, a_m; 73 | Eigen::Quaternion q; 74 | ssf_core::SSF_Core::ErrorStateCov P; 75 | 76 | // init values 77 | g << 0, 0, 9.81; /// gravity 78 | b_w << 0,0,0; /// bias gyroscopes 79 | b_a << 0,0,0; /// bias accelerometer 80 | 81 | v << 0,0,0; /// robot velocity (IMU centered) 82 | w_m << 0,0,0; /// initial angular velocity 83 | a_m =g; /// initial acceleration 84 | 85 | P.setZero(); // error state covariance; if zero, a default initialization in ssf_core is used 86 | 87 | // check if we have already input from the measurement sensor 88 | if(p_vc_.norm()==0) 89 | ROS_WARN_STREAM("No measurements received yet to initialize position - using [0 0 0]"); 90 | if((q_cv_.norm()==1) & (q_cv_.w()==1)) 91 | ROS_WARN_STREAM("No measurements received yet to initialize attitude - using [1 0 0 0]"); 92 | 93 | // calculate initial attitude and position based on sensor measurements 94 | q = (q_ci_ * q_cv_.conjugate() * q_wv_).conjugate(); 95 | q.normalize(); 96 | p = q_wv_.conjugate().toRotationMatrix()*p_vc_/scale - q.toRotationMatrix()*p_ci_; 97 | 98 | // call initialization in core 99 | ssf_core_.initialize(p,v,q,b_w,b_a,scale,q_wv_,P,w_m,a_m,g,q_ci_,p_ci_); 100 | 101 | ROS_INFO_STREAM("filter initialized to: \n" << 102 | "position: [" << p[0] << ", " << p[1] << ", " << p[2] << "]" << std::endl << 103 | "scale:" << scale << std::endl << 104 | "attitude (w,x,y,z): [" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << std::endl << 105 | "p_ci: [" << p_ci_[0] << ", " << p_ci_[1] << ", " << p_ci_[2] << std::endl << 106 | "q_ci: (w,x,y,z): [" << q_ci_.w() << ", " << q_ci_.x() << ", " << q_ci_.y() << ", " << q_ci_.z() << "]"); 107 | } 108 | }; 109 | 110 | #endif /* POSITION_MEASUREMENTS_H */ 111 | -------------------------------------------------------------------------------- /ssf_updates/src/position_sensor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #include "position_sensor.h" 33 | #include 34 | 35 | #define N_MEAS 9 // measurement size 36 | PositionSensorHandler::PositionSensorHandler(ssf_core::Measurements* meas) : 37 | MeasurementHandler(meas) 38 | { 39 | ros::NodeHandle pnh("~"); 40 | pnh.param("measurement_world_sensor", measurement_world_sensor_, true); 41 | pnh.param("use_fixed_covariance", use_fixed_covariance_, false); 42 | 43 | ROS_INFO_COND(measurement_world_sensor_, "interpreting measurement as sensor w.r.t. world"); 44 | ROS_INFO_COND(!measurement_world_sensor_, "interpreting measurement as world w.r.t. sensor (e.g. ethzasl_ptam)"); 45 | 46 | ROS_INFO_COND(use_fixed_covariance_, "using fixed covariance"); 47 | ROS_INFO_COND(!use_fixed_covariance_, "using covariance from sensor"); 48 | 49 | subscribe(); 50 | } 51 | 52 | void PositionSensorHandler::subscribe() 53 | { 54 | ros::NodeHandle nh("ssf_core"); 55 | subMeasurement_ = nh.subscribe("position_measurement", 1, &PositionSensorHandler::measurementCallback, this); 56 | 57 | measurements->ssf_core_.registerCallback(&PositionSensorHandler::noiseConfig, this); 58 | 59 | nh.param("meas_noise1",n_zp_,0.0001); // default position noise for laser tracker total station 60 | 61 | } 62 | 63 | void PositionSensorHandler::noiseConfig(ssf_core::SSF_CoreConfig& config, uint32_t level) 64 | { 65 | // if(level & ssf_core::SSF_Core_MISC) 66 | // { 67 | this->n_zp_ = config.meas_noise1; 68 | 69 | // } 70 | } 71 | 72 | void PositionSensorHandler::measurementCallback(const ssf_updates::PositionWithCovarianceStampedConstPtr & msg) 73 | { 74 | // ROS_INFO_STREAM("measurement received \n" 75 | // << "type is: " << typeid(msg).name()); 76 | 77 | // init variables 78 | ssf_core::State state_old; 79 | ros::Time time_old = msg->header.stamp; 80 | Eigen::MatrixH_old; 81 | Eigen::Matrix r_old; 82 | Eigen::Matrix R; 83 | 84 | H_old.setZero(); 85 | R.setZero(); 86 | 87 | // get measurements 88 | z_p_ = Eigen::Matrix(msg->position.x, msg->position.y, msg->position.z); 89 | 90 | 91 | if (!use_fixed_covariance_) // take covariance from sensor 92 | { 93 | R.block(0,0,3,3) = Eigen::Matrix(&msg->covariance[0]); 94 | Eigen::Matrix buffvec = Eigen::Matrix::Constant(1e-6); 95 | R.block(3,3,6,6) = buffvec.asDiagonal(); // measurement noise for q_vw, q_ci 96 | } 97 | else // alternatively take fix covariance from reconfigure GUI 98 | { 99 | const double s_zp = n_zp_ * n_zp_; 100 | R = (Eigen::Matrix() << s_zp, s_zp, s_zp, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished().asDiagonal(); 101 | } 102 | 103 | // feedback for init case 104 | measurements->p_vc_ = z_p_; 105 | 106 | unsigned char idx = measurements->ssf_core_.getClosestState(&state_old,time_old,0); 107 | if (state_old.time_ == -1) 108 | return; // // early abort // // 109 | 110 | // get rotation matrices 111 | Eigen::Matrix C_wv = state_old.q_wv_.conjugate().toRotationMatrix(); 112 | Eigen::Matrix C_q = state_old.q_.conjugate().toRotationMatrix(); 113 | Eigen::Matrix C_ci = state_old.q_ci_.conjugate().toRotationMatrix(); 114 | 115 | // preprocess for elements in H matrix 116 | Eigen::Matrix vecold; 117 | vecold = (state_old.p_+C_q.transpose()*state_old.p_ci_)*state_old.L_; 118 | Eigen::Matrix skewold = skew(vecold); 119 | 120 | Eigen::Matrix pci_sk = skew(state_old.p_ci_); 121 | 122 | // construct H matrix using H-blockx :-) 123 | // position 124 | H_old.block<3,3>(0,0) = C_wv.transpose()*state_old.L_; // p 125 | H_old.block<3,3>(0,6) = -C_wv.transpose()*C_q.transpose()*pci_sk*state_old.L_; // q 126 | H_old.block<3,1>(0,15) = C_wv.transpose()*C_q.transpose()*state_old.p_ci_ + C_wv.transpose()*state_old.p_; // L 127 | H_old.block<3,3>(0,16) = -C_wv.transpose()*skewold; // q_wv 128 | H_old.block<3,3>(0,22) = C_wv.transpose()*C_q.transpose()*state_old.L_; // use "camera"-IMU distance p_ci state here as position_sensor-IMU distance 129 | H_old.block<3,3>(3,16) = Eigen::Matrix::Identity(); // fix vision world drift q_wv since it does not exist here 130 | H_old.block<3,3>(6,19) = Eigen::Matrix::Identity(); // fix "camera"-IMU drift q_ci since it does not exist here 131 | 132 | // construct residuals 133 | // position 134 | r_old.block<3,1>(0,0) = z_p_ - C_wv.transpose()*(state_old.p_ + C_q.transpose()*state_old.p_ci_)*state_old.L_; 135 | // vision world drift q_wv 136 | r_old.block<3,1>(3,0) = -state_old.q_wv_.vec()/state_old.q_wv_.w()*2; 137 | // "camera"-IMU drift q_ci 138 | r_old.block<3,1>(6,0) = -state_old.q_ci_.vec()/state_old.q_ci_.w()*2; 139 | 140 | // call update step in core class 141 | measurements->ssf_core_.applyMeasurement(idx,H_old,r_old,R); 142 | } 143 | -------------------------------------------------------------------------------- /ssf_updates/src/position_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without 9 | modification, are permitted provided that the following conditions are met: 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of ETHZ-ASL nor the 16 | names of its contributors may be used to endorse or promote products 17 | derived from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 23 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | */ 31 | 32 | #ifndef POSITION_SENSOR_H 33 | #define POSITION_SENSOR_H 34 | 35 | #include 36 | #include 37 | 38 | class PositionSensorHandler: public ssf_core::MeasurementHandler 39 | { 40 | private: 41 | // measurements 42 | // Eigen::Quaternion z_q_; /// attitude measurement camera seen from world - here we do not have an attitude measurement 43 | Eigen::Matrix z_p_; /// position measurement camera seen from world 44 | double n_zp_ /*, n_zq_*/; /// position and attitude measurement noise - here we do not have an attitude measurement 45 | 46 | ros::Subscriber subMeasurement_; 47 | 48 | bool measurement_world_sensor_; ///< defines if the pose of the sensor is measured in world coordinates (true, default) or vice versa (false, e.g. PTAM) 49 | bool use_fixed_covariance_; ///< use fixed covariance set by dynamic reconfigure 50 | 51 | void subscribe(); 52 | void measurementCallback(const ssf_updates::PositionWithCovarianceStampedConstPtr & msg); 53 | void noiseConfig(ssf_core::SSF_CoreConfig& config, uint32_t level); 54 | 55 | public: 56 | PositionSensorHandler(); 57 | PositionSensorHandler(ssf_core::Measurements* meas); 58 | }; 59 | 60 | #endif /* POSITION_SENSOR_H */ 61 | --------------------------------------------------------------------------------