Photonvision Multitag Constant Output Drift

[zhangv25]

Hi, we use a mecanum robot with pose estimation through Neos.
Our MecanumDrivePoseEstimator works, but this season, we are trying to use photonvision’s multitag system to correct for the drift on mecanums.
However, we do not know if we are updating the photonPoseEstimator right. We have created a PhotonPoseEstimator object according to the docs:

photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cam, robotToCam);

Then, we use the following code to update the drivetrain pose estimator:

    // Get target
    var res = cam.getLatestResult();
    if (res.hasTargets() && photonPoseEstimator.getReferencePose() != null){
        var imageCaptureTime = res.getTimestampSeconds();
        // Store camera estimated pose, and calculate it based on current drivetrain position
        photonPoseEstimator.setReferencePose(photonPoseEstimator.getReferencePose());
        photonPoseEstimator.update();
        Pose2d estimatedPose = photonPoseEstimator.getReferencePose().toPose2d();
        
        System.out.println("Estimated pose: " + estimatedPose);
        poseEstimator.addVisionMeasurement(new Pose2d(estimatedPose.getTranslation(), estimatedPose.getRotation()), imageCaptureTime);
    }

    System.out.println(poseEstimator.getEstimatedPosition());

We do not know if we are supposed to setReferencePose with the drivetrain reference pose, or if we are supposed to set it with the last camera estimated reference pose. This has caused our projected coordinate values to constantly increase every update.

TL, DR: how should we update our drivetrain pose estimator based on our vision pose estimator; what is photonPoseEstimator.setReferencePose; and what do standard deviations have to do with all of this (and how do we set it up)?

Thanks in advance!

[Quake]

You do not need to set the reference pose when using MULTI_TAG_PNP_ON_COPROCESSOR, nor should you get the reference pose as your estimated pose. This ‘reference pose’ is only used for the CLOSEST_TO_REFERENCE_POSE strategy.

[zhangv25]

Thank you. So I would only call an update for the photonPoseEstimator? Am I passing in the pose into the drivetrain correctly?