-1

I am trying to read and compute the integral of two quantities, Linear speed and Angular speed. But for some reason, the orientation integral has a large offset, something of order of 6000000. Given that the loops are only called when the data is published (around 50Hz) and the size of the Imu-data is off the order of 0.05 the offset can't be drifting.

#include "ros/ros.h"
#include <sensor_msgs/Imu.h>

  ros::Time current_time_;
  ros::Time last_time_;
  double rotation = 0;
  double dt = 0;
  
  
void chatterCallback(const sensor_msgs::Imu::ConstPtr& scout_imu){
  //data parsing    
  sensor_msgs::Imu imu_msg;
  imu_msg = *scout_imu;
  current_time_ = imu_msg.header.stamp;
    
  //the step size
  dt= (current_time_ - last_time_).toSec();
  
  //integration
  rotation =rotation + imu_msg.angular_velocity.z*dt;
  ROS_INFO("rotation: [%f,%f,%f]",rotation,dt,imu_msg.angular_velocity.z);
  last_time_ = current_time_;
  
}




int main(int argc, char **argv){
    
    ros::init(argc, argv, "scout_subs");

    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/mavros/imu/data_raw", 1000, chatterCallback);
    
    
    ros::spin();

  return 0;
}

The reading part is ok. The problem is in the implementation of Integral. enter image description here

A.mir
  • 1
  • 1
  • Looking at the code it appears you're trying to calculate velocity and orientation from an IMU to localize off via dead reckoning, correct? If this is the case, and as a side note, I would *highly* advise against that method as it will end up producing very inaccurate pose estimations. – BTables Nov 02 '21 at 17:12
  • `double dt, dt1,dist,rotation;` these variables are uninitialized, so you can expect any kind of incorrect output. – pptaszni Nov 02 '21 at 17:22
  • Please trim your code to make it easier to find your problem. Follow these guidelines to create a [minimal reproducible example](https://stackoverflow.com/help/minimal-reproducible-example). – Community Nov 04 '21 at 13:04
  • I have trimmed the code a little bit and now I am just subscribing to the Imu data, and the problem is still the same. I add a screenshot of the output terminal. – A.mir Nov 05 '21 at 13:52

1 Answers1

0

The problem was with the definition of dt for the first iteration (last_time_ = 0), which meant dt = current_time_. That's the source of the big offset.

Adrian Mole
  • 49,934
  • 160
  • 51
  • 83
A.mir
  • 1
  • 1