├── .gitignore ├── README.md ├── gps_kf ├── CMakeLists.txt ├── include │ └── gps_kf │ │ ├── error_state_kf.hpp │ │ └── node.hpp ├── launch │ └── gps_kf.launch ├── msg │ └── Vector3WithCovarianceStamped.msg ├── package.xml └── src │ ├── main.cpp │ └── node.cpp ├── gps_odom ├── CMakeLists.txt ├── geoids │ ├── egm84-15.pgm │ ├── egm84-15.pgm.aux.xml │ ├── egm84-15.wld │ ├── egm84-30.pgm │ ├── egm84-30.pgm.aux.xml │ └── egm84-30.wld ├── include │ └── gps_odom │ │ └── node.hpp ├── install │ ├── README.md │ └── fixgeo.patch ├── launch │ └── gps_odom.launch ├── magnetic_models │ ├── igrf11.wmm │ ├── igrf11.wmm.cof │ ├── wmm2010.wmm │ └── wmm2010.wmm.cof ├── package.xml ├── src │ ├── main.cpp │ └── node.cpp └── tools │ ├── preview.html │ ├── red_circle.png │ └── red_circle@2x.png └── pressure_altimeter ├── CMakeLists.txt ├── include └── pressure_altimeter │ └── node.hpp ├── launch ├── pressure_altimeter.launch └── test.launch ├── msg └── Height.msg ├── package.xml └── src ├── main.cpp └── node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # BUILD PRODUCTS 2 | *.d 3 | *.o 4 | *.disasm 5 | *.map 6 | *.elf 7 | *.bin 8 | *.hex 9 | *.slo 10 | *.lo 11 | *.obj 12 | 13 | # Compiled python code 14 | *.pyc 15 | 16 | # CMAKE TEMPORARY FILES 17 | CMakeCache.txt 18 | CMakeFiles/ 19 | CMakeFiles/* 20 | 21 | # BUILD FOLDERS 22 | bin/ 23 | bin/* 24 | build/ 25 | build/* 26 | 27 | # ROS GENERATED PRODUCTS 28 | msg_gen/ 29 | msg_gen/* 30 | srv_gen/ 31 | srv_gen/* 32 | 33 | # ROS GENERATED PYTHON 34 | _*.py 35 | _*.pyc 36 | 37 | # ROS BAG FILES 38 | *.bag 39 | 40 | # ROS LOG FILES 41 | *.log 42 | 43 | # XCODE BUILD PRODUCTS 44 | *.build/ 45 | *.build/* 46 | 47 | # XCODE USER SETTINGS 48 | *.xcuserdatad/ 49 | *.xcuserdatad/* 50 | *.pbxuser 51 | *.pbxuser/* 52 | 53 | # QTCREATOR PROJECT 54 | *.includes 55 | *.creator 56 | *.creator.user 57 | *.creator.user.* 58 | *.config 59 | *.files 60 | 61 | # GEANY PROJECTS 62 | *.geany 63 | 64 | # JETBRAINS 65 | *.idea 66 | 67 | # ZEND/ECLIPSE 68 | *.buildpath 69 | *.settings 70 | *.project 71 | *.pydevproject 72 | 73 | # MACOSX SETTINGS 74 | *.DS_Store 75 | 76 | # GENERATED DOCUMENTATION 77 | html_docs/ 78 | html_docs/* 79 | 80 | # ANDROID SDK GENERATED JAVA 81 | gen/ 82 | gen/* 83 | 84 | # COMPILED DYNAMIC LIBRARIES 85 | *.so 86 | *.dylib 87 | *.dll 88 | 89 | # COMPILED STATIC LIBRARIES 90 | *.lai 91 | *.la 92 | *.a 93 | *.lib 94 | 95 | # EXECUTABLES 96 | *.exe 97 | *.out 98 | *.app 99 | *.pcd 100 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## gps_odom 2 | 3 | gps_odom accepts position and velocity (from GPS), height (from altimeter), and orientation (from IMU). At the output it generates a world-frame odometry message, suitable for use in a kalman filter, etc. 4 | 5 | Orientation is corrected by magnetic declination to geographic north. Odometry in meters is generated by setting a GPS reference point and calculating the local cartesian coordinates. 6 | 7 | All geographical corrections are provided by [GeographicLib](http://geographiclib.sourceforge.net). 8 | 9 | Depends on [kr_viz](https://github.com/KumarRobotics/kr_utils) for rviz messages. 10 | 11 | ### Input topics: 12 | 13 | - `fix`: instance of `sensor_msgs\NavSatFix`. 14 | - `fix_velocity`: instance of `geometry_msgs\TwistWithCovarianceStamped`. Angular velocity is ignored. 15 | - `imu`: instance of `sensor_msgs\Imu`. Only orientation component is used. 16 | - `pressure_height`: instance of `pressure_altimeter\Height`. 17 | -------------------------------------------------------------------------------- /gps_kf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gps_kf) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | eigen_conversions 6 | geometry_msgs 7 | kr_math 8 | message_generation 9 | nav_msgs 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | rviz_helper 14 | ) 15 | 16 | # generate messages 17 | add_message_files(FILES Vector3WithCovarianceStamped.msg) 18 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | ) 23 | 24 | include_directories( 25 | ${catkin_INCLUDE_DIRS} 26 | include 27 | ) 28 | 29 | add_definitions(-Wall -Werror -std=c++11 -DNDEBUG -O3) 30 | 31 | add_executable(${PROJECT_NAME} 32 | src/node.cpp 33 | src/main.cpp 34 | ) 35 | 36 | target_link_libraries(${PROJECT_NAME} 37 | ${catkin_LIBRARIES} 38 | ) 39 | 40 | add_dependencies(${PROJECT_NAME} 41 | ${catkin_EXPORTED_TARGETS} 42 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 43 | ) 44 | -------------------------------------------------------------------------------- /gps_kf/include/gps_kf/error_state_kf.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #ifndef ERROR_STATE_KF_HPP 6 | #define ERROR_STATE_KF_HPP 7 | 8 | /** 9 | * @class ErrorStateKF 10 | * @brief Implementation of error-state kalman filter for position and 11 | * orientation filtering. 12 | * 13 | * @note See: "Quaternion kinematics for the error-state KF", Joan Sola 14 | * @note See: "A Multi-State Constraint Kalman Filter for Vision-aided Inertial 15 | *Navigation", Mourikis and Roumeliotis 16 | */ 17 | template class ErrorStateKF { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | // Types 22 | typedef Eigen::Quaternion quat; 23 | typedef Eigen::Matrix vec3; 24 | typedef Eigen::Matrix mat3; 25 | typedef Eigen::Matrix mat15; 26 | 27 | /** 28 | * @brief Construct new instance of ESKF. 29 | * @note Initializes orientation to identity and all other state variables 30 | * to zero. 31 | */ 32 | ErrorStateKF(); 33 | 34 | /** 35 | * @brief Initialize a diagonal covariance matrix. 36 | * @param qVar Uncertainty on angular orientation (rad). 37 | * @param bgVar Uncertainty on gyro bias (rad/s). 38 | * @param vVar Uncertainty on velocity (rad/s). 39 | * @param baVar Uncertainty on acceleration bias (m/s^2). 40 | * @param pVar Uncertainty on position (m). 41 | */ 42 | void initCovariance(Scalar qStd, Scalar bgStd, Scalar vStd, Scalar baStd, 43 | Scalar pStd); 44 | 45 | /** 46 | * @brief initState 47 | * @param wQb 48 | * @param p 49 | * @param v 50 | */ 51 | void initState(const quat &wQb, const vec3 p, 52 | const vec3 &v); 53 | 54 | /** 55 | * @brief Run the prediction step of the filter. 56 | * @param wbody Body-frame angular velocity (rad/s). 57 | * @param varW Covariance of body-frame angular velocity. 58 | * @param abody Body-frame linear acceleration (m/s^2). 59 | * @param varA Covariance of body-frame linear acceleration. 60 | * @param dt Time step (seconds). 61 | */ 62 | void predict(const vec3 &wbody, const mat3 &varW, 63 | const vec3 &abody, const mat3 &varA, 64 | Scalar dt); 65 | 66 | /** 67 | * @brief Run the update step of the filter, with a velocity measurement. 68 | * @param qm Measured orientation as a quaternion. 69 | * @param varQ Covariance about x/y/z axes (rad). 70 | * @param pm Position (meters). 71 | * @param varP Covariance on position. 72 | * @param vm Velocity (m/s). 73 | * @param varV Covariance on velocity. 74 | * @return True if the update succeeds, false if the kalman gain is singular. 75 | */ 76 | bool update(const quat &qm, const mat3 &varQ, 77 | const vec3 &pm, const mat3 &varP, 78 | const vec3 &vm, const mat3 &varV); 79 | 80 | /** 81 | * @brief Orientation, transformation from body to world. 82 | * @return Quaternion. 83 | */ 84 | const quat &getOrientation() const { return q_; } 85 | 86 | /** 87 | * @brief Gyro bias estimate, rad/s. 88 | * @return R3 Vector. 89 | */ 90 | const vec3 &getGyroBias() const { return bg_; } 91 | 92 | /** 93 | * @brief Velocity estimate, m/s. 94 | * @return R3 Vector. 95 | */ 96 | const vec3 &getVelocity() const { return v_; } 97 | 98 | /** 99 | * @brief Accelerometer bias estimate, m/s^2. 100 | * @return R3 Vector. 101 | */ 102 | const vec3 &getAccelBias() const { return ba_; } 103 | 104 | /** 105 | * @brief Position estimate, m. 106 | * @return R3 Vector. 107 | */ 108 | const vec3 &getPosition() const { return p_; } 109 | 110 | /** 111 | * @brief Get system covariance. 112 | * @return 15x15 matrix. 113 | * @note Order of elements: [theta, gyro bias, vel, accel bias, pos] 114 | */ 115 | const mat15 &getCovariance() const { return P_; } 116 | 117 | /** 118 | * @brief Set std dev. of bias drift rates. 119 | * @param bg Gyro bias drift rate covariance, rad^2/s^4. 120 | * @param ba Accelerometer bias drift rate covariance, m^2/s^6. 121 | */ 122 | void setBiasUncertainties(const mat3 &bg, 123 | const mat3 &ba) { 124 | Qbg_ = bg; 125 | Qba_ = ba; 126 | } 127 | 128 | /** 129 | * @brief Set the magnitude of the gravity vector. 130 | * @param g Gravity magnitude, m/s^2. 131 | */ 132 | void setGravity(Scalar g) { g_ = g; } 133 | 134 | private: 135 | quat q_; /// Orientation 136 | vec3 bg_; /// Gyro bias 137 | vec3 v_; /// Velocity 138 | vec3 ba_; /// Accelerometer bias 139 | vec3 p_; /// Position 140 | 141 | mat15 P_; /// State covariance 142 | 143 | mat3 Qbg_; /// Gyro bias drift rate uncertainty 144 | mat3 Qba_; /// Accel bias drift rate uncertainty 145 | 146 | Scalar g_; /// Gravity constant 147 | }; 148 | 149 | template ErrorStateKF::ErrorStateKF() : g_(9.80665) { 150 | q_.setIdentity(); 151 | bg_.setZero(); 152 | v_.setZero(); 153 | ba_.setZero(); 154 | p_.setZero(); 155 | 156 | P_.setZero(); 157 | Qbg_.setZero(); 158 | Qba_.setZero(); 159 | } 160 | 161 | template 162 | void ErrorStateKF::initCovariance(Scalar qStd, Scalar bgStd, 163 | Scalar vStd, Scalar baStd, 164 | Scalar pStd) { 165 | static const mat3 I3 = mat3::Identity(); 166 | 167 | P_.template block<3, 3>(0, 0) = I3 * qStd * qStd; 168 | P_.template block<3, 3>(3, 3) = I3 * bgStd * bgStd; 169 | P_.template block<3, 3>(6, 6) = I3 * vStd * vStd; 170 | P_.template block<3, 3>(9, 9) = I3 * baStd * baStd; 171 | P_.template block<3, 3>(12, 12) = I3 * pStd * pStd; 172 | } 173 | 174 | template 175 | void ErrorStateKF::initState(const ErrorStateKF::quat &wQb, 176 | const ErrorStateKF::vec3 p, 177 | const ErrorStateKF::vec3 &v) { 178 | q_ = wQb; 179 | p_ = p; 180 | v_ = v; 181 | } 182 | 183 | template 184 | void ErrorStateKF::predict(const ErrorStateKF::vec3 &wbody, 185 | const ErrorStateKF::mat3 &varW, 186 | const ErrorStateKF::vec3 &abody, 187 | const ErrorStateKF::mat3 &varA, 188 | Scalar dt) { 189 | 190 | const vec3 ah = abody - ba_; // corrected inputs 191 | const vec3 wh = wbody - bg_; 192 | const mat3 wRb = q_.matrix(); // current orientation 193 | 194 | // integrate state forward w/ euler equations 195 | const quat dq = 196 | q_ * quat(0, wh[0] * 0.5, wh[1] * 0.5, wh[2] * 0.5); 197 | q_.w() += dq.w() * dt; 198 | q_.x() += dq.x() * dt; 199 | q_.y() += dq.y() * dt; 200 | q_.z() += dq.z() * dt; 201 | q_.normalize(); 202 | p_ += v_ * dt; 203 | v_ += (wRb * ah + vec3(0, 0, -g_)) * dt; 204 | 205 | // construct error-state jacobian 206 | mat15 F; 207 | F.setZero(); 208 | 209 | /// dth = [wh] x dth - bg 210 | F.template block<3, 3>(0, 0) = -kr::skewSymmetric(wh); 211 | F.template block<3, 3>(0, 3) = -mat3::Identity(); 212 | 213 | // dv = -R[ah] x dth - R[ba] 214 | F.template block<3, 3>(6, 0) = -wRb * kr::skewSymmetric(ah); 215 | F.template block<3, 3>(6, 9) = -wRb; 216 | 217 | // dp = dv 218 | F.template block<3, 3>(12, 6).setIdentity(); 219 | 220 | // form process covariance matrix 221 | Eigen::Matrix Q; 222 | Q.setZero(); 223 | Q.template block<3, 3>(0, 0) = varW; 224 | Q.template block<3, 3>(3, 3) = Qbg_; 225 | Q.template block<3, 3>(6, 6) = varA; 226 | Q.template block<3, 3>(9, 9) = Qba_; 227 | 228 | Eigen::Matrix G; 229 | G.setZero(); 230 | 231 | // angular vel. variance on error state angle 232 | G.template block<3, 3>(0, 0) = mat3::Identity() * -1; 233 | G.template block<3, 3>(3, 3).setIdentity(); 234 | G.template block<3, 3>(6, 6) = -wRb; // acceleration on linear velocity 235 | G.template block<3, 3>(9, 9).setIdentity(); 236 | 237 | // integrate covariance forward 238 | P_ += (F * P_ + P_ * F.transpose() + G * Q * G.transpose()) * dt; 239 | } 240 | 241 | template 242 | bool ErrorStateKF::update(const ErrorStateKF::quat &qm, 243 | const ErrorStateKF::mat3 &varQ, 244 | const ErrorStateKF::vec3 &pm, 245 | const ErrorStateKF::mat3 &varP, 246 | const ErrorStateKF::vec3 &vm, 247 | const ErrorStateKF::mat3 &varV) { 248 | // form the measurement jacobian 249 | Eigen::Matrix H; 250 | H.setZero(); 251 | 252 | // position and velocity 253 | H.template block<3, 3>(0, 12).setIdentity(); 254 | H.template block<3, 3>(3, 6).setIdentity(); 255 | 256 | // orientation 257 | // H.template block<3, 3>(6, 0).setIdentity(); 258 | H(6, 2) = 1; 259 | 260 | // residual 261 | Eigen::Matrix r; 262 | 263 | // non-linear rotation residual on yaw axis 264 | const quat dq = q_.conjugate() * qm; 265 | const Eigen::AngleAxis aa(dq); 266 | const vec3 rpy = kr::rotToEulerZYX(aa.matrix()); 267 | 268 | // r.template block<3, 1>(6, 0) = aa.angle()*aa.axis(); 269 | r(6, 0) = rpy[2]; 270 | 271 | // linear pos/velocity 272 | r.template block<3, 1>(0, 0) = pm - p_; 273 | r.template block<3, 1>(3, 0) = vm - v_; 274 | 275 | // measurement covariance 276 | Eigen::Matrix R; 277 | R.setZero(); 278 | R.template block<3, 3>(0, 0) = varP; 279 | R.template block<3, 3>(3, 3) = varV; 280 | // R.template block<3, 3>(6, 6) = varQ; 281 | R(6, 6) = varQ(2, 2); 282 | 283 | // kalman update 284 | Eigen::Matrix S = H * P_ * H.transpose() + R; 285 | auto LU = S.fullPivLu(); 286 | if (!LU.isInvertible()) { 287 | return false; 288 | } 289 | S = LU.inverse(); 290 | 291 | const Eigen::Matrix K = P_ * H.transpose() * S; 292 | const Eigen::Matrix dx = K * r; 293 | 294 | P_ = (Eigen::Matrix::Identity() - K * H) * P_; 295 | 296 | // update state 297 | q_ *= quat(1, dx[0] * 0.5, dx[1] * 0.5, dx[2] * 0.5); 298 | q_.normalize(); 299 | bg_.noalias() += dx.template block<3, 1>(3, 0); 300 | v_.noalias() += dx.template block<3, 1>(6, 0); 301 | ba_.noalias() += dx.template block<3, 1>(9, 0); 302 | p_.noalias() += dx.template block<3, 1>(12, 0); 303 | 304 | return true; 305 | } 306 | 307 | #endif // ERROR_STATE_KF_HPP 308 | -------------------------------------------------------------------------------- /gps_kf/include/gps_kf/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.hpp 3 | * 4 | * This file is part of gps_kf. 5 | * 6 | * Created on: 03/08/2014 7 | * Author: gareth 8 | */ 9 | 10 | #ifndef GPS_KF_NODE_HPP_ 11 | #define GPS_KF_NODE_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "gps_kf/error_state_kf.hpp" 20 | 21 | #include "rviz_helper/marker_visualizer.hpp" 22 | #include "rviz_helper/tf_publisher.hpp" 23 | 24 | namespace gps_kf { 25 | 26 | class Node { 27 | public: 28 | static constexpr double kOneG = 9.80665; 29 | 30 | /** 31 | * @brief Construct node and load parameters. 32 | */ 33 | Node(); 34 | 35 | /** 36 | * @brief Subscribe to topics and advertise outputs. 37 | */ 38 | void initialize(); 39 | 40 | private: 41 | void imuCallback(const sensor_msgs::ImuConstPtr &imu); 42 | 43 | void odoCallback(const nav_msgs::OdometryConstPtr &odometry); 44 | 45 | ErrorStateKF positionKF_; 46 | double gyroBiasDriftStd_; 47 | double accelBiasDriftStd_; 48 | double initDeadTime_; 49 | 50 | bool initialized_{false}; 51 | 52 | ros::NodeHandle nh_; 53 | ros::Publisher pubOdometry_; 54 | ros::Publisher pubAccelBias_; 55 | ros::Publisher pubGyroBias_; 56 | ros::Publisher pubPose_; 57 | ros::Subscriber subImu_; 58 | ros::Subscriber subOdometry_; 59 | ros::Time predictTime_; 60 | 61 | std::string worldFrameId_; 62 | kr::viz::TfPublisher tfPub_; 63 | kr::viz::TrajectoryVisualizer trajViz_; 64 | kr::viz::CovarianceVisualizer covViz_; 65 | }; 66 | 67 | } // namespace gps_kf 68 | 69 | #endif // GPS_KF_NODE_HPP_ 70 | -------------------------------------------------------------------------------- /gps_kf/launch/gps_kf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /gps_kf/msg/Vector3WithCovarianceStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # Instance of Vector3 4 | geometry_msgs/Vector3 vector 5 | 6 | # 3x3 Covariance matrix in order [x,y,z] 7 | float64[9] covariance 8 | -------------------------------------------------------------------------------- /gps_kf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps_kf 4 | 0.0.3 5 | Kalman filter to fuse IMU with GPS 6 | 7 | gareth 8 | Proprietary 9 | 10 | catkin 11 | 12 | eigen_conversions 13 | geometry_msgs 14 | kr_math 15 | message_generation 16 | nav_msgs 17 | roscpp 18 | sensor_msgs 19 | std_msgs 20 | rviz_helper 21 | 22 | eigen_conversions 23 | geometry_msgs 24 | kr_math 25 | message_runtime 26 | nav_msgs 27 | roscpp 28 | sensor_msgs 29 | std_msgs 30 | rviz_helper 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /gps_kf/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * This file is part of gps_odom. 5 | * 6 | * Created on: 03/08/2014 7 | * Author: gareth 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | int main(int argc, char ** argv) { 14 | ros::init(argc,argv,"gps_kf"); 15 | gps_kf::Node node; 16 | node.initialize(); 17 | ros::spin(); 18 | } 19 | -------------------------------------------------------------------------------- /gps_kf/src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.cpp 3 | * 4 | * This file is part of gps_kf. 5 | * 6 | * Created on: 03/08/2014 7 | * Author: gareth 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using ::gps_kf::Vector3WithCovarianceStamped; 17 | 18 | namespace gps_kf { 19 | 20 | Node::Node() : nh_("~"), trajViz_(nh_), covViz_(nh_) {} 21 | 22 | void Node::initialize() { 23 | // configure topics 24 | pubOdometry_ = nh_.advertise("odometry", 1); 25 | pubAccelBias_ = nh_.advertise("accel_bias", 1); 26 | pubGyroBias_ = nh_.advertise("gyro_bias", 1); 27 | pubPose_ = nh_.advertise("pose", 1); 28 | 29 | subImu_ = nh_.subscribe("imu", 1, &Node::imuCallback, this); 30 | subOdometry_ = nh_.subscribe("gps_odom", 1, &Node::odoCallback, this); 31 | 32 | nh_.param("gyro_bias_drift_std", gyroBiasDriftStd_, 1.0e-3); 33 | nh_.param("accel_bias_drift_std", accelBiasDriftStd_, 1.0e-2); 34 | 35 | // amount of time in seconds to wait before initializing filter 36 | nh_.param("init_dead_time", initDeadTime_, 0.25); 37 | 38 | ROS_INFO("Gyro bias drift uncertainty: %f", gyroBiasDriftStd_); 39 | ROS_INFO("Accel bias drift uncertainty: %f", accelBiasDriftStd_); 40 | ROS_INFO("Initialization dead time: %f", initDeadTime_); 41 | 42 | predictTime_ = ros::Time(0, 0); 43 | trajViz_.SetColor(kr::viz::colors::CYAN); 44 | trajViz_.SetAlpha(1); 45 | trajViz_.set_num_skip(12); 46 | trajViz_.SetScale(0.15); 47 | covViz_.SetColor(kr::viz::colors::CYAN); // transparent cyan 48 | covViz_.SetAlpha(0.5); 49 | } 50 | 51 | void Node::imuCallback(const sensor_msgs::ImuConstPtr &imu) { 52 | if (!initialized_) { 53 | return; // wait for first GPS 54 | } 55 | tfPub_.set_child_frame_id(imu->header.frame_id); 56 | 57 | Eigen::Vector3d accel; 58 | Eigen::Matrix3d varAccel; 59 | tf::vectorMsgToEigen(imu->linear_acceleration, accel); 60 | 61 | Eigen::Vector3d gyro; 62 | Eigen::Matrix3d varGyro; 63 | tf::vectorMsgToEigen(imu->angular_velocity, gyro); 64 | 65 | Eigen::Quaterniond wQb; 66 | Eigen::Matrix3d varRot; 67 | tf::quaternionMsgToEigen(imu->orientation, wQb); 68 | 69 | for (int i = 0; i < 3; i++) { 70 | for (int j = 0; j < 3; j++) { 71 | varAccel(i, j) = imu->linear_acceleration_covariance[i * 3 + j]; 72 | varGyro(i, j) = imu->angular_velocity_covariance[i * 3 + j]; 73 | varRot(i, j) = imu->orientation_covariance[i * 3 + j]; 74 | } 75 | } 76 | 77 | double delta = 0.0; 78 | if (predictTime_.sec != 0) { 79 | delta = (imu->header.stamp - predictTime_).toSec(); 80 | } 81 | predictTime_ = imu->header.stamp; 82 | 83 | // predict 84 | Eigen::Matrix3d Qbg; 85 | Qbg.setIdentity(); 86 | Qbg *= gyroBiasDriftStd_ * gyroBiasDriftStd_; 87 | Eigen::Matrix3d Qba; 88 | Qba.setIdentity(); 89 | Qba *= accelBiasDriftStd_ * accelBiasDriftStd_; 90 | positionKF_.setBiasUncertainties(Qbg, Qba); 91 | positionKF_.predict(gyro, varGyro, accel, varAccel, delta); 92 | 93 | // output covariance 94 | const Eigen::Matrix &P = positionKF_.getCovariance(); 95 | const auto &rotCov = P.block<3, 3>(0, 0); 96 | const auto &gBiasCov = P.block<3, 3>(3, 3); 97 | const auto &velCov = P.block<3, 3>(6, 6); 98 | const auto &aBiasCov = P.block<3, 3>(9, 9); 99 | const auto &posCov = P.block<3, 3>(12, 12); 100 | 101 | // publish odometry 102 | nav_msgs::Odometry odo; 103 | odo.header.stamp = imu->header.stamp; 104 | odo.header.frame_id = worldFrameId_; 105 | odo.child_frame_id = worldFrameId_; 106 | // NOTE: angular velocity is still in body frame, but we use worldFrameId_ 107 | // anyways 108 | tf::pointEigenToMsg(positionKF_.getPosition(), odo.pose.pose.position); 109 | tf::quaternionEigenToMsg(positionKF_.getOrientation(), 110 | odo.pose.pose.orientation); 111 | 112 | geometry_msgs::PoseStamped pose; 113 | pose.pose = odo.pose.pose; 114 | pose.header = odo.header; 115 | pubPose_.publish(pose); 116 | 117 | tfPub_.PublishTransform(odo.pose.pose, odo.header); 118 | trajViz_.PublishTrajectory(pose.pose.position, pose.header); 119 | 120 | // top left 3x3 (filter) and bottom right 3x3 (from imu) 121 | for (int i = 0; i < 3; i++) { 122 | for (int j = 0; j < 3; j++) { 123 | odo.pose.covariance[(i * 6) + j] = posCov(i, j); 124 | odo.pose.covariance[(i + 3) * 6 + (j + 3)] = rotCov(i, j); 125 | } 126 | } 127 | 128 | tf::vectorEigenToMsg(positionKF_.getVelocity(), odo.twist.twist.linear); 129 | 130 | // subtract our bias estimate and propagate covariance 131 | Eigen::Vector3d angRate; 132 | tf::vectorMsgToEigen(imu->angular_velocity, angRate); 133 | angRate.noalias() -= positionKF_.getGyroBias(); 134 | varGyro.noalias() += gBiasCov; 135 | tf::vectorEigenToMsg(angRate, odo.twist.twist.angular); 136 | 137 | // output velocity covariances 138 | for (int i = 0; i < 3; i++) { 139 | for (int j = 0; j < 3; j++) { 140 | odo.twist.covariance[(i * 6) + j] = velCov(i, j); 141 | odo.twist.covariance[(i + 3) * 6 + (j + 3)] = varGyro(i, j); 142 | } 143 | } 144 | pubOdometry_.publish(odo); 145 | covViz_.PublishCovariance(odo); 146 | 147 | // publish bias messages 148 | Vector3WithCovarianceStamped aBiasVector, gBiasVector; 149 | 150 | aBiasVector.header.stamp = odo.header.stamp; 151 | aBiasVector.header.frame_id = imu->header.frame_id; 152 | tf::vectorEigenToMsg(positionKF_.getAccelBias(), aBiasVector.vector); 153 | 154 | gBiasVector.header.stamp = odo.header.stamp; 155 | gBiasVector.header.frame_id = imu->header.frame_id; 156 | tf::vectorEigenToMsg(positionKF_.getGyroBias(), gBiasVector.vector); 157 | 158 | for (int i = 0; i < 3; i++) { 159 | for (int j = 0; j < 3; j++) { 160 | aBiasVector.covariance[i * 3 + j] = aBiasCov(i, j); 161 | gBiasVector.covariance[i * 3 + j] = gBiasCov(i, j); 162 | } 163 | } 164 | 165 | pubAccelBias_.publish(aBiasVector); 166 | pubGyroBias_.publish(gBiasVector); 167 | } 168 | 169 | void Node::odoCallback(const nav_msgs::OdometryConstPtr &odometry) { 170 | static ros::Time firstTs = odometry->header.stamp; 171 | 172 | Eigen::Vector3d p, v; 173 | Eigen::Quaterniond q; 174 | tf::quaternionMsgToEigen(odometry->pose.pose.orientation, q); 175 | tf::pointMsgToEigen(odometry->pose.pose.position, p); 176 | tf::vectorMsgToEigen(odometry->twist.twist.linear, v); 177 | 178 | Eigen::Matrix3d varP, varV, varQ; 179 | for (int i = 0; i < 3; i++) { 180 | for (int j = 0; j < 3; j++) { 181 | varP(i, j) = odometry->pose.covariance[(i * 6) + j]; 182 | varQ(i, j) = odometry->pose.covariance[(i + 3) * 6 + j + 3]; 183 | varV(i, j) = odometry->twist.covariance[(i * 6) + j]; 184 | } 185 | } 186 | 187 | if (!initialized_ && 188 | (odometry->header.stamp - firstTs).toSec() > initDeadTime_) { 189 | initialized_ = true; 190 | // take our output frame ID from the gps_odom 191 | worldFrameId_ = odometry->header.frame_id; 192 | positionKF_.initState(q, p, v); 193 | positionKF_.initCovariance(1e-1, 1e-4, 0.8, 0.2, 5.0); 194 | } else { 195 | if (!positionKF_.update(q, varQ, p, varP, v, varV)) { 196 | ROS_WARN("Warning: Kalman gain was singular in update"); 197 | } 198 | } 199 | } 200 | } 201 | -------------------------------------------------------------------------------- /gps_odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gps_odom) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | message_filters 7 | nav_msgs 8 | roscpp 9 | roslib 10 | sensor_msgs 11 | std_msgs 12 | pressure_altimeter 13 | rviz_helper 14 | visualization_msgs 15 | ) 16 | find_package(GeographicLib 1.35 REQUIRED) 17 | 18 | catkin_package() 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ${GeographicLib_INCLUDE_DIRS} 23 | include 24 | ) 25 | 26 | add_definitions(${GeographicLib_DEFINITIONS}) 27 | 28 | add_executable(${PROJECT_NAME} 29 | src/main.cpp 30 | src/node.cpp 31 | ) 32 | 33 | add_definitions("-Wall -std=c++11") 34 | 35 | add_dependencies(${PROJECT_NAME} 36 | ${catkin_EXPORTED_TARGETS} 37 | ) 38 | 39 | target_link_libraries(${PROJECT_NAME} 40 | ${catkin_LIBRARIES} 41 | ${GeographicLib_LIBRARIES} 42 | ) 43 | -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-15.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/geoids/egm84-15.pgm -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-15.pgm.aux.xml: -------------------------------------------------------------------------------- 1 | 2 | GEOGCS["WGS 84",DATUM["WGS_1984",SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],AUTHORITY["EPSG","6326"]],PRIMEM["Greenwich",0],UNIT["degree",0.017453292519943295769],AUTHORITY["EPSG","4326"]] 3 | -0.12500000000000000,0.25000000000000000,0,90.12500000000000000,0,-0.25000000000000000 4 | 5 | WGS84 EGM84, 15-minute grid 6 | http://earth-info.nga.mil/GandG/wgs84/gravitymod/wgs84_180/wgs84_180.html 7 | 2009-08-29 18:45:02 8 | 0.413 9 | 0.018 10 | 0.020 11 | 0.001 12 | -108 13 | 0.003 14 | Point 15 | WGS84 16 | pixel_corner 17 | 18 | 19 | -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-15.wld: -------------------------------------------------------------------------------- 1 | 0.25000000000000000 2 | 0 3 | 0 4 | -0.25000000000000000 5 | 0 6 | 90 7 | -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-30.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/geoids/egm84-30.pgm -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-30.pgm.aux.xml: -------------------------------------------------------------------------------- 1 | 2 | GEOGCS["WGS 84",DATUM["WGS_1984",SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],AUTHORITY["EPSG","6326"]],PRIMEM["Greenwich",0],UNIT["degree",0.017453292519943295769],AUTHORITY["EPSG","4326"]] 3 | -0.25000000000000000,0.50000000000000000,0,90.25000000000000000,0,-0.50000000000000000 4 | 5 | WGS84 EGM84, 30-minute grid 6 | http://earth-info.nga.mil/GandG/wgs84/gravitymod/wgs84_180/wgs84_180.html 7 | 2009-08-29 18:45:02 8 | 1.546 9 | 0.070 10 | 0.274 11 | 0.014 12 | -108 13 | 0.003 14 | Point 15 | WGS84 16 | pixel_corner 17 | 18 | 19 | -------------------------------------------------------------------------------- /gps_odom/geoids/egm84-30.wld: -------------------------------------------------------------------------------- 1 | 0.50000000000000000 2 | 0 3 | 0 4 | -0.50000000000000000 5 | 0 6 | 90 7 | -------------------------------------------------------------------------------- /gps_odom/include/gps_odom/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.hpp 3 | * 4 | * This file is part of gps_odom. 5 | * 6 | * Created on: 31/07/2014 7 | * Author: gareth 8 | */ 9 | 10 | #ifndef GPS_ODOM_NODE_HPP_ 11 | #define GPS_ODOM_NODE_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include "rviz_helper/marker_visualizer.hpp" 34 | #include "rviz_helper/tf_publisher.hpp" 35 | 36 | namespace gps_odom { 37 | 38 | class Node { 39 | public: 40 | Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh); 41 | 42 | private: 43 | static constexpr int kROSQueueSize = 200; 44 | 45 | ros::NodeHandle nh_, pnh_; 46 | std::string pkgPath_; 47 | std::string fixedFrame_; 48 | ros::Publisher pubOdometry_; 49 | ros::Publisher pubRefPoint_; 50 | 51 | message_filters::Subscriber subImu_; 52 | message_filters::Subscriber subFix_; 53 | message_filters::Subscriber 54 | subFixTwist_; 55 | message_filters::Subscriber subHeight_; 56 | 57 | // time sync policy for GPS data 58 | using TimeSyncGPS = message_filters::sync_policies::ApproximateTime< 59 | sensor_msgs::NavSatFix, geometry_msgs::TwistWithCovarianceStamped, 60 | sensor_msgs::Imu, pressure_altimeter::Height>; 61 | using SynchronizerGPS = message_filters::Synchronizer; 62 | std::shared_ptr syncGps_; 63 | 64 | void gpsCallback( 65 | const sensor_msgs::NavSatFixConstPtr &, 66 | const geometry_msgs::TwistWithCovarianceStampedConstPtr &navSatTwist, 67 | const sensor_msgs::ImuConstPtr &, 68 | const pressure_altimeter::HeightConstPtr &height); 69 | 70 | /// Load models from disk, if required. 71 | bool loadModels(); 72 | 73 | // geographic lib objects 74 | std::shared_ptr geoid_; 75 | std::shared_ptr magneticModel_; 76 | 77 | GeographicLib::LocalCartesian refPoint_; 78 | double refPressureHeight_; 79 | sensor_msgs::NavSatFix refFix_; 80 | bool refInitialized_{false}; 81 | 82 | bool refPublished_{false}; 83 | double currentDeclination_{0}; ///< Magnetic declination from north 84 | 85 | // Parameters 86 | double gpsCovScaleFactor_; 87 | bool shouldPublishTf_; 88 | double gpsRefLat_{0}, gpsRefLon_{0}; 89 | bool useFixedRef_{false}; 90 | 91 | kr::viz::TfPublisher tfPub_; 92 | kr::viz::TrajectoryVisualizer trajViz_; 93 | kr::viz::CovarianceVisualizer covViz_; 94 | }; 95 | 96 | } // namespace_gps_odom 97 | 98 | #endif // GPS_ODOM_NODE_HPP_ 99 | -------------------------------------------------------------------------------- /gps_odom/install/README.md: -------------------------------------------------------------------------------- 1 | ### Installing GeographicLib 2 | 3 | 1. Download the latest version of GeographicLib from [SourceForge](http://sourceforge.net/projects/geographiclib/files/distrib/). 4 | 5 | 2. Build and install GeographicLib using the following commands (assuming you are in the `GeographicLib-1.35` directory): 6 | ```bash 7 | mkdir build 8 | cd build 9 | cmake .. 10 | make -j4 11 | sudo make install 12 | ``` 13 | 14 | ### For version 1.35-36 15 | 16 | Versions 1.35 and 1.36 have a makefile failure which can be corrected using the patch provided in this folder: 17 | 18 | 1. `cd` into the directory you downloaded, and apply `fixgeo.patch` using the following command: 19 | ```bash 20 | patch -p1 < fixgeo.patch 21 | ``` 22 | 2. You should see output indicating that a documentation file has been patched. 23 | 24 | 3. Build GeoGraphiclib and install it, as specified above. **gps_odom** should now build without issue. 25 | -------------------------------------------------------------------------------- /gps_odom/install/fixgeo.patch: -------------------------------------------------------------------------------- 1 | diff -rupN GeographicLib-1.35/man/MagneticField.pod GeographicLib-1.35-mod/man/MagneticField.pod 2 | --- GeographicLib-1.35/man/MagneticField.pod 2014-03-13 14:57:22.000000000 -0400 3 | +++ GeographicLib-1.35-mod/man/MagneticField.pod 2014-08-04 17:37:35.413594413 -0400 4 | @@ -149,14 +149,14 @@ B computes the geomagneti 5 | following models 6 | 7 | wmm2010, the World Magnetic Model 2010, which approximates the 8 | - main magnetic field for the period 2010–2015. See 9 | + main magnetic field for the period 2010 to 2015. See 10 | http://ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml 11 | igrf11, the International Geomagnetic Reference Field (11th 12 | generation) which approximates the main magnetic field for 13 | - the period 1900–2015. See 14 | + the period 1900 to 2015. See 15 | http://ngdc.noaa.gov/IAGA/vmod/igrf.html 16 | emm2010, the Enhanced Magnetic Model 2010, which approximates the 17 | - main and crustal magnetic fields for the period 2010–2015. See 18 | + main and crustal magnetic fields for the period 2010 to 2015. See 19 | http://ngdc.noaa.gov/geomag/EMM/index.html 20 | 21 | These models approximate the magnetic field due to the earth's core and 22 | -------------------------------------------------------------------------------- /gps_odom/launch/gps_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /gps_odom/magnetic_models/igrf11.wmm: -------------------------------------------------------------------------------- 1 | WMMF-1 2 | # A World Magnetic Model (Format 1) file. For documentation on the 3 | # format of this file see 4 | # http://geographiclib.sf.net/html/magnetic.html#magneticformat 5 | Name igrf11 6 | Description International Geomagnetic Reference Field 11th Generation 7 | URL http://ngdc.noaa.gov/IAGA/vmod/igrf.html 8 | Publisher International Association of Geomagnetism and Aeronomy 9 | ReleaseDate 2009-12-15 10 | DataCutOff 2009-10-01 11 | ConversionDate 2011-11-04 12 | DataVersion 1 13 | Radius 6371200 14 | NumModels 23 15 | Epoch 1900 16 | DeltaEpoch 5 17 | MinTime 1900 18 | MaxTime 2015 19 | MinHeight -1000 20 | MaxHeight 600000 21 | 22 | # The coefficients are stored in a file obtained by appending ".cof" to 23 | # the name of this file. The coefficients were obtained from IGRF11.COF 24 | # in the geomag70 distribution. 25 | ID IGRF11-A 26 | -------------------------------------------------------------------------------- /gps_odom/magnetic_models/igrf11.wmm.cof: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/magnetic_models/igrf11.wmm.cof -------------------------------------------------------------------------------- /gps_odom/magnetic_models/wmm2010.wmm: -------------------------------------------------------------------------------- 1 | WMMF-1 2 | # A World Magnetic Model (Format 1) file. For documentation on the 3 | # format of this file see 4 | # http://geographiclib.sf.net/html/magnetic.html#magneticformat 5 | Name wmm2010 6 | Description World Magnetic Model 2010 7 | URL http://ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml 8 | Publisher National Oceanic and Atmospheric Administration 9 | ReleaseDate 2009-12-15 10 | DataCutOff 2009-10-01 11 | ConversionDate 2011-11-04 12 | DataVersion 1 13 | Radius 6371200 14 | Epoch 2010 15 | MinTime 2010 16 | MaxTime 2015 17 | MinHeight -10000 18 | MaxHeight 1000000 19 | 20 | # The coefficients are stored in a file obtained by appending ".cof" to 21 | # the name of this file. The coefficients were obtained from 22 | # WMM2010.COF in the geomag70 distribution. 23 | ID WMM2010A 24 | -------------------------------------------------------------------------------- /gps_odom/magnetic_models/wmm2010.wmm.cof: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/magnetic_models/wmm2010.wmm.cof -------------------------------------------------------------------------------- /gps_odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gps_odom 4 | 0.0.2 5 | Converts IMU+GPS messages into odometry for UKF 6 | 7 | gareth 8 | BSD 9 | 10 | catkin 11 | geometry_msgs 12 | message_filters 13 | nav_msgs 14 | roscpp 15 | roslib 16 | sensor_msgs 17 | std_msgs 18 | pressure_altimeter 19 | rviz_helper 20 | 21 | geometry_msgs 22 | message_filters 23 | nav_msgs 24 | roscpp 25 | roslib 26 | sensor_msgs 27 | std_msgs 28 | pressure_altimeter 29 | rviz_helper 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /gps_odom/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * This file is part of gps_odom. 5 | * 6 | * Created on: 31/07/2014 7 | * Author: gareth 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | int main(int argc, char** argv) { 14 | ros::init(argc, argv, "gps_odom"); 15 | ros::NodeHandle nh, pnh("~"); 16 | try { 17 | gps_odom::Node node(nh, pnh); 18 | ros::spin(); 19 | } 20 | catch (const std::exception& e) { 21 | ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what()); 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /gps_odom/src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.cpp 3 | * 4 | * This file is part of gps_odom. 5 | * 6 | * Created on: 31/07/2014 7 | * Author: gareth 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace gps_odom { 19 | 20 | Node::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) 21 | : nh_(nh), 22 | pnh_(pnh), 23 | pkgPath_(ros::package::getPath("gps_odom")), 24 | trajViz_(nh_), 25 | covViz_(nh_) { 26 | 27 | if (pkgPath_.empty()) { 28 | ROS_WARN("Failed to find path for package"); 29 | } 30 | // ID to place on outgoing odometry 31 | pnh_.param("fixed_frame", fixedFrame_, "world"); 32 | 33 | // scale factor for GPS covariance 34 | pnh_.param("gps_covariance_scale_factor", gpsCovScaleFactor_, 1.0); 35 | pnh_.param("publish_tf", shouldPublishTf_, false); 36 | 37 | if (pnh_.hasParam("reference/latitude") && 38 | pnh_.hasParam("reference/longitude")) { 39 | // user has requested we use a fixed reference point 40 | pnh_.getParam("reference/latitude", gpsRefLat_); 41 | pnh_.getParam("reference/longitude", gpsRefLon_); 42 | useFixedRef_ = true; 43 | 44 | ROS_INFO("Using GPS reference point: %.7f, %.7f", gpsRefLat_, gpsRefLon_); 45 | } 46 | 47 | // IMU and GPS are synchronized separately 48 | subFix_.subscribe(nh_, "fix", kROSQueueSize); 49 | subFixTwist_.subscribe(nh_, "fix_velocity", kROSQueueSize); 50 | 51 | subImu_.subscribe(pnh_, "imu", kROSQueueSize); 52 | subHeight_.subscribe(pnh_, "pressure_height", kROSQueueSize); 53 | 54 | syncGps_ = std::make_shared( 55 | TimeSyncGPS(kROSQueueSize), subFix_, subFixTwist_, subImu_, subHeight_); 56 | syncGps_->registerCallback( 57 | boost::bind(&Node::gpsCallback, this, _1, _2, _3, _4)); 58 | 59 | // advertise odometry as output 60 | pubOdometry_ = nh_.advertise("odometry", 1); 61 | // reference point on a latched topic 62 | pubRefPoint_ = nh_.advertise("reference", 1, true); 63 | 64 | ROS_INFO("Using a GPS covariance scale factor of %f", gpsCovScaleFactor_); 65 | 66 | trajViz_.SetColor(kr::viz::colors::RED); 67 | trajViz_.SetAlpha(1); 68 | covViz_.SetColor(kr::viz::colors::RED); 69 | covViz_.SetAlpha(0.5); 70 | } 71 | 72 | void Node::gpsCallback( 73 | const sensor_msgs::NavSatFixConstPtr &navSatFix, 74 | const geometry_msgs::TwistWithCovarianceStampedConstPtr &navSatTwist, 75 | const sensor_msgs::ImuConstPtr &imu, 76 | const pressure_altimeter::HeightConstPtr &height) { 77 | 78 | const double lat = navSatFix->latitude; 79 | const double lon = navSatFix->longitude; 80 | const double hWGS84 = navSatFix->altitude; 81 | const double tYears = 82 | navSatFix->header.stamp.toSec() / (365 * 24 * 3600.0) + 1970; 83 | 84 | // will load models from disk if possible 85 | if (!loadModels()) { 86 | return; 87 | } 88 | 89 | // convert to height above sea level 90 | const double hMSL = geoid_->ConvertHeight( 91 | lat, lon, hWGS84, GeographicLib::Geoid::ELLIPSOIDTOGEOID); 92 | 93 | if (!refInitialized_) { 94 | 95 | // height is always taken from altimeter 96 | refPressureHeight_ = height->height; 97 | 98 | if (useFixedRef_) { 99 | // initialize using user provided lat/lon 100 | refPoint_ = GeographicLib::LocalCartesian(gpsRefLat_, gpsRefLon_, 0); 101 | 102 | // create a NavSatFix to publish 103 | refFix_.header = navSatFix->header; 104 | refFix_.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; 105 | refFix_.status.status = sensor_msgs::NavSatStatus::SERVICE_GPS; 106 | refFix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; 107 | refFix_.latitude = gpsRefLat_; 108 | refFix_.longitude = gpsRefLon_; 109 | refFix_.altitude = refPressureHeight_; 110 | } else { 111 | // initialize using first message received 112 | refPoint_ = GeographicLib::LocalCartesian(lat, lon, 0); 113 | refFix_ = *navSatFix; 114 | 115 | ROS_INFO("Initialized reference point to %.7f, %.7f", 116 | lat, lon); 117 | } 118 | 119 | refInitialized_ = true; 120 | } 121 | 122 | double locX, locY, locZ; 123 | refPoint_.Forward(lat, lon, hMSL, locX, locY, locZ); 124 | 125 | // determine magnetic declination 126 | double bEast, bNorth, bUp; 127 | magneticModel_->operator()(tYears, lat, lon, hWGS84, bEast, bNorth, bUp); 128 | currentDeclination_ = -std::atan2(bEast, bNorth); 129 | const double degDec = currentDeclination_ * 180 / M_PI; 130 | ROS_INFO_ONCE_NAMED("gps_odom_mag_dec", 131 | "Magnetic declination set to %f degrees", degDec); 132 | 133 | // calculate corrected yaw angle 134 | // matrix composition is of the form wRb = Rz * Ry * Rx 135 | Eigen::AngleAxisd aa; 136 | aa.angle() = currentDeclination_; 137 | aa.axis() = Eigen::Vector3d(0.0, 0.0, 1.0); 138 | 139 | Eigen::Quaterniond wQb = 140 | Eigen::Quaterniond(imu->orientation.w, imu->orientation.x, 141 | imu->orientation.y, imu->orientation.z); 142 | wQb = aa * wQb; 143 | 144 | nav_msgs::Odometry odometry; 145 | odometry.header.stamp = navSatFix->header.stamp; 146 | odometry.header.frame_id = fixedFrame_; 147 | odometry.child_frame_id = fixedFrame_; 148 | odometry.pose.pose.orientation.w = wQb.w(); 149 | odometry.pose.pose.orientation.x = wQb.x(); 150 | odometry.pose.pose.orientation.y = wQb.y(); 151 | odometry.pose.pose.orientation.z = wQb.z(); 152 | odometry.pose.pose.position.x = locX; 153 | odometry.pose.pose.position.y = locY; 154 | odometry.pose.pose.position.z = height->height - refPressureHeight_; 155 | 156 | // generate covariance (6x6 with order: x,y,z,rot_x,rot_y,rot_z) 157 | Eigen::Matrix poseCovariance; 158 | poseCovariance.setZero(); 159 | for (int i = 0; i < 3; i++) { 160 | for (int j = 0; j < 3; j++) { 161 | poseCovariance(i, j) = 162 | navSatFix->position_covariance[i * 3 + j] * gpsCovScaleFactor_; 163 | } 164 | } 165 | // replace covariance w/ number from altimeter 166 | poseCovariance(2, 2) = height->variance; 167 | 168 | // orientation: copy from IMU 169 | for (int i = 0; i < 3; i++) { 170 | for (int j = 0; j < 3; j++) { 171 | poseCovariance(3 + i, 3 + j) = imu->orientation_covariance[i * 3 + j]; 172 | } 173 | } 174 | 175 | // linear velocity from GPS, no angular velocity 176 | odometry.twist.twist.linear = navSatTwist->twist.twist.linear; 177 | odometry.twist.twist.angular.x = 0; 178 | odometry.twist.twist.angular.y = 0; 179 | odometry.twist.twist.angular.z = 0; 180 | 181 | // velocity covariance [x,y,z,rot_x,rot_y,rot_z] 182 | // linear from GPS, no angular 183 | Eigen::Matrix velCovariance; 184 | velCovariance.setZero(); 185 | for (int i = 0; i < 3; i++) { 186 | for (int j = 0; j < 3; j++) { 187 | velCovariance(i, j) = 188 | navSatTwist->twist.covariance[(i * 6) + j] * gpsCovScaleFactor_; 189 | velCovariance(i + 3, j + 3) = -1; // unsupported 190 | } 191 | } 192 | 193 | // copy covariance to output 194 | for (int i = 0; i < 6; i++) { 195 | for (int j = 0; j < 6; j++) { 196 | odometry.pose.covariance[i * 6 + j] = poseCovariance(i, j); 197 | odometry.twist.covariance[i * 6 + j] = velCovariance(i, j); 198 | } 199 | } 200 | pubOdometry_.publish(odometry); 201 | 202 | // publish reference point 203 | if (!refPublished_) { 204 | // publish only once 205 | refFix_.header.seq = 0; 206 | pubRefPoint_.publish(refFix_); 207 | refPublished_ = true; 208 | } 209 | 210 | // publish tf stuff and trajectory visualizer 211 | if (shouldPublishTf_) { 212 | tfPub_.set_child_frame_id(imu->header.frame_id); 213 | tfPub_.PublishTransform(odometry.pose.pose, odometry.header); 214 | } 215 | trajViz_.PublishTrajectory(odometry.pose.pose.position, odometry.header); 216 | covViz_.PublishCovariance(odometry); 217 | } 218 | 219 | bool Node::loadModels() { 220 | if (!geoid_) { 221 | try { 222 | geoid_ = std::make_shared("egm84-15", 223 | pkgPath_ + "/geoids"); 224 | } 225 | catch (GeographicLib::GeographicErr &e) { 226 | ROS_ERROR("Failed to load geoid. Reason: %s", e.what()); 227 | return false; 228 | } 229 | } 230 | if (!magneticModel_) { 231 | try { 232 | magneticModel_ = std::make_shared( 233 | "wmm2010", pkgPath_ + "/magnetic_models"); 234 | } 235 | catch (GeographicLib::GeographicErr &e) { 236 | ROS_ERROR("Failed to load model. Reason: %s", e.what()); 237 | return false; 238 | } 239 | } 240 | return true; 241 | } 242 | 243 | } // namespace gps_odom 244 | -------------------------------------------------------------------------------- /gps_odom/tools/preview.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gps_odom preview 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 |
22 | 23 | 24 |
25 | 26 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /gps_odom/tools/red_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/tools/red_circle.png -------------------------------------------------------------------------------- /gps_odom/tools/red_circle@2x.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/gps-tools/1ce2c590e0e0c48d0899fd84278772b25360385f/gps_odom/tools/red_circle@2x.png -------------------------------------------------------------------------------- /pressure_altimeter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pressure_altimeter) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | roscpp 7 | sensor_msgs 8 | std_msgs 9 | ) 10 | 11 | # generate messages 12 | add_message_files(FILES Height.msg) 13 | generate_messages(DEPENDENCIES std_msgs) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | ) 18 | 19 | include_directories( 20 | ${catkin_INCLUDE_DIRS} 21 | include 22 | ) 23 | 24 | add_definitions("-Wall -Werror -std=c++11") 25 | add_executable(${PROJECT_NAME} 26 | src/main.cpp 27 | src/node.cpp 28 | ) 29 | 30 | target_link_libraries(${PROJECT_NAME} 31 | ${catkin_LIBRARIES} 32 | ) 33 | 34 | add_dependencies(${PROJECT_NAME} 35 | ${catkin_EXPORTED_TARGETS} 36 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 37 | ) 38 | -------------------------------------------------------------------------------- /pressure_altimeter/include/pressure_altimeter/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.hpp 3 | * 4 | * This file is part of pressure_altimeter. 5 | * 6 | * Created on: 24/08/2014 7 | */ 8 | 9 | #ifndef GALT_PRESSURE_ALTIMETER_NODE_HPP_ 10 | #define GALT_PRESSURE_ALTIMETER_NODE_HPP_ 11 | 12 | #include 13 | #include 14 | 15 | namespace galt { 16 | namespace pressure_altimeter { 17 | 18 | /** 19 | * @brief Converts fluid pressure message into height above sea level. 20 | */ 21 | class Node { 22 | public: 23 | /** 24 | * @brief Load parameters from ROS param and subscribe to fluid pressure. 25 | */ 26 | Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh); 27 | 28 | private: 29 | /** 30 | * @brief Receive pressure message and convert to height. 31 | * @param pressure 32 | */ 33 | void pressureCallback(const sensor_msgs::FluidPressureConstPtr &pressure); 34 | 35 | ros::NodeHandle nh_, pnh_; 36 | ros::Subscriber subPressure_; 37 | ros::Publisher pubHeight_; 38 | std::string worldFrameId_; 39 | 40 | // Physical constants (refer to Wikipedia for significance) 41 | constexpr static double kP0 = 101325; // Pressure at sea level (Pa) 42 | constexpr static double kT0 = 288.15; // Temperature at sea level (K) 43 | constexpr static double kL = 0.0065; // Temperature lapse rate (K/m) 44 | constexpr static double kG = 9.80665; // Gravitational acceleration (m/s^2) 45 | constexpr static double kM = 0.0289644; // Molar mass of dry air (kg / mol) 46 | constexpr static double kR = 8.31447; // Universal gas constant J/(mol * K) 47 | }; 48 | 49 | } // pressure_altimeter 50 | } // galt 51 | 52 | #endif // GALT_PRESSURE_ALTIMETER_NODE_HPP_ 53 | -------------------------------------------------------------------------------- /pressure_altimeter/launch/pressure_altimeter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /pressure_altimeter/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /pressure_altimeter/msg/Height.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Height above sea level in meters 4 | float64 height 5 | 6 | # Variance in height estimate, m^2 7 | float64 variance -------------------------------------------------------------------------------- /pressure_altimeter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pressure_altimeter 4 | 0.0.1 5 | Convert fluid pressure into altitude 6 | 7 | gareth 8 | BSD 9 | 10 | catkin 11 | message_generation 12 | roscpp 13 | sensor_msgs 14 | std_msgs 15 | message_runtime 16 | roscpp 17 | sensor_msgs 18 | std_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /pressure_altimeter/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * main.cpp 3 | * 4 | * This file is part of pressure_altimeter. 5 | * 6 | * Created on: 24/08/2014 7 | */ 8 | 9 | #include 10 | 11 | int main(int argc, char** argv) { 12 | ros::init(argc, argv, "pressure_altimeter"); 13 | ros::NodeHandle nh, pnh("~"); 14 | try { 15 | galt::pressure_altimeter::Node node(nh, pnh); 16 | ros::spin(); 17 | } 18 | catch (const std::exception& e) { 19 | ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what()); 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /pressure_altimeter/src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * node.cpp 3 | * 4 | * This file is part of pressure_altimeter. 5 | * 6 | * Created on: 24/08/2014 7 | */ 8 | 9 | #include 10 | #include // published message 11 | #include 12 | 13 | namespace galt { 14 | namespace pressure_altimeter { 15 | 16 | Node::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) 17 | : nh_(nh), pnh_(pnh) { 18 | subPressure_ = 19 | pnh_.subscribe("pressure", 1, &Node::pressureCallback, this); 20 | pubHeight_ = pnh_.advertise<::pressure_altimeter::Height>("height", 1); 21 | 22 | pnh_.param("fixed_frame", worldFrameId_, std::string("world")); 23 | } 24 | 25 | void Node::pressureCallback( 26 | const sensor_msgs::FluidPressureConstPtr &pressure) { 27 | ::pressure_altimeter::Height height_msg; 28 | height_msg.header.stamp = pressure->header.stamp; 29 | height_msg.header.frame_id = worldFrameId_; 30 | 31 | const double pressurePA = pressure->fluid_pressure * 100; // mb to Pa 32 | if (pressurePA > 0) { 33 | // calculate height from barometer 34 | const double c = kG * kM / (kR * kL); 35 | const double lhs = std::exp(std::log(pressurePA / kP0) * (1 / c)); 36 | const double h = (1 - lhs) * kT0 / kL; 37 | 38 | // value of jacobian to propagate variance 39 | const double J = -lhs * kT0 / (kL * c * pressurePA); 40 | const double h_var = J * J * (pressure->variance * 100 * 100); // mb to Pa 41 | 42 | height_msg.height = h; 43 | height_msg.variance = h_var; 44 | } else { 45 | height_msg.height = 0; 46 | height_msg.variance = -1; 47 | } 48 | 49 | pubHeight_.publish(height_msg); 50 | } 51 | 52 | } // pressure_altimeter 53 | } // galt 54 | --------------------------------------------------------------------------------