robot motion estimation

edit

Hello

I have some problems with my amcl , the motion estimation and the calculated linear and angular velocity. My robot is an electrical wheelchair with camera, laser range finder and IMU. For the pose estimation am using the amcl ( which use only as an input the laser scans). So my amcl jump lots and also have a very noise datas. So my linear and angular velocity profiles are very very noise. So how to improve that??Should I use extended kalman filter for the localisation?? Im my node that calculate the velocity Im not using IMU, so how to improve that??This is my speed node and my angular velocity profiles

void speedCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{

    btScalar r,p,y;
    btQuaternion q(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
    btMatrix3x3(q).getEulerYPR(y,p,r);
    std_msgs::Float64 yaw;
    yaw.data = y;
    //ROS_INFO("Yaw: %f", yaw.data*57.29);


    if (count2 == 0){
        time_prev = ros::Time::now().toSec();
        count2=1;
    }

    double time_now = ros::Time::now().toSec();

    //double curr_theta = msg->theta;
    //double curr_x = msg->x;
    //double curr_y = msg->y;

    double curr_theta = yaw.data;
    double curr_x = msg->pose.pose.position.x;
    double curr_y = msg->pose.pose.position.y;

    //ROS_INFO(" before curr_x = %f, curr_y = %f", curr_x, curr_y);

    if (curr_x < 0){
        curr_x = curr_x * -1;
    }
    if (curr_y < 0){
        curr_y = curr_y * -1;
    }
    //ROS_INFO("after curr_x = %f, curr_y = %f", curr_x, curr_y);
    double time_dif = time_now - time_prev;
    if (prev_x != 0 || prev_y != 0){
        if (time_dif != 0){
            double dist_theta = (curr_theta - prev_theta);
            //if (dist_theta < 0){
              //  dist_theta = dist_theta * -1;
            //}
            double dist_x = (curr_x - prev_x);
            if (dist_x < 0){
                dist_x = dist_x * -1;
            }
            double dist_y = (curr_y - prev_y);
            if (dist_y < 0){
                dist_y = dist_y * -1;
            }
            double dist_tot = sqrt((dist_x * dist_x) + (dist_y * dist_y));// * 10;
            double speed = (dist_tot / time_dif);// * 3.6;
            double speed_theta = dist_theta /time_dif;

            if (speed < 0.12){
                speed = 0;
                zero_count++;
            }
            if (speed_theta < 0.07){
                speed_theta = 0;
            }

        ROS_INFO("linear speed= %f, anglular speed=  %f", speed, speed_theta)
                speed_sum = speed_sum + speed;
                speed_sum_theta = speed_sum_theta + speed_theta;
            if (speed_count == 20){
                speed = speed_sum / speed_count;
                speed_theta = speed_sum_theta / speed_count;
                if (zero_count > 7){
                    speed = 0;
                }
                speed_count = 0;
                speed_sum = 0;
                speed_sum_theta = 0;
                zero_count = 0;


                count = count + 1;

            }

        }
    }

    prev_theta = curr_theta;
    prev_x = curr_x;
    prev_y = curr_y;

    time_prev = time_now;

}

Velocity profile after passing a filter, because they were very very noisy.