PoseeEstimation pose is teleporting to a random spot after losing sight of apriltag

[daniel.y]

We are displaying robot pose on AdvantageScope tool, and when the limelight vision camera is lined up with an aprilTag, we get the correct robot pose, but once we lose sight of the aprilTag and rotate away from it, the robot pose on AdvantageScope teleports randomly to another position.

We think that the odometry based position is not being updated correctly when we use the limelight with pose. How can we fix this issue?

Here is the vision code:

package common.Util;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.subsystems.SwerveDrive;

public class PhoseStuff implements Runnable {
    private SwerveDrive swerveDrive;
    private Vision vision;
    private SwerveDrivePoseEstimator poseEstimator;
    private Field2d fieldSim;

    public PhoseStuff(SwerveDrive swerveDrive, Vision vision) {
        this.swerveDrive = swerveDrive;
        this.vision = vision;

        // Creates the pose estimator.
        this.poseEstimator = new SwerveDrivePoseEstimator(
                swerveDrive.getKin(),
                swerveDrive.getGyroRotation2d(),
                swerveDrive.getSwerveModulePositions(),
                new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)),
                VecBuilder.fill(.1, .1, .1),
                VecBuilder.fill(.9, .9, .9));

        this.fieldSim = new Field2d();
        SmartDashboard.putData("Field", fieldSim);
    }

    @Override
    public void run() {
        tick();
    }

    // This is ran every tick.
    public void tick() {
        
        // double random = Math.random();
        // if (random > .9){
        //     System.out.println("numtags: " + vision.getNumTags());
        //     System.out.println("distance: " + vision.getDistance());
        // }

        // Updates the pose estimator based on the gyro and swerve module positions.
        poseEstimator.update(
                swerveDrive.getGyroRotation2d(),
                swerveDrive.getSwerveModulePositions());

        if(Math.random() > .9){
            System.out.println("Pose: " + get());
        }
        // Vision calculations are ran here.
        visionStuff();

        fieldSim.setRobotPose(get());
    }

    // Gets the pose estimate.
    public Pose2d get() {
        return poseEstimator.getEstimatedPosition();
    }

    // Resets the pose estimator.
    public void reset(Pose2d pose) {
        poseEstimator.resetPosition(
                swerveDrive.getGyroRotation2d(),
                swerveDrive.getSwerveModulePositions(),
                pose);

         fieldSim.setRobotPose(poseEstimator.getEstimatedPosition());
    }

    // Vision calculations are ran here.
    public void visionStuff() {
        Pose2d visionResult = vision.getPose();
        double visionTimeStamp = vision.getTimestamp();

        if ((vision.getNumTags() == 1 && vision.getDistance() < 75.00) ||
            (vision.getNumTags() == 2 && vision.getDistance() < 120.00) ||
            (vision.getNumTags() == 3 && vision.getDistance() < 185.00)) 
        {
            if(Math.random() > .9){
                System.out.println("Pushing");
            }
            poseEstimator.addVisionMeasurement(visionResult, visionTimeStamp);
        }        
    }
}

[JaiCodes]

The 4th parameter in the constructor for SwerveDrivePoseEstimatior is the default pose if an AprilTag is not in vision.

[daniel.y]

Would you mind elaborating more?

[marshall]

I don’t have much to add but this almost sounds like the Kidnapped robot problem - Wikipedia

[gdefender]

@JaiCodes

The 4th parameter in the constructor for SwerveDrivePoseEstimatior is the default pose if an AprilTag is not in vision.

This is not correct. The fourth parameter, initialPose, is the starting pose of the robot at the time you’re instantiating SwerveDrivePoseEstimator.

[gdefender]

Does the robot teleport to a random pose, or does it teleport to the same spot every time there is no tag?
I’m guessing it goes to (0, 0, 0°). You haven’t shared enough of your code to know what vision.getPose() and vision.getTimestamp() return when there is no tag visible. Unless the pose falls outside of your if block in visionStuff, or your accurately calculating the timestamp (which Limelight Lib does not do) it’s going into the poseEstimator.addVisionMeasurement method as the current pose.

You probably just need to check if there was a tag visible before calling addVisionMeasurement.

[UnofficialForth]

While I agree that this is probably the case, this wouldn’t necessarily fix the issue. If a camera processor doesn’t pass all data at the same time stamp, it can throw a false positive.

Time Stamp	Location	Is Visible
0001	(5, 5, 5)	True
0002	(5.2, 4.8, 5)	True
0003	(0,0,0)	False
0004	(0,0,0)	False

If the “visibility” is grabbed from 0002 but the location is grabbed from 0003, then your robot thinks it knows where it is but updates with an incorrect position. As of last year, Limelights didn’t have default synced data, you had to get it from a json dump if you wanted to guarantee it was from the same time stamp. I’ve heard Photonvision by default grabs from the same timestamp. Don’t know about other programs.

[gdefender]

This is a good point. There isn’t enough of the code included to know. We don’t know if they are getting the pose and timestamp from the same frame, and we don’t know if they’re properly calculating the timestamp.

@daniel.y If you can’t solve this and you’d like more help, please share your entire codebase, ideally in a GitHub repo. Thanks!