Skip to content

A ROS-based open-source software for fusing mobile sensor data and visualizing SmartPhone Orientation

License

Notifications You must be signed in to change notification settings

karry3775/SensorFusionWS

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

60 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

SensorFusionWS

A ROS-based open-source software for fusing mobile sensor data and visualizing SmartPhone Orientation

System and software used:

  • Python
  • ROS melodic
  • Ubuntu 18.04 LTS
  • Gazebo 9 simulator

Connecting the Phone through PhonePi App

  • Connect your phone and computer to the same network (Hotspot can be used)
  • Enter <ip address>:5000 to the URL field in PhonePi app
  • ip addess could be find using ifconfig command on linux terminal and ipconfig on windows cmd prompt.

Visualize Phone Orientation (Just from Accelerometer and Magnetometer data)

$ roscore
$ rosrun sensor_fusion_pkg sensor_streamer.py
$ rosrun sensor_fusion_pkg test_node_for_accel_mag.py
$ roslaunch phone_description phone_spawn.launch
$ roslaunch phone_description phone_rviz.launch

Visualize Phone Orientation (Just from Gyroscope data) [transform broadcaster needs to be enabled]

$ roscore
$ rosrun sensor_fusion_pkg sensor_streamer.py
$ rosrun sensor_fusion_pkg gyro_only_orientation.py
$ roslaunch phone_description phone_spawn.launch
$ roslaunch phone_description phone_rviz.launch

Visualize Phone Orientation (Fused orientation from Gyro, Accelerometer and Magnetometer using Complementary Filter)

[transform broadcaster needs to be enabled]

$ roscore
$ rosrun sensor_fusion_pkg sensor_streamer.py
$ rosrun sensor_fusion_pkg complimentary_filter.py
$ roslaunch phone_description phone_spawn.launch
$ roslaunch phone_description phone_rviz.launch

Equations used for Complementary Filter

Accelerometer

roll_accel

pitch_accel

Magnetometer

M_x

M_y

m_yaw

Gyroscope

roll_g

pitch_g

yaw_g

Real Time Visualization

Complimentary Filter

Kalman Filter Estimates

Kalman Filter

Complementary Filter Estimates

Complementary Filter

Visualize Phone Orientation (Fused orientation from Gyro, Accelerometer and Magnetometer using Kalman Filter)

[transform broadcaster needs to be enabled]

$ roscore
$ rosrun sensor_fusion_pkg sensor_streamer.py
$ rosrun sensor_fusion_pkg kalman_filter.py
$ roslaunch phone_description phone_spawn.launch
$ roslaunch phone_description phone_rviz.launch

Equations used for Kalman Filter

Prediction

kf_prediction

kf_prediction_cov

Kalman Gain

kgain_1

kgain_2

Correction

kf_correction_state

kf_correction_cov

Comparison between fused and gyro only estimates (RPYCF - complementary filter, RPYG - gyro only, RPYKF - kalman filter)

Body Yaw

Body Yaw

Body Pitch

Body Yaw

Body Roll

Body Yaw

Comparison with accurate Angular Benchmark

$ roscore
$ rosrun sensor_fusion_pkg sensor_streamer.py
$ rosrun sensor_fusion_pkg ground_truth_comparison.py
$ rosrun sensor_fusion_pkg kalman_filter.py
$ rosrun sensor_fusion_pkg complimentary_filter.py
$ rosrun sensor_fusion_pkg gyro_only_orientation.py
Note : The phone needs to be aligned with the angular benchmark in such a way that 0 degree yaw corresponds with 0 degrees. Successive positioning of the phones heading on the benchmark is carried out.

benchmark compare

Benchmark using ground truth obtained using articulated robotic arm (ABB4600)

$ roscore
$ download bagfiles from google drive (square_traj_01.bag and rot_180_01.bag) and place in random folder
$ open new terminal and type "rqt_bag" and navigate to one of the bagfiles
$ right click on the topics "Accel_topic_stamped", "Gyro_topic_stamped", "Magno_topic_stamped" and "ee_rotation_stamped" and click "publish" to publish these topics
$ "rosrun sensor_fusion_pkg kalman_filter_robot.py" in separate terminal
$ Click the play button to publish topics.  You should see text being printed in executable terminal
$ Stop publishing at any time and press ctrl+c in terminal.  Plot will be generated

robot

The graphic above is output by following the above procedure. The red line is the ground truth orientation data collected form the robotic manipulator using forward kinematics. The blue line is the kalman filter's estimate of the phone orientation during execution of the trajectory.

About

A ROS-based open-source software for fusing mobile sensor data and visualizing SmartPhone Orientation

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published