odometry with imu

edit

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)
    else:
        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 = rospy.Time.now()
    current_time = rospy.Time.now()
    last_time = rospy.Time.now()
    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
        yaw_=yaw_+yaw
        pitch_=pitch_+pitch

        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 = rospy.Time.now()
        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
        pub.publish(i)

        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

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 ...