From 68f5f22aedf11f91e2184e27a5d527eb432376b9 Mon Sep 17 00:00:00 2001 From: libing64 Date: Sun, 28 Feb 2016 00:04:00 +0800 Subject: [PATCH] first commit, add prediction, not runnable --- CMakeLists.txt | 201 ++++++++++++++++++++++++++ launch/att_ekf.launch | 9 ++ package.xml | 64 +++++++++ rviz_cfg/laser_localization.rviz | 232 +++++++++++++++++++++++++++++++ src/att_ekf.cpp | 87 ++++++++++++ src/att_ekf.h | 28 ++++ src/att_ekf_node.cpp | 49 +++++++ src/conversion.cpp | 78 +++++++++++ src/conversion.h | 12 ++ 9 files changed, 760 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 launch/att_ekf.launch create mode 100644 package.xml create mode 100644 rviz_cfg/laser_localization.rviz create mode 100644 src/att_ekf.cpp create mode 100644 src/att_ekf.h create mode 100644 src/att_ekf_node.cpp create mode 100644 src/conversion.cpp create mode 100644 src/conversion.h diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..98f0f5c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,201 @@ +cmake_minimum_required(VERSION 2.8.3) +project(att_ekf) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + message_generation + tf) + +find_package(Eigen3 REQUIRED) + +include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + nav_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs + DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a C++ library +# add_library(att_ekf +# src/${PROJECT_NAME}/att_ekf.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(att_ekf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(att_ekf_node src/att_ekf_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(att_ekf_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(att_ekf_node +# ${catkin_LIBRARIES} +# ) + +add_executable(att_ekf + src/att_ekf_node.cpp + src/att_ekf.cpp + src/conversion.cpp + ) +target_link_libraries(att_ekf ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS att_ekf att_ekf_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_att_ekf.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/launch/att_ekf.launch b/launch/att_ekf.launch new file mode 100644 index 0000000..2abbe51 --- /dev/null +++ b/launch/att_ekf.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..82f88a1 --- /dev/null +++ b/package.xml @@ -0,0 +1,64 @@ + + + att_ekf + 0.0.0 + The att_ekf package + + + + + libing + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + sensor_msgs + tf + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + tf + + + + + + + + \ No newline at end of file diff --git a/rviz_cfg/laser_localization.rviz b/rviz_cfg/laser_localization.rviz new file mode 100644 index 0000000..62d0471 --- /dev/null +++ b/rviz_cfg/laser_localization.rviz @@ -0,0 +1,232 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Map1 + Splitter Ratio: 0.5 + Tree Height: 478 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + laser0_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /mmap + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /front_cam/camera/image + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + Grid: false + LaserScan: false + Map: false + Path: true + RobotModel: false + Value: true + Zoom Factor: 1 + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /trajectory + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /mpath + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 22.9162 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.66903 + Y: -0.217464 + Z: -0.674569 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.2198 + Target Frame: base_stabilized + Value: Orbit (rviz) + Yaw: 3.15041 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1026 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000012e00000378fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000026d000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000029b000001050000001600fffffffb0000000a0049006d00610067006501000002d9000000c70000000000000000000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000410000035f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1631 + X: 55 + Y: 14 diff --git a/src/att_ekf.cpp b/src/att_ekf.cpp new file mode 100644 index 0000000..dc2ad4e --- /dev/null +++ b/src/att_ekf.cpp @@ -0,0 +1,87 @@ +#include "att_ekf.h" +#include +#include + +using namespace std; +using namespace Eigen; + + +Matrix3d skew_symmetric(Vector3d& v) +{ + Matrix3d m; + m << 0, -v(2), v(1), + v(2), 0, -v(0), + -v(1), v(0), 0; + return m; +} + +Att_ekf::Att_ekf() +{ + x.setZero(); + imu_initialized = false; + mag_initialized = false; +} +Att_ekf::~Att_ekf() +{ + +} +void Att_ekf::predict(double t) +{ + double dt = t - dt; + Vector3d w = x.head(3); + Vector3d ra = x.segment<3>(6); + Vector3d rm = x.tail(3); + + Matrix3d skew_w = skew_symmetric(w); + x.segment<3>(0) = x.segment<3>(0) + x.segment<3>(3)*dt; + x.segment<3>(6) = x.segment<3>(6) + skew_w*x.segment<3>(6)*dt; + x.segment<3>(9) = x.segment<3>(9) + skew_w*x.segment<3>(9)*dt; + + Matrix A = MatrixXd::Identity(12, 12); + A.block<3, 3>(0, 3) += MatrixXd::Identity(3, 3)*dt; + A.block<3, 3>(2*3, 2*3) += -skew_symmetric(ra)*dt; + A.block<3, 3>(3*3, 3*3) += -skew_symmetric(rm)*dt; + + P = A*P*A.transpose() + Q; + +} +void Att_ekf::update_magnetic(Vector3d& mag, double t) +{ + if(!mag_initialized) + { + x.tail(3) = mag; + mag_initialized = true; + curr_t = t; + return; + } + predict(t); + + + +} +void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t) +{ + if(!imu_initialized) + { + x.head(3) = gyro; + x.segment<3>(6) = acc; + imu_initialized = true; + curr_t = t; + return; + } + predict(t); +} + + +Matrix3d Att_ekf::get_rotation_matrix() +{ + Matrix3d Rbn;//NED to body + Vector3d ra = x.segment<3>(6); + Vector3d rm = x.segment<3>(9); + Vector3d Iz = -ra/ra.norm(); + Vector3d Iy = skew_symmetric(Iz)*rm; + Iy /= Iy.norm(); + Vector3d Ix = skew_symmetric(Iy)*Iz; + Rbn.col(0) = Ix; Rbn.col(1) = Iy; Rbn.col(2) = Iz; + return Rbn; +} \ No newline at end of file diff --git a/src/att_ekf.h b/src/att_ekf.h new file mode 100644 index 0000000..e89526a --- /dev/null +++ b/src/att_ekf.h @@ -0,0 +1,28 @@ +#include +#include +#include "conversion.h" + +using namespace std; +using namespace Eigen; +class Att_ekf +{ +public: + Att_ekf(); + ~Att_ekf(); + void predict(double t); + void update_magnetic(Vector3d& mag, double t); + void update_imu(Vector3d &acc, Vector3d & gyro, double t); + Matrix3d get_rotation_matrix(); + +private: + double curr_t; + Matrix x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field + Matrix P;//covariance + Matrix Q;//process noise + + Matrix R_imu;//measurement noise + Matrix R_mag;//measurement noise + + bool imu_initialized; + bool mag_initialized; +}; diff --git a/src/att_ekf_node.cpp b/src/att_ekf_node.cpp new file mode 100644 index 0000000..3b78779 --- /dev/null +++ b/src/att_ekf_node.cpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "att_ekf.h" +#include "conversion.h" + +using namespace std; +using namespace Eigen; + + +void magCallback(const sensor_msgs::MagneticFieldConstPtr& msg) +{ + Vector3d mag; + mag(0) = msg->magnetic_field.x; + mag(1) = msg->magnetic_field.y; + mag(2) = msg->magnetic_field.z; + cout << "mag: " << mag.transpose() << endl; +} + + +void imuCallback(const sensor_msgs::ImuConstPtr & msg) +{ + Quaterniond q; + q.w() = msg->orientation.w; + q.x() = msg->orientation.x; + q.y() = msg->orientation.y; + q.z() = msg->orientation.z; + cout << "q: " << q.w() << " " << q.vec().transpose() << endl; +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "laser_localization"); + ros::NodeHandle n("~"); + ros::Subscriber imu_sub = n.subscribe("/imu", 100, imuCallback); + ros::Subscriber mag_sub = n.subscribe("/magnetic_field", 100, magCallback); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/conversion.cpp b/src/conversion.cpp new file mode 100644 index 0000000..e38319a --- /dev/null +++ b/src/conversion.cpp @@ -0,0 +1,78 @@ +#include +#include +#include +using namespace std; +using namespace Eigen; + +/** +Euler angle defination: zyx +Rotation matrix: C_body2ned +**/ +Quaterniond euler2quaternion(Vector3d euler) +{ + double cr = cos(euler(0)/2); + double sr = sin(euler(0)/2); + double cp = cos(euler(1)/2); + double sp = sin(euler(1)/2); + double cy = cos(euler(2)/2); + double sy = sin(euler(2)/2); + Quaterniond q; + q.w() = cr*cp*cy + sr*sp*sy; + q.x() = sr*cp*cy - cr*sp*sy; + q.y() = cr*sp*cy + sr*cp*sy; + q.z() = cr*cp*sy - sr*sp*cy; + return q; +} + + +Matrix3d quaternion2mat(Quaterniond q) +{ + Matrix3d m; + double a = q.w(), b = q.x(), c = q.y(), d = q.z(); + m << a*a + b*b - c*c - d*d, 2*(b*c - a*d), 2*(b*d+a*c), + 2*(b*c+a*d), a*a - b*b + c*c - d*d, 2*(c*d - a*b), + 2*(b*d - a*c), 2*(c*d+a*b), a*a-b*b - c*c + d*d; + return m; +} + +Vector3d mat2euler(Matrix3d m) +{ + double r = atan2(m(2, 1), m(2, 2)); + double p = asin(-m(2, 0)); + double y = atan2(m(1, 0), m(0, 0)); + Vector3d rpy(r, p, y); + return rpy; +} + +Quaterniond mat2quaternion(Matrix3d m) +{ + //return euler2quaternion(mat2euler(m)); + Quaterniond q; + double a, b, c, d; + a = sqrt(1 + m(0, 0) + m(1, 1) + m(2, 2))/2; + b = (m(2, 1) - m(1, 2))/(4*a); + c = (m(0, 2) - m(2, 0))/(4*a); + d = (m(1, 0) - m(0, 1))/(4*a); + q.w() = a; q.x() = b; q.y() = c; q.z() = d; + return q; +} + +Matrix3d euler2mat(Vector3d euler) +{ + double cr = cos(euler(0)); + double sr = sin(euler(0)); + double cp = cos(euler(1)); + double sp = sin(euler(1)); + double cy = cos(euler(2)); + double sy = sin(euler(2)); + Matrix3d m; + m << cp*cy, -cr*sy + sr*sp*cy, sr*sy + cr*sp*cy, + cp*sy, cr*cy + sr*sp*sy, -sr*cy + cr*sp*sy, + -sp, sr*cp, cr*cp; + return m; +} + +Vector3d quaternion2euler(Quaterniond q) +{ + return mat2euler(quaternion2mat(q)); +} \ No newline at end of file diff --git a/src/conversion.h b/src/conversion.h new file mode 100644 index 0000000..0796cb1 --- /dev/null +++ b/src/conversion.h @@ -0,0 +1,12 @@ +#include +#include +#include +using namespace std; +using namespace Eigen; + +Quaterniond euler2quaternion(Vector3d euler); +Matrix3d quaternion2mat(Quaterniond q); +Vector3d mat2euler(Matrix3d m); +Quaterniond mat2quaternion(Matrix3d m); +Matrix3d euler2mat(Vector3d euler); +Vector3d quaternion2euler(Quaterniond q); \ No newline at end of file