Skip to content

vision angle std fix -> counter master #147

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
DanielleCarmi09 wants to merge 8 commits into
base: counter-master
Choose a base branch
from
Open

vision angle std fix -> counter master #147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
550a6aa
angle std fix
DanielleCarmi09 Feb 12, 2026
d8e7cc2
CR
DanielleCarmi09 Feb 12, 2026
b79a0b6
test
DanielleCarmi09 Feb 12, 2026
0c7b0d4
CR
DanielleCarmi09 Feb 13, 2026
1a198fd
CR
DanielleCarmi09 Feb 13, 2026
0e7a31c
CR
DanielleCarmi09 Feb 13, 2026
7864b78
CR
DanielleCarmi09 Feb 13, 2026
78dd099
CR
DanielleCarmi09 Feb 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,6 @@ public class WPILibPoseEstimatorConstants {

public static final StandardDeviations2D DEFAULT_VISION_STD_DEV = new StandardDeviations2D(0.0003, 0.0003, 0.003);

public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D();

public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2;

public static final Rotation2d MINIMUM_TILT_IMU_ROLL = Rotation2d.fromDegrees(4);
Expand All @@ -29,6 +27,8 @@ public class WPILibPoseEstimatorConstants {

public static final double SKID_ODOMETRY_ACCURACY_REDUCTION_FACTOR = 0;

public static final double CALIBRATED_IMU_OFFSET_VISION_ANGLE_STD_DEVS_ADDITION = 0;

/**
* constant represents the addition to odometryAccuracy when the compensated vision X and Y StdDevs average is equal to one
*/
Expand Down
27 changes: 23 additions & 4 deletions src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -229,11 +229,16 @@ private void updateOdometryAccuracy(OdometryData data) {
}

private void updateVision(RobotPoseObservation visionRobotPoseObservation) {
addVisionMeasurement(visionRobotPoseObservation);
RobotPoseObservation compensatedVisionObservation = new RobotPoseObservation(
visionRobotPoseObservation.timestampSeconds(),
visionRobotPoseObservation.robotPose(),
compensateByPoseEstimatorConditions(visionRobotPoseObservation.stdDevs())
);
addVisionMeasurement(compensatedVisionObservation);

getEstimatedPoseToIMUYawDifference(
imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()),
visionRobotPoseObservation.timestampSeconds()
imuYawBuffer.getSample(compensatedVisionObservation.timestampSeconds()),
compensatedVisionObservation.timestampSeconds()
).ifPresent(yawDifference -> {
poseToIMUYawDifferenceBuffer.insert(yawDifference);

Expand All @@ -247,11 +252,25 @@ private void addVisionMeasurement(RobotPoseObservation visionObservation) {
poseEstimator.addVisionMeasurement(
visionObservation.robotPose(),
visionObservation.timestampSeconds(),
compensateByOdometryAccuracy(visionObservation.stdDevs()).asColumnVector()
visionObservation.stdDevs().asColumnVector()
);
this.lastVisionObservation = visionObservation;
}

private StandardDeviations2D compensateByPoseEstimatorConditions(StandardDeviations2D visionStdDevs) {
return compensateByIsIMUOffsetCalibrated(compensateByOdometryAccuracy(visionStdDevs));
}

private StandardDeviations2D compensateByIsIMUOffsetCalibrated(StandardDeviations2D visionStdDevs) {
return new StandardDeviations2D(
visionStdDevs.xStandardDeviations(),
visionStdDevs.yStandardDeviations(),
isIMUOffsetCalibrated
? visionStdDevs.angleStandardDeviations() + WPILibPoseEstimatorConstants.CALIBRATED_IMU_OFFSET_VISION_ANGLE_STD_DEVS_ADDITION
: visionStdDevs.angleStandardDeviations()
);
}

private StandardDeviations2D compensateByOdometryAccuracy(StandardDeviations2D visionStdDevs) {
StandardDeviations2D compensatedStdDevs = new StandardDeviations2D(
visionStdDevs.xStandardDeviations() * odometryAccuracy,
Expand Down