Closed webbbt456 closed 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.
This was just sample code to expose the bug. We are using a Intel Realsense T265 camera for VSLAM.
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:
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.
Desktop (please complete the following information):
Additional context Example Code Attached. Robot_h.txt
Robot_cpp.txt