search

ros-drivers / phidgets_drivers

ROS drivers for various Phidgets devices
Other
45 stars 61 forks source link

Spatial precision - underreported velocity #68

Closed msadowski closed 4 years ago

msadowski commented 4 years ago

Hi!

I think I'm running into an issue with PhidgetSpatial Precision 3/3/3 High Resolution and thought maybe someone here would be able to offer some advice on the problem.

What I have noticed while testing my robot is that the odometry coming out from the EKF would be worse than the wheel odometry of the robot:

image

In the above image the green odometry is from wheel encoders and the red one is wheel_encoders odometry fused with the phidgets IMU.

In the IMU part of the EKF I'm only fusing angular rates. On the robot I've mounted an Realsense T265 tracking camera that also contains the IMU. Comparing the angular rates of the relevant axes you can see that there is slight difference in measurements:

image

Another thing to note is that I see this bot with the /imu/data_raw and /imu/data coming out from the madgwick filter.

If you have any advice I'd appreciate it. I think this setup should be correct but maybe I missed something.

mintar commented 4 years ago

According to the second plot, the two IMUs disagree by about 0.02 rad/s (approx. 1°/s), so that doesn't sound too bad. I believe that shouldn't lead to the problems you're experiencing. You can test that theory by using the T265's IMU as input to your EKF; if the problem persists, you can rule out the velocity difference between the IMUs as an error source.

Another thing to note is that I see this bot with the /imu/data_raw and /imu/data coming out from the madgwick filter.

Nope, the madgwick filter subscribes to /imu/data_raw, fills the orientation field and republishes the data to /imu/data.

In summary, I suspect that there's something wrong with your EKF configuration.

msadowski commented 4 years ago

Thanks for a swift reply @mintar! Indeed you are correct that my problem will lie somewhere else. Here is the same setup using EKF fusing the Realsense IMU:

image

The first thing that strikes me is that now the odometry coming out of EKF is rotated to the right w.r.t. the pure wheel odom (with the Phidget it was tilted to the left). It might be somehow related to the fact that the realsense is mounted in the front of the robot and Phidget on the back.

I'll close this issue now as it's clearly not the Phidget's issue. If you happen to have any EKF advice then I'd happily take it!