Ask Your Question

odometry with imu

asked 2020-08-18 03:02:52 -0600

dinesh gravatar image

updated 2020-08-18 03:08:11 -0600

I'm using robot localization package to estimate the odometry value of my robot. Here right now i only have mpu6050 imu. i'v also ordered encoders from outside but it will take few weeks for me to use the encoder odometry right now. Here i've created a imu node which publishes sensor_msgs/Imu :

#!/usr/bin/env python
# license removed for brevity
import rospy
from sensor_msgs.msg import Imu
import smbus
import math
from math import sin, cos, pi
import tf 
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

# Register
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

def read_byte(reg):
    return bus.read_byte_data(address, reg)

def read_word(reg):
    h = bus.read_byte_data(address, reg)
    l = bus.read_byte_data(address, reg+1)
    value = (h << 8) + l
    return value

def read_word_2c(reg):
    val = read_word(reg)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
        return val

bus = smbus.SMBus(1) # bus = smbus.SMBus(0) fuer Revision 1
address = 0x68       # via i2cdetect

# Aktivieren, um das Modul ansprechen zu koennen
bus.write_byte_data(address, power_mgmt_1, 0)

def talker():
    pub = rospy.Publisher('example/imu', Imu, queue_size=50)
    rospy.init_node('imu_node', anonymous=True)
    start_time =
    current_time =
    last_time =
    roll_ = 0
    yaw_ = 0
    pitch_ = 0
    rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): odom = Odometry()
    while not rospy.is_shutdown():
        gyroskop_xout = read_word_2c(0x43)
        gyroskop_yout = read_word_2c(0x45)
        gyroskop_zout = read_word_2c(0x47)

        roll = gyroskop_xout / 131
        pitch = gyroskop_yout / 131
        yaw = gyroskop_zout / 131

        roll_ = roll_+roll

        quat = tf.transformations.quaternion_from_euler(roll_,pitch_,yaw_)

        accleration_xout = read_word_2c(0x3b)
        accleration_yout = read_word_2c(0x3d)
        accleration_zout = read_word_2c(0x3f)

        a_x = accleration_xout / 1535
        a_y = accleration_yout / 1535
        a_z = accleration_zout / 1535

        i = Imu()
        i.header.stamp =
        i.header.frame_id = 'base_link'
        i.orientation.x = quat[0]
        i.orientation.y = quat[1]
        i.orientation.z = quat[2]
        i.orientation.w = quat[3]
        i.angular_velocity.x = roll
        i.angular_velocity.y = pitch
        i.angular_velocity.z = yaw
        i.linear_acceleration.x = a_x
        i.linear_acceleration.y = a_y
        i.linear_acceleration.z = a_z


if __name__ == '__main__':
    except rospy.ROSInterruptException:

After i run this node, i ran the ekf node by modifything the template they provided to configure given imu topic. Here this is the template configuration file of robot localization package. here i have changed nothing. here the imu topic is /example/imu which i have changed in the imu node instead. so this file is just for demonstratiuno purpose.

# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30

silent_tf_failure: false
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which ...
edit retag flag offensive close merge delete


A couple things:

  1. Please include sample sensor messages, as that will help me spot issues more rapidly than reviewing your IMU code.
  2. Please get rid of all the comments in your EKF config. Those are for documentation, and they clutter up the question here. Also get rid of any parameters that aren't being used, e.g., if use_control is false, don't include it or any of the control-related parameters.
Tom Moore gravatar image Tom Moore  ( 2020-08-31 03:58:15 -0600 )edit

Are you saying this question is answered now?

Tom Moore gravatar image Tom Moore  ( 2020-09-04 04:18:05 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-08-31 04:47:55 -0600

dinesh gravatar image

after i used the in built dmp of the MPU6050 it is giving some good gyro and accleration value.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2020-08-18 03:02:52 -0600

Seen: 537 times

Last updated: Aug 18 '20