├── .gitignore ├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── include └── kr_attitude_eskf │ ├── AttitudeESKF.hpp │ ├── AttitudeMagCalib.hpp │ └── Node.hpp ├── launch ├── attitude_eskf.launch └── test.launch ├── package.xml └── src ├── AttitudeESKF.cpp ├── AttitudeMagCalib.cpp ├── Node.cpp └── kr_attitude_eskf.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 | # CMAKE TEMPORARY FILES 14 | CMakeCache.txt 15 | CMakeFiles/ 16 | CMakeFiles/* 17 | 18 | # BUILD FOLDERS 19 | bin/ 20 | bin/* 21 | build/ 22 | build/* 23 | 24 | # ROS GENERATED PRODUCTS 25 | msg_gen/ 26 | msg_gen/* 27 | srv_gen/ 28 | srv_gen/* 29 | 30 | # ROS GENERATED PYTHON 31 | _*.py 32 | _*.pyc 33 | 34 | # ROS BAG FILES 35 | *.bag 36 | 37 | # XCODE BUILD PRODUCTS 38 | *.build/ 39 | *.build/* 40 | 41 | # XCODE USER SETTINGS 42 | *.xcuserdatad/ 43 | *.xcuserdatad/* 44 | *.pbxuser 45 | *.pbxuser/* 46 | 47 | # QTCREATOR PROJECT 48 | *.includes 49 | *.creator 50 | *.creator.user 51 | *.config 52 | *.files 53 | 54 | # GEANY PROJECTS 55 | *.geany 56 | 57 | # JETBRAINS 58 | *.idea 59 | 60 | # ZEND/ECLIPSE 61 | *.buildpath 62 | *.settings 63 | *.project 64 | *.pydevproject 65 | 66 | # MACOSX SETTINGS 67 | *.DS_Store 68 | 69 | # GENERATED DOCUMENTATION 70 | html_docs/ 71 | html_docs/* 72 | 73 | # ANDROID SDK GENERATED JAVA 74 | gen/ 75 | gen/* 76 | 77 | # COMPILED DYNAMIC LIBRARIES 78 | *.so 79 | *.dylib 80 | *.dll 81 | 82 | # COMPILED STATIC LIBRARIES 83 | *.lai 84 | *.la 85 | *.a 86 | *.lib 87 | 88 | # EXECUTABLES 89 | *.exe 90 | *.out 91 | *.app 92 | 93 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kr_attitude_eskf) 3 | find_package(catkin REQUIRED COMPONENTS 4 | roscpp 5 | geometry_msgs 6 | sensor_msgs 7 | message_filters 8 | message_generation 9 | eigen_conversions 10 | diagnostic_updater 11 | ) 12 | 13 | find_package(cmake_modules) 14 | find_package(Eigen REQUIRED) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS message_runtime 18 | ) 19 | 20 | # include boost 21 | find_package(Boost REQUIRED) 22 | link_directories(${Boost_LIBRARY_DIR}) 23 | include_directories(${Boost_INCLUDE_DIR}) 24 | 25 | # include headers 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ${Eigen_INCLUDE_DIRS} 29 | include 30 | include/kr_attitude_eskf 31 | ) 32 | 33 | # build shared library 34 | add_library(krAttitudeESKF 35 | src/AttitudeESKF.cpp 36 | src/AttitudeMagCalib.cpp 37 | ) 38 | 39 | set(RELEVANT_LIBRARIES 40 | krAttitudeESKF 41 | ${catkin_LIBRARIES} 42 | ) 43 | 44 | # rigorous error checking 45 | add_definitions("-Wall -Werror -DATTITUDE_ESKF_BUILD_MAG -O3 -std=c++11") 46 | 47 | # build main filter 48 | add_executable(${PROJECT_NAME} 49 | src/kr_attitude_eskf.cpp 50 | src/Node.cpp 51 | ) 52 | target_link_libraries(${PROJECT_NAME} ${RELEVANT_LIBRARIES}) 53 | 54 | # ensures messages are generated before hand 55 | add_dependencies(${PROJECT_NAME} 56 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 57 | ${catkin_EXPORTED_TARGETS} 58 | ) 59 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyrigh 2014 Gareth Cross 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kr_attitude_eskf 2 | 3 | kr_attitude_eskf is an implementation of the Error-State Kalman Filter described in: 4 | 5 | * “Attitude Error Representations for Kalman Filtering” - F. Landis Markley 6 | 7 | ## Version History 8 | 9 | * **0.0.8**: 10 | - Removed the reference calib option, which produced somewhat poor results anyways. 11 | - Mag calib code now uses either float or double. 12 | - Removed kr_math, eigen_conversions dependencies. 13 | * **0.0.7**: 14 | - AttitudeESKF accepts matrices for covariance parameters, instead of assuming diagonal noise. 15 | - Noise parameters are not provided by `rosparam` anymore, and are expected to arrive in the incoming messages. 16 | - Process model treats gyro covariance with the correct units (rad/s)^2 instead of (rad)^2. 17 | - Magnetic bias/scale/reference vectors are loaded as lists instead of key-value arrays. 18 | - Magnetometer calibration now uses least squares for initial guess, and LM iteration is robustified with Cauchy weighting. 19 | - Calibration is triggered by a callable ROS service, instead of from the launch file options. 20 | - Added support for `diagnostic_updater`. 21 | - Renamed `adjusted_field` topic to `corrected_field`. 22 | - Refactored ROS code into a Node class. 23 | - Removed `broadcast_frame` option and `tf`-dependent code. 24 | - Pose is always published. 25 | * **0.0.6**: 26 | - Added `publish_pose` option. 27 | * **0.0.5**: 28 | - First public release. 29 | 30 | ## Description 31 | 32 | This implementation utilizes accelerometer, gyroscope, and (optionally) the magnetometer in order to provide an attitude estimate. Gyroscope biases are estimated online with a moving average filter. 33 | 34 | A 4th order Runge Kutta process model is employed, along with a classical EKF update - save for the error-state modifications. Internally, the filter uses a quaternion parameterization of SO(3), thereby avoiding the uglier qualities of euler angles. 35 | 36 | ## Dependencies 37 | 38 | The core ESKF code depends on Eigen. The ROS node + magnetometer calibrator depends on [kr_math](https://github.com/KumarRobotics/kr_math) also. 39 | 40 | ## Parameters 41 | 42 | The ROS implementation exposes several parameters: 43 | 44 | |Parameter|Definition|Default| 45 | |---|---|---| 46 | |`enable_magnetometer`|If true, magnetometer readings are included in the state estimation update.|`false`| 47 | |`mag_calib/bias`|Bias of the magnetometer.|`[0,0,0]`| 48 | |`mag_calib/scale`|Scale of the magnetometer.|`[1,1,1]`| 49 | |`mag_calib/reference`|World frame reference vector of the magnetometer.|`[0,0,0]`| 50 | |`process_scale_factor`|'Fudge factor' to multiply by incoming gyro covariances.|1| 51 | |`gyro_bias_thresh`|Threshold of motion below which we may estimate gyro bias.|0.01 rad/s| 52 | 53 | When using the node, you should remap `~imu` and `~field` to the appropriate topics. See `attitude_eskf.launch` for an example. 54 | 55 | ## Topics 56 | 57 | The following topics are always published: 58 | 59 | |Name|Description| 60 | |---|---| 61 | |`filtered_imu`|Filter orientation, covariance, IMU acceleration, and angular velocity (with bias subtracted).| 62 | |`bias`|Current estimate of gyroscope bias.| 63 | |`status`|Status of filter. See `Status.msg` for details.| 64 | |`pose`|Stamped pose, with translation component zeroed.| 65 | 66 | If `enable_magnetometer` is set, the following topics will also be published: 67 | 68 | |Name|Description| 69 | |---|---| 70 | |`corrected_field`|Magnetic field, with bias and scale estimates applied.| 71 | 72 | ## Calibration 73 | 74 | This node includes a built-in magnetometer calibration mode. Calibration is started by calling the `begin_calibration` service. You will observe a log message on `rosout` indicating that calibration has started. Steps: 75 | 76 | 1. Rotate the device about the world z axis 360 degrees. 77 | 2. Rotate the device 90 degrees about the roll/pitch axis, and then again 360 degrees about the world z axis. 78 | 3. You should see the resulting parameters printed on `rosout`. They will also be saved to `rosparam`. 79 | 4. After calibration, `enable_magnetometer` will be set to true and the magnetic field will be included in the estimate. 80 | 81 | ## Important Operating Notes 82 | 83 | * It is assumed that all IMU topics are **strictly synchronized**. 84 | * It is assumed that all IMU sensors are **physically aligned**. 85 | * The default value of `mag_calib/reference` is **not** valid. You must either calibrate the device, or set a known value using a magnetic model (such as [IGRF11](http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html)). 86 | 87 | ## Bug? 88 | 89 | Please report issues to the official package maintainer. 90 | 91 | ## Links 92 | 93 | * [Early version running on iOS](http://www.youtube.com/watch?v=ijK2ndEGBXA) 94 | * Avik De has used this filter on STM32 w/ MPU6000: [Details](http://avikde.me/IMU-Filtering-STM32-MPU6000/) 95 | * [Video of a pico-quadrotor using ESKF](http://www.youtube.com/watch?v=HpO8DpeYC2k&spfreload=10) 96 | 97 | -------------------------------------------------------------------------------- /include/kr_attitude_eskf/AttitudeESKF.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AttitudeESKF.hpp 3 | * 4 | * Copyright (c) 2013 Gareth Cross. Apache 2 License. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 12/24/2013 9 | * Author: gareth 10 | */ 11 | 12 | #ifndef KR_ATTITUDE_ESKF_H_ 13 | #define KR_ATTITUDE_ESKF_H_ 14 | 15 | #include 16 | #include 17 | 18 | namespace kr { 19 | 20 | /** 21 | * @class AttitudeESKF 22 | * @brief Implementation of an error-state EKF for attitude determination using 23 | * quaternions. 24 | * @note Gravity and magnetic field are the supported reference vectors. 25 | * @see 'Attitude Error Representations for Kalman Filtering', F. Landis 26 | * Markley, JPL 27 | */ 28 | class AttitudeESKF { 29 | public: 30 | typedef double scalar_t; /**< Type used for all calculations, change as 31 | performance requires */ 32 | 33 | typedef Eigen::Matrix vec3; /// Vector in R3 34 | typedef Eigen::Matrix mat3; /// Matrix in R3 35 | typedef Eigen::Quaternion quat; /// Member of S4 36 | 37 | static constexpr scalar_t kOneG = 9.80665; /// Earth gravity 38 | 39 | /** 40 | * @brief Ctor, initializes state to all zeros. 41 | */ 42 | AttitudeESKF(); 43 | 44 | /** 45 | * @brief predict Perform the prediction step. 46 | * @param wg Uncorrected gyroscope readings in body frame. 47 | * @param dt Time step in seconds. 48 | * @param cov Covariance on gyro measurement, units of (rad/s)^2. 49 | * @param useRK4 If true, use RK4 integration - otherwise euler is used. 50 | */ 51 | void predict(const vec3 &wg, scalar_t dt, 52 | const mat3& cov, bool useRK4=false); 53 | 54 | /** 55 | * @brief update Perform the update step. 56 | * @param ab Accelerometer reading in body frame (units of m/s^2). 57 | * @param aCov Covariance on accelerometer measurement, units of (m/s^2)^2. 58 | * @param mb Measured magnetic field in body frame (units of gauss) 59 | * @param mCov Covariance on magnetometer measurement, units of gauss^2. 60 | * 61 | * @note Magnetometer elements only used if usesMagnetometer is set to true. 62 | */ 63 | void update(const vec3 &ab, 64 | const mat3 &aCov, 65 | const vec3 &mb = vec3::Zero(), 66 | const mat3 &mCov = mat3::Zero()); 67 | 68 | /** 69 | * @brief externalYawUpdate Update the z-axis rotation with an external 70 | * value. 71 | * @param yaw Yaw angle as the measurement. 72 | * @param alpha Interpolation fraction between current and external value. 73 | * 74 | * @note This method was added for use in Vicon, where only one external 75 | * angle is used. A complementary filter action is used for this update. 76 | * 77 | * The update only takes place if the dip angle is ~ below 30 degrees. 78 | */ 79 | void externalYawUpdate(scalar_t yaw, scalar_t alpha = 0.5); 80 | 81 | /** 82 | * @brief initialize Initialize the pose. 83 | * 84 | * @param ab Measured body acceleration (units of m/s^2). 85 | * @param aCov Diagonal uncertainty on acceleration, units of (m/s^2)^2. 86 | * @param mb Measured magnetic field in body frame (units of gauss). 87 | * @param mCov Diagonal uncertainty on magnetic field, units of gauss^2. 88 | * 89 | * @note Uses non-linear least squares to formulate initial rotation vector. 90 | * The inverse covariance matrices are used to weight the input vectors. 91 | * 92 | * @note If magnetometer is disabled, a roll and pitch angles are determined 93 | * from the gravity vector. The yaw angle is zeroed. 94 | * 95 | * @return True on success, false on failure. Failure will occur if aCov or 96 | * mCov are not invertible (and the magnetometer is enabled). 97 | */ 98 | bool initialize(const vec3 &ab, 99 | const vec3 &aCov, 100 | const vec3 &mb = vec3::Zero(), 101 | const vec3 &mCov = vec3::Zero()); 102 | 103 | /** 104 | * @brief setEstimatesBias Enable/Disable bias estimation. 105 | * @param estBias If true, bias is estimated online. Default false. 106 | * @note Biases are estimated with a moving average filter when the platform 107 | * is not in motion. 108 | */ 109 | void setEstimatesBias(bool estBias) { estBias_ = estBias; } 110 | 111 | /** 112 | * @brief setGyroBiasThreshold Set the gyro bias threshold. 113 | * @param thresh Threshold, units of rad/s. 114 | * @note Below this threshold of rotation, gyro biases are estimated. 115 | */ 116 | void setGyroBiasThreshold(scalar_t thresh) { biasThresh_ = thresh; } 117 | 118 | /** 119 | * @brief setIgnoresZUpdate Enable/disable zeroing the z-axis update. 120 | * @param ignoreZ If true, updates about the local z-axis are ignored. 121 | */ 122 | void setIgnoresZUpdate(bool ignoreZ) { ignoreZ_ = ignoreZ; } 123 | 124 | /** 125 | * @brief setUsesMagnetometer Enable/disable magnetometer update. 126 | * @param useMag If true, mag update is enabled. Default false. 127 | */ 128 | void setUsesMagnetometer(bool useMag) { useMag_ = useMag; } 129 | 130 | /** 131 | * @brief setMagneticReference Set the magnetic reference vector. 132 | * @param magRef Vector determining the North magnetic field. 133 | */ 134 | void setMagneticReference(const vec3 &magRef) { magRef_ = magRef; } 135 | 136 | /** 137 | * @brief getQuat Get the state as a quaternion. 138 | * @return Instance of kr::quat. 139 | */ 140 | const quat &getQuat() const { return q_; } 141 | 142 | /** 143 | * @brief getAngularVelocity Get angular velocity (corrected for bias). 144 | * @return Angular velocity in rad/s. 145 | */ 146 | const vec3 &getAngularVelocity() const { return w_; } 147 | 148 | /** 149 | * @brief getGyroBias Get the current gyro bias estimate. 150 | * @return Gyro bias in units of rad/s. 151 | */ 152 | const vec3 &getGyroBias() const { return b_; } 153 | 154 | /** 155 | * @brief getCovariance Get the system covariance on the error state. 156 | * @return 3x3 covariance matrix. 157 | */ 158 | const mat3 &getCovariance() const { return P_; } 159 | mat3 &getCovariance() { return P_; } 160 | 161 | /** 162 | * @brief getPredictedField Get the predicted magnetic field. 163 | * @return The predicted magnetic field for the current state, units of gauss. 164 | */ 165 | const vec3 &getPredictedField() const { return predMag_; } 166 | 167 | /** 168 | * @brief getCorrection Get the last correction (error state) generated. 169 | * @return The previous error state. 170 | */ 171 | const vec3 &getCorrection() const { return dx_; } 172 | 173 | /** 174 | * @brief isStable Determine if the filter is stable. 175 | * @return True if the Kalman gain was non-singular at the last update. 176 | */ 177 | bool isStable() const { return isStable_; } 178 | 179 | /** 180 | * @brief getRPY Utility function, get roll-pitch-yaw. 181 | * @param R 3x3 rotation matrix (body to world). 182 | * @return Rotations about the x-y-z axes. 183 | */ 184 | static vec3 getRPY(const mat3& R); 185 | 186 | private: 187 | 188 | quat q_; /// Orientation 189 | mat3 P_; /// System covariance 190 | 191 | vec3 w_; 192 | vec3 b_; 193 | unsigned long steadyCount_; 194 | scalar_t biasThresh_; 195 | 196 | vec3 magRef_; 197 | vec3 predMag_; 198 | 199 | vec3 dx_; 200 | 201 | bool isStable_; 202 | bool estBias_; 203 | bool ignoreZ_; 204 | bool useMag_; 205 | }; 206 | 207 | } // namespace kr 208 | 209 | #endif /* defined(KR_ATTITUDE_ESKF_H_) */ 210 | -------------------------------------------------------------------------------- /include/kr_attitude_eskf/AttitudeMagCalib.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AttitudeMagCalib.hpp 3 | * 4 | * Copyright (c) 2013 Gareth Cross. All rights reserved. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 23/06/2014 9 | * Author: gareth 10 | */ 11 | 12 | #ifndef KR_ATTITUDE_MSG_CALIB_H_ 13 | #define KR_ATTITUDE_MSG_CALIB_H_ 14 | 15 | #include 16 | #include 17 | #include "kr_attitude_eskf/AttitudeESKF.hpp" 18 | 19 | namespace kr { 20 | 21 | /** 22 | * @class AttitudeMagCalib 23 | * @brief Tool for calibrating the system magnetometer. 24 | */ 25 | class AttitudeMagCalib { 26 | public: 27 | /// Types 28 | typedef kr::AttitudeESKF::scalar_t scalar_t; 29 | typedef kr::AttitudeESKF::vec3 vec3; 30 | typedef kr::AttitudeESKF::mat3 mat3; 31 | typedef kr::AttitudeESKF::quat quat; 32 | 33 | /** 34 | * @brief Types of calibration that can be performed. 35 | */ 36 | enum CalibrationType : int { 37 | FullCalibration = 0, /// Calculate bias and scale 38 | }; 39 | 40 | /** 41 | * @brief singular_hessian Thrown when non-linear least squares fails because the hessian cannot be inverted. 42 | */ 43 | class singular_hessian : public std::exception {}; 44 | 45 | /** 46 | * @brief insufficient_data Thrown if calibration is attempted with an insufficient number of points. 47 | */ 48 | class insufficient_data : public std::exception {}; 49 | 50 | /** 51 | * @brief Ctor, initializes to reset state. 52 | */ 53 | AttitudeMagCalib(); 54 | 55 | /** 56 | * @brief reset Erase collected points and reset the calibration state. 57 | */ 58 | void reset(); 59 | 60 | /** 61 | * @brief appendSample Append an attitude estimate and field measurement. 62 | * @param att Current attitude at time of measurement. 63 | * @param field Current magnetic field. 64 | */ 65 | void appendSample(const quat &att, const vec3 &field); 66 | 67 | /** 68 | * @brief isReady Is the system ready to calibrate? 69 | * @return True if enough points have been collected. 70 | */ 71 | bool isReady() const; 72 | 73 | /** 74 | * @brief isCalibrated Are the calibration values set? 75 | * @return True if calibrate() ran succesfully and the system has not been reset. 76 | */ 77 | bool isCalibrated() const; 78 | 79 | /** 80 | * @brief Run calibration on data recorded. 81 | * @throws insufficient_data, singular_hessian 82 | * 83 | * @note This method will optimize the scale and bias parameters using 84 | * a two step process: 85 | * 1) Least-squares fit of a sphere as an initial guess. 86 | * 2) LM fit of an axis-aligned spheroid as a refined estimate. 87 | */ 88 | void calibrate(AttitudeMagCalib::CalibrationType type = FullCalibration); 89 | 90 | /** 91 | * @brief getBias Get the estimate of magnetometer bias. 92 | * @return Vector in R3, units of gauss. 93 | */ 94 | const vec3& getBias() const { return bias_; } 95 | 96 | /** 97 | * @brief getScale Get the estimate of magnetometer scale. 98 | * @return Vector in R3, unitless. 99 | */ 100 | const vec3& getScale() const { return scale_; } 101 | 102 | private: 103 | constexpr static int kBinMaxCount = 40; /// Number of samples to take on each axis 104 | 105 | struct SampleBin { 106 | vec3 field; /// Field measured in this sample 107 | quat q; /// Unreferenced quaternion for this sample 108 | }; 109 | 110 | std::map binV_; 111 | std::map binH_; 112 | bool calibrated_; 113 | 114 | vec3 bias_; 115 | vec3 scale_; 116 | }; 117 | 118 | } // namespace kr 119 | 120 | #endif // KR_ATTITUDE_MSG_CALIB_H_ 121 | -------------------------------------------------------------------------------- /include/kr_attitude_eskf/Node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Node.hpp 3 | * 4 | * Copyright (c) 2014 Gareth Cross. All rights reserved. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 31/08/2014 9 | * Author: gareth 10 | */ 11 | 12 | #ifndef KR_ATTITUDE_NODE_HPP_ 13 | #define KR_ATTITUDE_NODE_HPP_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace kr_attitude_eskf { 29 | 30 | class Node { 31 | public: 32 | Node(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 33 | 34 | // save calibration to rosparam 35 | void saveCalibration(); 36 | 37 | private: 38 | typedef kr::AttitudeESKF::vec3 vec3; 39 | 40 | static constexpr unsigned int kROSQueueSize = 100; 41 | 42 | ros::NodeHandle nh_; 43 | ros::Publisher pubImu_; 44 | ros::Publisher pubBias_; 45 | ros::Publisher pubPose_; 46 | ros::Publisher pubField_; 47 | ros::ServiceServer srvCalibrate_; 48 | 49 | // subsribers 50 | message_filters::Subscriber subImu_; 51 | message_filters::Subscriber subField_; 52 | ros::Subscriber subImuUnsync_; 53 | 54 | // time sync 55 | typedef message_filters::sync_policies::ExactTime TimeSyncPolicy; 57 | message_filters::Synchronizer sync_; 58 | 59 | // diagnostic updater 60 | diagnostic_updater::Updater updater_; 61 | 62 | // options 63 | bool enableMag_; 64 | double gyroBiasThresh_; 65 | double processScaleFactor_; 66 | 67 | // implementation 68 | vec3 magBias_; 69 | vec3 magScale_; 70 | vec3 magReference_; 71 | kr::AttitudeESKF eskf_; 72 | kr::AttitudeMagCalib calib_; 73 | ros::Time prevStamp_; 74 | enum { 75 | Uncalibrated = 0, 76 | Calibrating = 1, 77 | CalibrationComplete = 2, 78 | } calibState_; 79 | bool init_; 80 | 81 | // callbacks 82 | void inputCallback(const sensor_msgs::ImuConstPtr&, 83 | const sensor_msgs::MagneticFieldConstPtr&); 84 | 85 | bool beginCalibration(std_srvs::Empty::Request&, 86 | std_srvs::Empty::Response&); 87 | 88 | void diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper &stat); 89 | }; 90 | 91 | } // namespace kr_attitude_eskf 92 | 93 | #endif // KR_ATTITUDE_NODE_HPP_ 94 | -------------------------------------------------------------------------------- /launch/attitude_eskf.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 | -------------------------------------------------------------------------------- /launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kr_attitude_eskf 4 | 0.0.8 5 | Error state kalman filter for attitude determination 6 | 7 | gcross 8 | Gareth Cross 9 | Apache 2 10 | 11 | catkin 12 | 13 | geometry_msgs 14 | roscpp 15 | sensor_msgs 16 | galt_common 17 | message_filters 18 | message_generation 19 | eigen_conversions 20 | diagnostic_updater 21 | 22 | geometry_msgs 23 | roscpp 24 | sensor_msgs 25 | galt_common 26 | message_filters 27 | message_runtime 28 | eigen_conversions 29 | diagnostic_updater 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/AttitudeESKF.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AttitudeESKF.cpp 3 | * 4 | * Copyright (c) 2013 Gareth Cross. Apache 2 License. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 12/24/2013 9 | * Author: gareth 10 | */ 11 | 12 | #ifndef NDEBUG 13 | #define NDEBUG 14 | #endif 15 | 16 | #include "AttitudeESKF.hpp" 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace Eigen; 23 | 24 | namespace kr { 25 | 26 | // skew symmetric matrix 27 | template 28 | static inline Matrix crossSkew(const Matrix &w) { 29 | Matrix W; 30 | 31 | W(0, 0) = 0; 32 | W(0, 1) = -w(2); 33 | W(0, 2) = w(1); 34 | 35 | W(1, 0) = w(2); 36 | W(1, 1) = 0; 37 | W(1, 2) = -w(0); 38 | 39 | W(2, 0) = -w(1); 40 | W(2, 1) = w(0); 41 | W(2, 2) = 0; 42 | 43 | return W; 44 | } 45 | 46 | // hardcoded 3x3 invert (unchecked) 47 | template 48 | static inline Matrix invert(const Matrix &A, T det) { 49 | Matrix C; 50 | det = 1 / det; 51 | 52 | C(0, 0) = (-A(2, 1) * A(1, 2) + A(1, 1) * A(2, 2)) * det; 53 | C(0, 1) = (-A(0, 1) * A(2, 2) + A(0, 2) * A(2, 1)) * det; 54 | C(0, 2) = (A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1)) * det; 55 | 56 | C(1, 0) = (A(2, 0) * A(1, 2) - A(1, 0) * A(2, 2)) * det; 57 | C(1, 1) = (-A(2, 0) * A(0, 2) + A(0, 0) * A(2, 2)) * det; 58 | C(1, 2) = (A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2)) * det; 59 | 60 | C(2, 0) = (-A(2, 0) * A(1, 1) + A(1, 0) * A(2, 1)) * det; 61 | C(2, 1) = (A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1)) * det; 62 | C(2, 2) = (-A(1, 0) * A(0, 1) + A(0, 0) * A(1, 1)) * det; 63 | 64 | return C; 65 | } 66 | 67 | // hardcoded determinant 68 | template static inline T determinant(const Matrix &A) { 69 | return A(0, 0) * (A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1)) - 70 | A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) + 71 | A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0)); 72 | } 73 | 74 | // Eigen does not define these operators, which we use for integration 75 | template 76 | static inline Eigen::Quaternion operator + (const Eigen::Quaternion& a, 77 | const Eigen::Quaternion& b) { 78 | return Eigen::Quaternion(a.w()+b.w(), 79 | a.x()+b.x(), 80 | a.y()+b.y(), 81 | a.z()+b.z()); 82 | } 83 | 84 | template 85 | static inline Eigen::Quaternion operator * (const Eigen::Quaternion& q, 86 | Scalar s) { 87 | return Eigen::Quaternion(q.w() * s, 88 | q.x() * s, 89 | q.y() * s, 90 | q.z() * s); 91 | } 92 | 93 | /** 94 | * @brief Integrate a rotation quaterion using Euler integration 95 | * @param q Quaternion to integrate 96 | * @param w Angular velocity (body frame), stored in 3 complex terms 97 | * @param dt Time interval in seconds 98 | * @param normalize If True, quaternion is normalized after integration 99 | */ 100 | template 101 | static inline void integrateEuler(Eigen::Quaternion &q, Eigen::Quaternion &w, Scalar dt, 102 | bool normalize = true) { 103 | q = q + (q * w * static_cast(0.5)) * dt; 104 | 105 | if (normalize) { 106 | q.normalize(); 107 | } 108 | } 109 | 110 | /** 111 | * @brief Integrate a rotation quaternion using 4th order Runge Kutta 112 | * @param q Quaternion to integrate 113 | * @param w Angular velocity (body frame), stored in 3 complex terms 114 | * @param dt Time interval in seconds 115 | * @param normalize If true, quaternion is normalized after integration 116 | */ 117 | template 118 | static inline void integrateRungeKutta4(Eigen::Quaternion &q, const Eigen::Quaternion &w, Scalar dt, 119 | bool normalize = true) { 120 | const static Scalar half = static_cast(0.5); 121 | const static Scalar two = static_cast(2); 122 | 123 | Eigen::Quaternion qw = q * w * half; 124 | Eigen::Quaternion k2 = (q + qw * dt * half) * w * half; 125 | Eigen::Quaternion k3 = (q + k2 * dt * half) * w * half; 126 | Eigen::Quaternion k4 = (q + k3 * dt) * w * half; 127 | 128 | q = q + (qw + k2 * two + k3 * two + k4) * (dt / 6); 129 | 130 | if (normalize) { 131 | q.normalize(); 132 | } 133 | } 134 | 135 | template 136 | static inline Eigen::Matrix 137 | rodrigues(const Eigen::Matrix& w) { 138 | const auto norm = w.norm(); 139 | if (norm < std::numeric_limits::epsilon()*10) { 140 | return Eigen::Matrix::Identity() + crossSkew(w); 141 | } 142 | return Eigen::AngleAxis(norm, w / norm).matrix(); 143 | } 144 | 145 | AttitudeESKF::AttitudeESKF() 146 | : q_(1,0,0,0), steadyCount_(0), biasThresh_(0), isStable_(true) { 147 | P_.setZero(); 148 | b_.setZero(); 149 | w_.setZero(); 150 | dx_.setZero(); 151 | 152 | magRef_.setZero(); 153 | predMag_.setZero(); 154 | 155 | estBias_ = false; 156 | ignoreZ_ = false; 157 | useMag_ = false; 158 | } 159 | 160 | void AttitudeESKF::predict(const AttitudeESKF::vec3 &wb, 161 | AttitudeESKF::scalar_t dt, 162 | const AttitudeESKF::mat3 &cov, 163 | bool useRK4) { 164 | static const Matrix I3 = 165 | Matrix::Identity(); // identity R3 166 | 167 | scalar_t wb2 = wb[0] * wb[0] + wb[1] * wb[1] + wb[2] * wb[2]; 168 | if (wb2 < biasThresh_ * biasThresh_) { 169 | steadyCount_++; // not rotating, update moving average 170 | 171 | if (estBias_ && steadyCount_ > 20) { 172 | b_ = (b_ * (steadyCount_ - 1) + wb) / steadyCount_; 173 | } 174 | } else { 175 | steadyCount_ = 0; 176 | } 177 | 178 | w_ = (wb - b_); // true gyro reading 179 | 180 | // error-state jacobian 181 | const Matrix F = I3 - crossSkew(w_ * dt); 182 | 183 | // integrate state and covariance 184 | Eigen::Quaternion wQuat(0, w_[0], w_[1], w_[2]); 185 | if (!useRK4) { 186 | integrateEuler(q_, wQuat, dt, true); 187 | } else { 188 | integrateRungeKutta4(q_, wQuat, dt, true); 189 | } 190 | 191 | // noise jacobian 192 | const Matrix G = -I3 * dt; 193 | P_ = F*P_*F.transpose() + G*cov*G.transpose(); 194 | } 195 | 196 | void AttitudeESKF::update(const AttitudeESKF::vec3 &ab, 197 | const mat3 &aCov, 198 | const AttitudeESKF::vec3 &mb, 199 | const mat3 &mCov) { 200 | Matrix A; 201 | 202 | // rotation matrix: world -> body 203 | const Matrix bRw = q_.conjugate().matrix(); 204 | 205 | vec3 gravity; 206 | gravity[0] = 0.0; 207 | gravity[1] = 0.0; 208 | gravity[2] = kOneG; 209 | 210 | // predicted gravity vector 211 | const vec3 aPred = bRw * gravity; 212 | 213 | if (!useMag_) { 214 | // calculate jacobian 215 | Matrix H = crossSkew(aPred); 216 | Matrix r = ab - aPred; 217 | 218 | // solve for the kalman gain 219 | const Matrix S = H * P_ * H.transpose() + aCov; 220 | Matrix Sinv; 221 | 222 | const scalar_t det = determinant(S); 223 | if (std::abs(det) < static_cast(1e-5)) { 224 | isStable_ = false; 225 | return; 226 | } else { 227 | isStable_ = true; 228 | } 229 | Sinv = invert(S, det); 230 | 231 | const Matrix K = P_ * H.transpose() * Sinv; 232 | 233 | A = K * H; 234 | dx_ = K * r; 235 | } 236 | else { 237 | #ifdef ATTITUDE_ESKF_BUILD_MAG // stop compilation of FullPivLU 238 | // m-field prediction 239 | vec3 field = bRw * magRef_; 240 | predMag_ = field; 241 | 242 | Matrix r; 243 | r.block<3, 1>(0, 0) = ab - aPred; 244 | r.block<3, 1>(3, 0) = mb - field; 245 | 246 | Matrix H; 247 | H.setZero(); 248 | 249 | // jacobians for gravity and magnetic field 250 | H.block<3, 3>(0, 0) = crossSkew(aPred); 251 | H.block<3, 3>(3, 0) = crossSkew(field); 252 | 253 | // covariance for both sensors 254 | Matrix covR; 255 | covR.setZero(); 256 | covR.block<3,3>(0,0) = aCov; 257 | covR.block<3,3>(3,3) = mCov; 258 | 259 | const Matrix S = H * P_ * H.transpose() + covR; 260 | Matrix Sinv; 261 | 262 | Eigen::FullPivLU> LU(S); 263 | isStable_ = LU.isInvertible(); 264 | 265 | if (!isStable_) { 266 | return; 267 | } 268 | Sinv = LU.inverse(); 269 | 270 | // generate update 271 | const Matrix K = P_ * H.transpose() * Sinv; 272 | dx_ = K * r; 273 | A = K * H; 274 | #else 275 | dx_.setZero(); 276 | A.setZero(); 277 | #endif 278 | } 279 | 280 | if (ignoreZ_) { 281 | // cancel body-frame z update 282 | dx_[2] = 0; 283 | } 284 | 285 | // perform state update 286 | P_ = (Matrix::Identity() - A) * P_; 287 | 288 | q_ = q_ * quat(1, dx_[0]/2, dx_[1]/2, dx_[2]/2); 289 | q_.normalize(); 290 | } 291 | 292 | void AttitudeESKF::externalYawUpdate(scalar_t yaw, scalar_t alpha) { 293 | // check if we are near the hover state 294 | const Matrix wRb = q_.matrix(); 295 | Matrix g; 296 | g[0] = 0; 297 | g[1] = 0; 298 | g[2] = 1; 299 | 300 | g = wRb.transpose() * g; 301 | if (g[2] > 0.85) { 302 | // break into roll pitch yaw 303 | Matrix rpy = getRPY(wRb); 304 | // interpolate between prediction and estimate 305 | rpy[2] = rpy[2]*(1-alpha) + yaw*alpha; 306 | q_ = Eigen::AngleAxis(rpy[2],vec3(0,0,1)) * 307 | Eigen::AngleAxis(rpy[1],vec3(0,1,0)) * 308 | Eigen::AngleAxis(rpy[0],vec3(1,0,0)); 309 | } 310 | } 311 | 312 | bool AttitudeESKF::initialize(const vec3 &ab, 313 | const vec3 &aCov, 314 | const vec3 &mb, 315 | const vec3 &mCov) { 316 | if (!useMag_) { 317 | // determine attitude angles 318 | scalar_t ay = ab[1]; 319 | if (ay > kOneG) { ay = kOneG; } 320 | else if (ay < -kOneG) { ay = -kOneG; } 321 | const scalar_t& ax = ab[0]; 322 | const scalar_t& az = ab[2]; 323 | 324 | const scalar_t phi = std::asin(-ay / kOneG); // roll 325 | const scalar_t theta = std::atan2(ax, az); // pitch 326 | 327 | q_ = Eigen::AngleAxis(theta, vec3(0,1,0)) * 328 | Eigen::AngleAxis(phi, vec3(1,0,0)); 329 | } 330 | else { 331 | /// @todo: This is kind of ugly, find some simpler mechanism to do this. 332 | 333 | #ifdef ATTITUDE_ESKF_BUILD_MAG 334 | const static scalar_t eps(1e-6); 335 | for (int i=0; i < 3; i++) { 336 | if (aCov[i] < eps || mCov[i] < eps) { 337 | return false; 338 | } 339 | } 340 | // jacobian 341 | Eigen::Matrix J; 342 | J.block<3,3>(0,0) = crossSkew(ab); 343 | J.block<3,3>(3,0) = crossSkew(mb); 344 | 345 | // weight matrix 346 | Eigen::Matrix S; 347 | S.setZero(); 348 | for (int i=0; i < 3; i++) { 349 | S(i,i) = 1 / aCov[i]; 350 | S(i+3,i+3) = 1 / mCov[i]; 351 | } 352 | 353 | // hessian 354 | const mat3 H = J.transpose() * S * J; 355 | const Eigen::LDLT ldlt(H); 356 | 357 | // optimize 358 | vec3 w(0,0,0); 359 | Matrix r; 360 | for (unsigned int iter=0; iter < 5; iter++) { 361 | const mat3 W = rodrigues(w); 362 | // residuals 363 | r.block<3,1>(0,0) = (W * vec3(0,0,kOneG)) - ab; 364 | r.block<3,1>(3,0) = (W * magRef_) - mb; 365 | // step 366 | w.noalias() += ldlt.solve(J.transpose() * S * r); 367 | } 368 | q_ = quat(rodrigues(w).transpose()); 369 | #endif 370 | } 371 | // start w/ a large uncertainty 372 | P_.setIdentity(); 373 | P_ *= M_PI*M_PI; 374 | 375 | return true; 376 | } 377 | 378 | AttitudeESKF::vec3 AttitudeESKF::getRPY(const mat3& R) { 379 | vec3 rpy; 380 | scalar_t sth = -R(2, 0); 381 | if (sth > 1) { 382 | sth = 1; 383 | } else if (sth < -1) { 384 | sth = -1; 385 | } 386 | 387 | const scalar_t theta = std::asin(sth); 388 | const scalar_t cth = std::sqrt(1 - sth*sth); 389 | 390 | scalar_t phi, psi; 391 | if (cth < static_cast(1.0e-6)) { 392 | phi = std::atan2(R(0, 1), R(1, 1)); 393 | psi = 0; 394 | } else { 395 | phi = std::atan2(R(2, 1), R(2, 2)); 396 | psi = std::atan2(R(1, 0), R(0, 0)); 397 | } 398 | 399 | rpy[0] = phi; // x, [-pi,pi] 400 | rpy[1] = theta; // y, [-pi/2,pi/2] 401 | rpy[2] = psi; // z, [-pi,pi] 402 | return rpy; 403 | } 404 | 405 | } // namespace kr 406 | 407 | -------------------------------------------------------------------------------- /src/AttitudeMagCalib.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AttitudeMagCalib.cpp 3 | * 4 | * Copyright (c) 2013 Gareth Cross. All rights reserved. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 23/06/2014 9 | * Author: gareth 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace Eigen; 17 | 18 | namespace kr { 19 | 20 | AttitudeMagCalib::AttitudeMagCalib() { reset(); } 21 | 22 | void AttitudeMagCalib::reset() { 23 | binH_.clear(); 24 | binV_.clear(); 25 | calibrated_ = false; 26 | bias_.setZero(); 27 | scale_.setOnes(); 28 | } 29 | 30 | void AttitudeMagCalib::appendSample(const quat &att, 31 | const vec3 &field) { 32 | SampleBin bin; 33 | bin.field = field; 34 | bin.q = att; 35 | 36 | const vec3 localG = att.conjugate().matrix() * vec3(0,0,1); 37 | 38 | // determine local angle 39 | if (std::abs(localG[2]) < 0.1) { 40 | // world vertical is approx. in the local X/Y plane 41 | const vec3 worldZ = att.matrix() * vec3(0,0,1); 42 | const scalar_t ang = std::atan2(worldZ[1], worldZ[0]); 43 | const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount; 44 | binV_[key] = bin; 45 | } else if (std::abs(localG[2]) > 0.9) { 46 | // world vertical is approx. vertical 47 | const vec3 worldX = att.matrix() * vec3(1,0,0); 48 | const scalar_t ang = std::atan2(worldX[1], worldX[0]); 49 | const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount; 50 | binH_[key] = bin; 51 | } 52 | } 53 | 54 | bool AttitudeMagCalib::isReady() const { 55 | if (binV_.size() < kBinMaxCount*8/10) { 56 | return false; 57 | } 58 | if (binH_.size() < kBinMaxCount*8/10) { 59 | return false; 60 | } 61 | return true; 62 | } 63 | 64 | bool AttitudeMagCalib::isCalibrated() const { return calibrated_; } 65 | 66 | void AttitudeMagCalib::calibrate(AttitudeMagCalib::CalibrationType type) { 67 | 68 | if (!isReady()) { 69 | throw insufficient_data(); 70 | } 71 | 72 | if (type == AttitudeMagCalib::FullCalibration) { 73 | // perform full estimation of bias and scale 74 | vec3 bias(0,0,0), scl(1,1,1); 75 | 76 | std::vector meas; 77 | for (const std::pair& s : binH_) { 78 | meas.push_back(s.second.field); 79 | } 80 | for (const std::pair&s : binV_) { 81 | meas.push_back(s.second.field); 82 | } 83 | const size_t N = meas.size(); 84 | 85 | // fit to sphere 86 | Matrix A(N, 4); 87 | Matrix b(N, 1); 88 | for (size_t i=0; i < meas.size(); i++) { 89 | A(i,0) = 2*meas[i][0]; 90 | A(i,1) = 2*meas[i][1]; 91 | A(i,2) = 2*meas[i][2]; 92 | A(i,3) = 1; 93 | b(i,0) = meas[i][0]*meas[i][0] + meas[i][1]*meas[i][1] + 94 | meas[i][2]*meas[i][2]; 95 | } 96 | // solve system for center 97 | const Matrix x = A.colPivHouseholderQr().solve(b); 98 | bias = x.block<3,1>(0,0); 99 | 100 | scalar_t mean_rad = 0.0, mean_rad_sqr = 0.0; 101 | // calculate estimate of mean radius 102 | for (const vec3 &v : meas) { 103 | const scalar_t r = (v - bias).norm(); 104 | mean_rad += r; 105 | mean_rad_sqr += r * r; 106 | } 107 | mean_rad /= N; 108 | mean_rad_sqr /= N; 109 | 110 | // refine with GN-NLS 111 | Matrix J(N, 6); 112 | Matrix r(N, 1); 113 | Matrix W(N,N); 114 | W.setZero(); 115 | 116 | for (int iter = 0; iter < 20; iter++) { 117 | for (size_t i = 0; i < meas.size(); i++) { 118 | const scalar_t x = (meas[i][0] - bias[0]) / scl[0]; 119 | const scalar_t y = (meas[i][1] - bias[1]) / scl[1]; 120 | const scalar_t z = (meas[i][2] - bias[2]) / scl[2]; 121 | const scalar_t r2 = x*x + y*y + z*z; 122 | 123 | // residual 124 | r(i,0) = mean_rad*mean_rad - r2; 125 | // jacobian 126 | J(i,0) = -2 * x * x / scl[0]; 127 | J(i,1) = -2 * y * y / scl[1]; 128 | J(i,2) = -2 * z * z / scl[2]; 129 | J(i,3) = -2 * x / scl[0]; 130 | J(i,4) = -2 * y / scl[1]; 131 | J(i,5) = -2 * z / scl[2]; 132 | } 133 | 134 | // calculate mean squared error 135 | scalar_t sigmaSquared=0; 136 | for (int j=0; j < r.rows(); j++) { 137 | sigmaSquared += r(j,0)*r(j,0); 138 | } 139 | sigmaSquared /= r.rows(); 140 | // calculate cauchy weights 141 | for (int j=0; j < r.rows(); j++) { 142 | const scalar_t errSqr = r(j,0)*r(j,0); 143 | W(j,j) = 1.0 / (1 + errSqr/sigmaSquared); 144 | } 145 | 146 | Matrix H = J.transpose() * W * J; 147 | for (int i = 0; i < 3; i++) { // prior on scale 148 | H(i,i) *= 1.01; 149 | } 150 | Eigen::LDLT> LDLT(H); 151 | const Matrix update = LDLT.solve(J.transpose() * W * r); 152 | 153 | scl += update.block<3, 1>(0, 0); 154 | bias += update.block<3, 1>(3, 0); 155 | } 156 | 157 | bias_ = bias; 158 | scale_ = scl; 159 | } else { 160 | bias_.setZero(); 161 | scale_.setOnes(); 162 | } 163 | 164 | calibrated_ = true; 165 | } 166 | 167 | } // namespace kr 168 | -------------------------------------------------------------------------------- /src/Node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Node.cpp 3 | * 4 | * Copyright (c) 2014 Gareth Cross. All rights reserved. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 31/08/2014 9 | * Author: gareth 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace kr_attitude_eskf { 18 | 19 | Node::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) : nh_(pnh), 20 | sync_(TimeSyncPolicy(kROSQueueSize), subImu_, subField_), 21 | calibState_(Uncalibrated), init_(false) { 22 | 23 | // load settings 24 | nh_.param("enable_magnetometer", enableMag_, false); 25 | nh_.param("gyro_bias_thresh", gyroBiasThresh_, 0.01); 26 | nh_.param("process_scale_factor", processScaleFactor_, 1.0); 27 | // load magnetometer calibration 28 | magBias_.setZero(); 29 | magScale_.setConstant(1); 30 | magReference_ << 0, 0.205779, -0.478950; // default to Philadelphia 31 | if (enableMag_) { 32 | try { 33 | std::vector biasVec, scaleVec, refVec; 34 | nh_.getParam("mag_calib/bias", biasVec); 35 | nh_.getParam("mag_calib/scale", scaleVec); 36 | if (nh_.hasParam("mag_calib/reference")) { 37 | nh_.getParam("mag_calib/reference", refVec); 38 | } 39 | if (biasVec.size() != 3 || scaleVec.size() != 3 || refVec.size() != 3) { 40 | throw std::invalid_argument("List w/ invalid size in mag_calib."); 41 | } 42 | 43 | magBias_ << biasVec[0], biasVec[1], biasVec[2]; 44 | magScale_ << scaleVec[0], scaleVec[1], scaleVec[2]; 45 | magReference_ << refVec[0], refVec[1], refVec[2]; 46 | 47 | ROS_INFO("Loaded mag_calib:"); 48 | ROS_INFO("Bias: %f,%f,%f",magBias_[0],magBias_[1],magBias_[2]); 49 | ROS_INFO("Scale: %f,%f,%f",magScale_[0],magScale_[1],magScale_[2]); 50 | ROS_INFO("Reference: %f,%f,%f",magReference_[0],magReference_[1], 51 | magReference_[2]); 52 | 53 | calibState_ = CalibrationComplete; 54 | } catch (std::exception& e) { 55 | ROS_WARN("Warning: Failed to load mag_calib parameters."); 56 | ROS_WARN("Reason: %s", e.what()); 57 | } 58 | 59 | // subscribe using time sync 60 | ROS_INFO("Subscribing to ~imu and ~magnetic_field."); 61 | 62 | subImu_.subscribe(nh_,"imu",kROSQueueSize); 63 | subField_.subscribe(nh_,"magnetic_field",kROSQueueSize); 64 | sync_.registerCallback(boost::bind(&Node::inputCallback, this, _1, _2)); 65 | } else { 66 | // subscribe only to IMU 67 | ROS_INFO("Subscribing to ~imu."); 68 | subImuUnsync_ = nh_.subscribe("imu", kROSQueueSize, 69 | boost::bind(&Node::inputCallback, this, _1, 70 | sensor_msgs::MagneticFieldConstPtr())); 71 | } 72 | 73 | if (enableMag_ && calibState_==Uncalibrated) { 74 | ROS_WARN("Warning: Magnetometer is enabled but not calibrated."); 75 | } 76 | 77 | pubImu_ = nh_.advertise("filtered_imu", 1); 78 | pubBias_ = nh_.advertise("bias", 1); 79 | pubPose_ = nh_.advertise("pose", 1); 80 | // 'unbiased' and scaled magnetic field 81 | pubField_ = nh_.advertise("corrected_field",1); 82 | 83 | // register updater 84 | if (!nh_.hasParam("diagnostic_period")) { 85 | nh_.setParam("diagnostic_period", 0.2); 86 | } 87 | updater_.setHardwareID(ros::this_node::getName()); 88 | updater_.add("diagnostic_info", 89 | boost::bind(&Node::diagnosticCallback, this, _1)); 90 | 91 | // register for service callback 92 | ros::NodeHandle publicNh(nh); 93 | srvCalibrate_ = publicNh.advertiseService(ros::this_node::getName() + "/begin_calibration", 94 | &Node::beginCalibration, this); 95 | 96 | // configure filter 97 | eskf_.setEstimatesBias(true); 98 | eskf_.setGyroBiasThreshold(gyroBiasThresh_); 99 | } 100 | 101 | void Node::saveCalibration() { 102 | std::vector vec(3); 103 | vec[0] = magBias_[0]; 104 | vec[1] = magBias_[1]; 105 | vec[2] = magBias_[2]; 106 | nh_.setParam("mag_calib/bias", vec); 107 | vec[0] = magScale_[0]; 108 | vec[1] = magScale_[1]; 109 | vec[2] = magScale_[2]; 110 | nh_.setParam("mag_calib/scale", vec); 111 | } 112 | 113 | void Node::inputCallback(const sensor_msgs::ImuConstPtr& imuMsg, 114 | const sensor_msgs::MagneticFieldConstPtr& magMsg) { 115 | if (enableMag_) { 116 | assert(magMsg); 117 | } 118 | 119 | vec3 wm; // measured angular rate 120 | vec3 am; // measured acceleration 121 | vec3 mm(0,0,0); // measured magnetic field 122 | 123 | wm = vec3(imuMsg->angular_velocity.x, 124 | imuMsg->angular_velocity.y, 125 | imuMsg->angular_velocity.z); 126 | am = vec3(imuMsg->linear_acceleration.x, 127 | imuMsg->linear_acceleration.y, 128 | imuMsg->linear_acceleration.z); 129 | if (magMsg) { 130 | mm = vec3(magMsg->magnetic_field.x, 131 | magMsg->magnetic_field.y, 132 | magMsg->magnetic_field.z); 133 | } 134 | 135 | // copy covariances from message to matrix 136 | kr::AttitudeESKF::mat3 wCov, aCov, mCov = kr::AttitudeESKF::mat3::Zero(); 137 | for (int i=0; i < 3; i++) { 138 | for (int j=0; j < 3; j++) { 139 | wCov(i,j) = imuMsg->angular_velocity_covariance[i*3 + j]; 140 | aCov(i,j) = imuMsg->linear_acceleration_covariance[i*3 + j]; 141 | if (magMsg) { 142 | mCov(i,j) = magMsg->magnetic_field_covariance[i*3 + j]; 143 | } 144 | } 145 | } 146 | wCov *= processScaleFactor_; // apply fudge factor 147 | 148 | if (enableMag_ && calibState_==CalibrationComplete) { 149 | // correct magnetic field 150 | mm.noalias() -= magBias_; 151 | for (int i=0; i < 3; i++) { 152 | mm[i] /= magScale_[i]; 153 | } 154 | eskf_.setMagneticReference(magReference_); 155 | eskf_.setUsesMagnetometer(true); 156 | } else { 157 | eskf_.setUsesMagnetometer(false); 158 | } 159 | 160 | if (prevStamp_.sec != 0) { 161 | if (!init_) { 162 | const vec3 aCovDiag(aCov(0,0),aCov(1,1),aCov(2,2)); 163 | const vec3 mCovDiag(mCov(0,0),mCov(1,1),mCov(2,2)); 164 | eskf_.initialize(am,aCovDiag,mm,mCovDiag); 165 | init_ = true; 166 | ROS_INFO("Initialized ESKF"); 167 | } 168 | 169 | // run kalman filter 170 | const double delta = (imuMsg->header.stamp - prevStamp_).toSec(); 171 | eskf_.predict(wm, delta, wCov); 172 | eskf_.update(am, aCov, mm, mCov); 173 | 174 | const kr::AttitudeESKF::quat wQb = eskf_.getQuat(); 175 | const vec3 w = eskf_.getAngularVelocity(); // ang vel. minus bias 176 | 177 | if (calibState_ == Calibrating) { 178 | // update the calibrator 179 | if (!calib_.isCalibrated()) { 180 | calib_.appendSample(wQb, mm); 181 | if (calib_.isReady()) { 182 | ROS_INFO("Collected sufficient samples. Calibrating..."); 183 | // calibrate bias, scale and reference vector 184 | try { 185 | calib_.calibrate(kr::AttitudeMagCalib::FullCalibration); 186 | 187 | ROS_INFO_STREAM("Bias: " << calib_.getBias().transpose()); 188 | ROS_INFO_STREAM("Scale: " << calib_.getScale().transpose()); 189 | 190 | magBias_ = calib_.getBias(); 191 | magScale_ = calib_.getScale(); 192 | 193 | // save to rosparam 194 | nh_.setParam("enable_magnetometer", true); 195 | saveCalibration(); 196 | 197 | // done, we can use this new calibration immediately 198 | calibState_ = CalibrationComplete; 199 | enableMag_ = true; 200 | } 201 | catch (std::exception& e) { 202 | ROS_ERROR("Calibration failed: %s", e.what()); 203 | } 204 | } 205 | } 206 | } 207 | 208 | sensor_msgs::Imu imu = *imuMsg; 209 | imu.header.seq = 0; 210 | 211 | imu.angular_velocity.x = w[0]; 212 | imu.angular_velocity.y = w[1]; 213 | imu.angular_velocity.z = w[2]; 214 | imu.orientation.w = wQb.w(); 215 | imu.orientation.x = wQb.x(); 216 | imu.orientation.y = wQb.y(); 217 | imu.orientation.z = wQb.z(); 218 | // append our covariance estimate to the new IMU message 219 | for (int i = 0; i < 3; i++) { 220 | for (int j = 0; j < 3; j++) { 221 | imu.orientation_covariance[i*3 + j] = eskf_.getCovariance()(i,j); 222 | } 223 | } 224 | geometry_msgs::Vector3Stamped bias; 225 | bias.header = imu.header; 226 | bias.vector.x = eskf_.getGyroBias()[0]; 227 | bias.vector.y = eskf_.getGyroBias()[1]; 228 | bias.vector.z = eskf_.getGyroBias()[2]; 229 | // pose with no position 230 | geometry_msgs::PoseStamped pose; 231 | pose.header = imu.header; 232 | pose.pose.orientation = imu.orientation; 233 | 234 | if (enableMag_) { 235 | sensor_msgs::MagneticField field = *magMsg; 236 | field.header.seq = 0; 237 | field.magnetic_field.x = mm[0]; 238 | field.magnetic_field.y = mm[1]; 239 | field.magnetic_field.z = mm[2]; 240 | pubField_.publish(field); 241 | } 242 | 243 | pubImu_.publish(imu); 244 | pubBias_.publish(bias); 245 | pubPose_.publish(pose); 246 | } 247 | prevStamp_ = imuMsg->header.stamp; 248 | updater_.update(); 249 | } 250 | 251 | bool Node::beginCalibration(std_srvs::Empty::Request&, 252 | std_srvs::Empty::Response&) { 253 | // enter calibration mode 254 | calibState_ = Calibrating; 255 | calib_.reset(); 256 | ROS_INFO("Entering calibration mode."); 257 | return true; 258 | } 259 | 260 | void 261 | Node::diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper &stat) { 262 | 263 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Running"); 264 | 265 | stat.add("Magnetometer enabled:", enableMag_); 266 | stat.add("Gyro bias threshold:", gyroBiasThresh_); 267 | stat.add("Process scale factor:", processScaleFactor_); 268 | std::stringstream ss; 269 | // current bias estimate 270 | ss << eskf_.getGyroBias().transpose(); 271 | stat.add("Gyroscope bias:", ss.str()); 272 | // mag calib state 273 | std::string calib; 274 | if (calibState_ == Uncalibrated) { 275 | calib = "Uncalibrated"; 276 | } else if (calibState_ == Calibrating) { 277 | calib = "Calibrating"; 278 | } else { 279 | calib = "Complete"; 280 | } 281 | stat.add("Calibration state:", calib); 282 | } 283 | 284 | } 285 | -------------------------------------------------------------------------------- /src/kr_attitude_eskf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * kr_attitude_eskf.cpp 3 | * 4 | * Copyright (c) 2014 Kumar Robotics. All rights reserved. 5 | * 6 | * This file is part of kr_attitude_eskf. 7 | * 8 | * Created on: 17/6/2014 9 | * Author: gareth 10 | */ 11 | 12 | #include 13 | 14 | int main(int argc, char **argv) { 15 | ros::init(argc, argv, "kr_attitude_eskf"); 16 | ros::NodeHandle nh; 17 | ros::NodeHandle pnh("~"); 18 | kr_attitude_eskf::Node node(nh,pnh); 19 | ros::spin(); 20 | return 0; 21 | } 22 | --------------------------------------------------------------------------------