first commit, add prediction, not runnable
This commit is contained in:
libing64
commit
68f5f22aed
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
<launch>
|
||||
<node pkg="att_ekf" type="att_ekf" name="att_ekf" output="screen">
|
||||
<remap from="/imu" to="/raw_imu"/>
|
||||
<remap from="/scan" to="/scan" />
|
||||
</node>
|
||||
<!-- Start rviz visualization with preset config -->
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_ekf.rviz"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>att_ekf</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The att_ekf package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="libing@todo.todo">libing</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/att_ekf</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
<!-- <run_depend>message_runtime</run_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -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: <Fixed 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
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#include "att_ekf.h"
|
||||
#include <iostream>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
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<double, 12, 12> 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#include <iostream>
|
||||
#include <Eigen/Eigen>
|
||||
#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<double, 12, 1> x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field
|
||||
Matrix<double, 12, 12> P;//covariance
|
||||
Matrix<double, 12, 12> Q;//process noise
|
||||
|
||||
Matrix<double, 6, 6> R_imu;//measurement noise
|
||||
Matrix<double, 3, 3> R_mag;//measurement noise
|
||||
|
||||
bool imu_initialized;
|
||||
bool mag_initialized;
|
||||
};
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
#include <iostream>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/MagneticField.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <Eigen/Eigen>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#include <iostream>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/Dense>
|
||||
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));
|
||||
}
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
#include <iostream>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/Dense>
|
||||
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);
|
||||
Loading…
Reference in New Issue