search

wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.07k stars 614 forks source link

frc::DifferentialDrivePoseEstimator not initializing variables correctly on ResetPosition in C++ #3229

Closed webbbt456 closed 3 years ago

webbbt456 commented 3 years ago

When using frc::DifferentialDrivePoseEstimator with a vision source (i.e. calling AddVisionMeasurement), I get weird values from the pose estimator after calling ResetPosition multiple times. I have provided code that simulates 4 robots using frc::DifferentialDriveOdometry and frc::DifferentialDrivePoseEstimator. The first robot uses only frc::DifferentialDriveOdometry and is stationary. The second robot uses both frc::DifferentialDriveOdometry and frc::DifferentialDrivePoseEstimator and is stationary, the pose from frc::DifferentialDriveOdometry is used as the pose to AddVisionMeasurement. The third and fourth robots are stationary for 2 seconds, move forward for 2 seconds, set stationary for 2 seconds, move in reverse 2 seconds and repeat. The third robot uses only frc::DifferentialDrivePoseEstimator and UpdatewithTime without AddVisionMeasurement. The fourth robot uses both frc::DifferentialDriveOdometry and frc::DifferentialDrivePoseEstimator, the pose from frc::DifferentialDriveOdometry is used as the pose to AddVisionMeasurement. The same basic code is run in Telop and Autonomous. The difference is that the Estimator objects are dynamically allocated in Autonomous and only reset in Telop. Autonomous functions correctly, the fourth robot in Telop does not work correctly.

The code I'm currently using with an Intel Realsense T265 as the vision measurement source behaves even worst than the example code. The code allocates a new Estimator objects when the robot is enabled. While the robot is stationary, the pose's rotation spins wildly. As soon as the robot starts moving, the values from the Estimator all become NaN.

To Reproduce Steps to reproduce the behavior:

  1. Code attached
  2. Deploy the code to a roborio
  3. Start Glass
  4. Start Driver Station
  5. Display the Field
  6. Enable the robot in Telop
  7. At this time everything is going as expected.
  8. When the robots start moving on the field, Disable the robot.
  9. Enable and Disable the robot randomly
  10. Eventually the fourth robot will have problems.

Expected behavior The call frc::DifferentialDrivePoseEstimator::ResetPosition should reset all values in the class so that previous calls to AddVisionMeasurment have no effect on results after the call to ResetPosition.

Screenshots If applicable, add screenshots to help explain your problem. GlassField

Desktop (please complete the following information):

Additional context Example Code Attached. Robot_h.txt

Robot_cpp.txt

pietroglyph commented 3 years ago

Ok, there are a few things to unpack here...

First off: you should not feed the estimated pose from DifferentialDriveOdometry to DifferentialDrivePoseEstimator; the pose estimator already does all of the odometry calculations in the predict step when you call Update or UpdateWithTime. In fact, if you never call AddVisionMeasurement on DifferentialDrivePoseEstimator, you'll get pretty much the same behavior as you would if you were just using DifferentialDriveOdometry in its place. AddVisionMeasurement really is intended only for pose global pose measurements from non-encoder sources, like vision or VSLAM.

ResetPosition likely has the behavior you observe because it resets the state estimate but not the error covariance. This is a bug. I'll PR a fix shortly.

webbbt456 commented 3 years ago

This was just sample code to expose the bug. We are using a Intel Realsense T265 camera for VSLAM.