How to fuse encoder ticks + IMU for odometry?

edit

Hi,

I have an IMU which shows the orientation perfectly and also installed some optical encoders. When I move the robot straight, the encoders will show the movement more or less correctly. However, when I turn and then move, IMU first displays orientation then when I move after the turn suddenly my robot gets drifted back and then move. I guess that this is because encoders do their job separately but how can I fuse them together?

This is my encoder tick callback (one of them, second will be similar):

void Odometry::leftencoderCb(const std_msgs::Int64::ConstPtr& left_ticks)
{
    double enc = left_ticks->data;

    if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap))
    {
        lmult = lmult + 1;
    }

    if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap))
    {
        lmult = lmult + 1;
    }

    left = 1.0 * (enc + lmult * (encoder_max - encoder_min));
    prev_lencoder = enc;
}

Here is my IMU callback:

void Odometry::ImuCb(const sensor_msgs::Imu::ConstPtr& data)
{
    orient_x = data->orientation.x;
    orient_t = data->orientation.y;
    orient_z = data->orientation.z;
    orient_w = data->orientation.w;

}

And finally, here is my update code which updates odometry periodically:

void Odometry::update()
{

    ros::Time now = ros::Time::now();
    double elapsed;
    double d_left, d_right, d, th,x,y;

    elapsed = now.toSec() - then.toSec();

    if(enc_left == 0)
    {
        d_left = 0;
        d_right = 0;
    }
        else
        {
            d_left = (left - enc_left) / ( ticks_meter);
            d_right = (right - enc_right) / ( ticks_meter);
        }

        enc_left = left;
        enc_right = right;

        d = (d_left + d_right ) / 2.0;
        th = ( d_right - d_left ) / base_width;

        dx = d / elapsed;
        dr = th / elapsed;

        if (d != 0)
        {
            ROS_INFO("Distance has been changed!");
            x = cos(th) * d;
            y = -sin(th) * d;
            x_final += (cos(theta_final) * x - sin(theta_final) * y);
            y_final += (sin(theta_final) * x + cos(theta_final) * y);
        }


        theta_final += th;

        geometry_msgs::Quaternion odom_quat ;

        odom_quat.x = this->orient_x;
        odom_quat.y = this->orient_y;
        odom_quat.z = this->orient_z;
        odom_quat.w = this->orient_w;

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = now;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = x_final;
        odom_trans.transform.translation.y = y_final;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        odom_broadcaster.sendTransform(odom_trans);

            // Odometry values:
        nav_msgs::Odometry odom;
        odom.header.stamp = now;
        odom.header.frame_id = "odom";

        odom.pose.pose.position.x = x_final;
        odom.pose.pose.position.y = y_final;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_footprint";
        odom.twist.twist.linear.x = dx;
        odom.twist.twist.linear.y = 0.0; //dy;
        odom.twist.twist.angular.z = 0.0; //dr;

        odom_pub.publish(odom);

        then = now;

        ros::spinOnce();



    else ROS_INFO_STREAM("Not in a loop");
}

Can someone suggest me how can I write odometry code more correctly?