Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
+ accurate estimation, originally made by Team 1466.
Add a vision measurement to the SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with
@@ -123,6 +127,10 @@ loadScripts(document, 'script');
Add a vision measurement to the SwerveDrivePoseEstimator and update the SwerveIMU gyro reading with
the given timestamp of the vision measurement.
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
+ accurate estimation, originally made by Team 1466.
Updates the robot's position on the field using forward kinematics and integration of the pose over time. This
+ method automatically calculates the current time to calculate period (difference between two timestamps). The
+ period is used to calculate the change in distance from a velocity. This also takes in an angle parameter which is
+ used instead of the angular rate that is calculated from forward kinematics. This also takes in pitch and roll to
+ allow for more accurate pose estimation on angled surfaces using a rotation matrix.
+
+
Parameters:
+
gyroYaw - The yaw reported by the gyro.
+
pitch - The pitch reported by the gyro.
+
roll - The roll reported by the gyro.
+
modulePositions - The current position of all swerve modules. Please provide the positions in the same order
+ in which you instantiated your SwerveDriveKinematics.
Represents an odometry record. The record contains the inputs provided as well as the pose that was observed based
+ on these inputs, as well as the previous record and its inputs.
+
+
+
+
+
+
+
Field Summary
+
Fields
+
+
Modifier and Type
+
Field
+
Description
+
private final edu.wpi.first.math.geometry.Rotation2d
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
+ accurate estimation, originally made by Team 1466.
Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
+
+
The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+ and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9
+ meters for y, and 0.9 radians for heading.
+
+
Parameters:
+
kinematics - A correctly-configured kinematics object for your drivetrain.
+
gyroAngle - The current gyro angle.
+
modulePositions - The current distance measurements and rotations of the swerve modules.
kinematics - A correctly-configured kinematics object for your drivetrain.
+
gyroAngle - The current gyro angle.
+
modulePositions - The current distance and rotation measurements of the swerve modules.
+
initialPoseMeters - The starting pose estimate.
+
stateStdDevs - Standard deviations of the pose estimate (x position in meters, y position in
+ meters, and heading in radians). Increase these numbers to trust your state
+ estimate less.
+
visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y
+ position in meters, and heading in radians). Increase these numbers to trust the
+ vision pose measurement less.
Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements
+ after the autonomous period, or to change trust as distance to a vision target increases.
+
+
Parameters:
+
visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust
+ global measurements from vision less. This matrix is in the form [x, y, theta]áµ€,
+ with units in meters and radians.
Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
+ for measurement noise.
+
+
This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.
+
+
To promote stability of the pose estimate and make it robust to bad vision data, we
+ recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
+
+
Parameters:
+
visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
+
timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your
+ own time source by calling
+ SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]) then you must use a timestamp with an epoch since FPGA
+ startup (i.e., the epoch of this timestamp is the same epoch as
+ Timer.getFPGATimestamp().) This means that you should
+ use Timer.getFPGATimestamp() as your time source or sync
+ the epochs.
Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
+ for measurement noise.
+
+
This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.
+
+
To promote stability of the pose estimate and make it robust to bad vision data, we
+ recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
+
+
Note that the vision measurement standard deviations passed into this method will continue
+ to apply to future measurements until a subsequent call to
+ SwerveDrivePoseEstimator.setVisionMeasurementStdDevs(Matrix) or this method.
+
+
Parameters:
+
visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
+
timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your
+ own time source by calling
+ SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]), then you must use a timestamp with an epoch since FPGA
+ startup (i.e., the epoch of this timestamp is the same epoch as
+ Timer.getFPGATimestamp()). This means that you should
+ use Timer.getFPGATimestamp() as your time source in
+ this case.
+
visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y
+ position in meters, and heading in radians). Increase these numbers to trust the
+ vision pose measurement less.
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
+ accurate estimation, originally made by Team 1466.
diff --git a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
index 4d6d2e9..aeed7c9 100644
--- a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
+++ b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
@@ -1,7 +1,7 @@
-
+
SparkMaxBrushedMotorSwerve
diff --git a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
index c91ea84..31d7c33 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
@@ -1,7 +1,7 @@
-
+
SparkMaxSwerve.SparkMAX_slotIdx
diff --git a/docs/swervelib/motors/SparkMaxSwerve.html b/docs/swervelib/motors/SparkMaxSwerve.html
index 55cc0d3..93bd318 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.html
@@ -1,7 +1,7 @@
-
+
SparkMaxSwerve
diff --git a/docs/swervelib/motors/SwerveMotor.html b/docs/swervelib/motors/SwerveMotor.html
index 757b059..fd87ef8 100644
--- a/docs/swervelib/motors/SwerveMotor.html
+++ b/docs/swervelib/motors/SwerveMotor.html
@@ -1,7 +1,7 @@
-
+
SwerveMotor
diff --git a/docs/swervelib/motors/TalonFXSwerve.html b/docs/swervelib/motors/TalonFXSwerve.html
index b2b2861..6cc0c55 100644
--- a/docs/swervelib/motors/TalonFXSwerve.html
+++ b/docs/swervelib/motors/TalonFXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonFXSwerve
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index f18d31b..2422d4c 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSwerve
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index 14b1f7c..5f9c073 100644
--- a/docs/swervelib/motors/package-summary.html
+++ b/docs/swervelib/motors/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors
diff --git a/docs/swervelib/motors/package-tree.html b/docs/swervelib/motors/package-tree.html
index 207af10..4840c3a 100644
--- a/docs/swervelib/motors/package-tree.html
+++ b/docs/swervelib/motors/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors Class Hierarchy
diff --git a/docs/swervelib/package-summary.html b/docs/swervelib/package-summary.html
index 376c6ad..25cd61d 100644
--- a/docs/swervelib/package-summary.html
+++ b/docs/swervelib/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib
diff --git a/docs/swervelib/package-tree.html b/docs/swervelib/package-tree.html
index efdb3cd..d13324c 100644
--- a/docs/swervelib/package-tree.html
+++ b/docs/swervelib/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib Class Hierarchy
diff --git a/docs/swervelib/parser/PIDFConfig.html b/docs/swervelib/parser/PIDFConfig.html
index fdd116b..f84e7aa 100644
--- a/docs/swervelib/parser/PIDFConfig.html
+++ b/docs/swervelib/parser/PIDFConfig.html
@@ -1,7 +1,7 @@
-
+
PIDFConfig
diff --git a/docs/swervelib/parser/SwerveControllerConfiguration.html b/docs/swervelib/parser/SwerveControllerConfiguration.html
index fb24cee..dfd754d 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveControllerConfiguration
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index 0e4d302..af6aee1 100644
--- a/docs/swervelib/parser/SwerveDriveConfiguration.html
+++ b/docs/swervelib/parser/SwerveDriveConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveConfiguration
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index e7dbf45..e58520d 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleConfiguration
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index 7e6d68c..2211d64 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,7 +1,7 @@
-
+
SwerveModulePhysicalCharacteristics
diff --git a/docs/swervelib/parser/SwerveParser.html b/docs/swervelib/parser/SwerveParser.html
index 18287bd..44ce32a 100644
--- a/docs/swervelib/parser/SwerveParser.html
+++ b/docs/swervelib/parser/SwerveParser.html
@@ -1,7 +1,7 @@
-
+
SwerveParser
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index 025b276..9b9f84a 100644
--- a/docs/swervelib/parser/deserializer/PIDFRange.html
+++ b/docs/swervelib/parser/deserializer/PIDFRange.html
@@ -1,7 +1,7 @@
-
+
PIDFRange
diff --git a/docs/swervelib/parser/deserializer/package-summary.html b/docs/swervelib/parser/deserializer/package-summary.html
index 9a834d3..572e014 100644
--- a/docs/swervelib/parser/deserializer/package-summary.html
+++ b/docs/swervelib/parser/deserializer/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer
diff --git a/docs/swervelib/parser/deserializer/package-tree.html b/docs/swervelib/parser/deserializer/package-tree.html
index ab52627..2ebdd56 100644
--- a/docs/swervelib/parser/deserializer/package-tree.html
+++ b/docs/swervelib/parser/deserializer/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer Class Hierarchy
diff --git a/docs/swervelib/parser/json/ControllerPropertiesJson.html b/docs/swervelib/parser/json/ControllerPropertiesJson.html
index 4264d01..3b7dff8 100644
--- a/docs/swervelib/parser/json/ControllerPropertiesJson.html
+++ b/docs/swervelib/parser/json/ControllerPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
ControllerPropertiesJson
diff --git a/docs/swervelib/parser/json/DeviceJson.html b/docs/swervelib/parser/json/DeviceJson.html
index 63e8c20..a82d44f 100644
--- a/docs/swervelib/parser/json/DeviceJson.html
+++ b/docs/swervelib/parser/json/DeviceJson.html
@@ -1,7 +1,7 @@
-
+
DeviceJson
diff --git a/docs/swervelib/parser/json/ModuleJson.html b/docs/swervelib/parser/json/ModuleJson.html
index 301dcae..af29591 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,7 +1,7 @@
-
+
ModuleJson
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index db9d818..343f609 100644
--- a/docs/swervelib/parser/json/MotorConfigDouble.html
+++ b/docs/swervelib/parser/json/MotorConfigDouble.html
@@ -1,7 +1,7 @@
-
+
MotorConfigDouble
diff --git a/docs/swervelib/parser/json/MotorConfigInt.html b/docs/swervelib/parser/json/MotorConfigInt.html
index 798b52e..23d29cb 100644
--- a/docs/swervelib/parser/json/MotorConfigInt.html
+++ b/docs/swervelib/parser/json/MotorConfigInt.html
@@ -1,7 +1,7 @@
-
+
MotorConfigInt
diff --git a/docs/swervelib/parser/json/PIDFPropertiesJson.html b/docs/swervelib/parser/json/PIDFPropertiesJson.html
index a71a782..3d1b4a0 100644
--- a/docs/swervelib/parser/json/PIDFPropertiesJson.html
+++ b/docs/swervelib/parser/json/PIDFPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PIDFPropertiesJson
diff --git a/docs/swervelib/parser/json/PhysicalPropertiesJson.html b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
index 375e419..4a1e503 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PhysicalPropertiesJson
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index c502379..d8b2be0 100644
--- a/docs/swervelib/parser/json/SwerveDriveJson.html
+++ b/docs/swervelib/parser/json/SwerveDriveJson.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveJson
diff --git a/docs/swervelib/parser/json/modules/BoolMotorJson.html b/docs/swervelib/parser/json/modules/BoolMotorJson.html
index 17c1432..983749c 100644
--- a/docs/swervelib/parser/json/modules/BoolMotorJson.html
+++ b/docs/swervelib/parser/json/modules/BoolMotorJson.html
@@ -1,7 +1,7 @@
-
+
BoolMotorJson
diff --git a/docs/swervelib/parser/json/modules/LocationJson.html b/docs/swervelib/parser/json/modules/LocationJson.html
index f98bd3c..96c4830 100644
--- a/docs/swervelib/parser/json/modules/LocationJson.html
+++ b/docs/swervelib/parser/json/modules/LocationJson.html
@@ -1,7 +1,7 @@
-
+
LocationJson
diff --git a/docs/swervelib/parser/json/modules/package-summary.html b/docs/swervelib/parser/json/modules/package-summary.html
index da4a656..bd80b31 100644
--- a/docs/swervelib/parser/json/modules/package-summary.html
+++ b/docs/swervelib/parser/json/modules/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json.modules
diff --git a/docs/swervelib/parser/json/modules/package-tree.html b/docs/swervelib/parser/json/modules/package-tree.html
index 47a8d13..781d8bf 100644
--- a/docs/swervelib/parser/json/modules/package-tree.html
+++ b/docs/swervelib/parser/json/modules/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json.modules Class Hierarchy
diff --git a/docs/swervelib/parser/json/package-summary.html b/docs/swervelib/parser/json/package-summary.html
index c7fc94f..d142da6 100644
--- a/docs/swervelib/parser/json/package-summary.html
+++ b/docs/swervelib/parser/json/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json
diff --git a/docs/swervelib/parser/json/package-tree.html b/docs/swervelib/parser/json/package-tree.html
index 1bf9224..87e5c0b 100644
--- a/docs/swervelib/parser/json/package-tree.html
+++ b/docs/swervelib/parser/json/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json Class Hierarchy
diff --git a/docs/swervelib/parser/package-summary.html b/docs/swervelib/parser/package-summary.html
index ff7d729..2d8c845 100644
--- a/docs/swervelib/parser/package-summary.html
+++ b/docs/swervelib/parser/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index 4a33daf..9a424b5 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser Class Hierarchy
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index e373b40..20f01f9 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveIMUSimulation
diff --git a/docs/swervelib/simulation/SwerveModuleSimulation.html b/docs/swervelib/simulation/SwerveModuleSimulation.html
index f7e368c..50915d0 100644
--- a/docs/swervelib/simulation/SwerveModuleSimulation.html
+++ b/docs/swervelib/simulation/SwerveModuleSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleSimulation
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
index ba9597d..b5baa95 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim.SimProfile
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.html b/docs/swervelib/simulation/ctre/PhysicsSim.html
index f636626..eafb3e4 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim
diff --git a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
index 29cb582..6053d1e 100644
--- a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonFXSimProfile
diff --git a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
index 0144707..ecec8a0 100644
--- a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSimProfile
diff --git a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
index 86a59bb..3b982fc 100644
--- a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
@@ -1,7 +1,7 @@
-
+
VictorSPXSimProfile
diff --git a/docs/swervelib/simulation/ctre/package-summary.html b/docs/swervelib/simulation/ctre/package-summary.html
index fd8dc07..78f6430 100644
--- a/docs/swervelib/simulation/ctre/package-summary.html
+++ b/docs/swervelib/simulation/ctre/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre
diff --git a/docs/swervelib/simulation/ctre/package-tree.html b/docs/swervelib/simulation/ctre/package-tree.html
index 839df8a..ab5f08e 100644
--- a/docs/swervelib/simulation/ctre/package-tree.html
+++ b/docs/swervelib/simulation/ctre/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre Class Hierarchy
diff --git a/docs/swervelib/simulation/package-summary.html b/docs/swervelib/simulation/package-summary.html
index 9b0982a..1ce0161 100644
--- a/docs/swervelib/simulation/package-summary.html
+++ b/docs/swervelib/simulation/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation
diff --git a/docs/swervelib/simulation/package-tree.html b/docs/swervelib/simulation/package-tree.html
index bec01a4..04b3705 100644
--- a/docs/swervelib/simulation/package-tree.html
+++ b/docs/swervelib/simulation/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation Class Hierarchy
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
index 4c405da..bc9a0c3 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry.TelemetryVerbosity
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
index 95309ae..ec96133 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index 0019313..96f31fd 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index eda414b..b799c24 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry Class Hierarchy
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index 8c42835..fd1ed39 100644
--- a/docs/type-search-index.js
+++ b/docs/type-search-index.js
@@ -1 +1 @@
-typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveKinematics2"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.math","l":"SwerveModuleState2"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
+typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.math","l":"SwervePoseEstimator2.InterpolationRecord"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.math","l":"SwerveDriveOdometry2"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveKinematics2"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.math","l":"SwerveModuleState2"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.math","l":"SwervePoseEstimator2"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index a5896a5..0857150 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -28,6 +28,7 @@ import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
import swervelib.math.SwerveModuleState2;
+import swervelib.math.SwervePoseEstimator2;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
@@ -51,7 +52,7 @@ public class SwerveDrive
/**
* Swerve odometry.
*/
- public final SwerveDrivePoseEstimator swerveDrivePoseEstimator;
+ public final SwervePoseEstimator2 swerveDrivePoseEstimator;
/**
* Swerve modules.
*/
@@ -129,7 +130,7 @@ public class SwerveDrive
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
- new SwerveDrivePoseEstimator(
+ new SwervePoseEstimator2(
kinematics,
getYaw(),
getModulePositions(),
@@ -685,7 +686,7 @@ public class SwerveDrive
public void updateOdometry()
{
// Update odometry
- swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
+ swerveDrivePoseEstimator.update(getYaw(), getPitch(), getRoll(), getModulePositions());
// Update angle accumulator if the robot is simulated
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
diff --git a/swervelib/math/SwerveDriveOdometry2.java b/swervelib/math/SwerveDriveOdometry2.java
new file mode 100644
index 0000000..8cc71cd
--- /dev/null
+++ b/swervelib/math/SwerveDriveOdometry2.java
@@ -0,0 +1,246 @@
+package swervelib.math;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Clone of {@link SwerveDriveOdometry} except uses gyro pitch and roll to achieve a more accurate estimation.
+ * Originally made by Team 1466.
+ */
+public class SwerveDriveOdometry2 extends SwerveDriveOdometry
+{
+
+ /**
+ * Swerve drive kinematics.
+ */
+ private final SwerveDriveKinematics m_kinematics;
+ /**
+ * Number of swerve modules.
+ */
+ private final int m_numModules;
+ /**
+ * Previous swerve module positions.
+ */
+ private final SwerveModulePosition[] m_previousModulePositions;
+ /**
+ * Zero module states.
+ */
+ private final SwerveModuleState[] m_zeroModuleStates;
+ /**
+ * Estimated pose.
+ */
+ private Pose2d m_poseMeters;
+ /**
+ * Gyro offset.
+ */
+ private Rotation2d m_gyroOffset;
+ /**
+ * Previous gyroscope angle.
+ */
+ private Rotation2d m_previousAngle;
+
+ /**
+ * Constructs a SwerveDriveOdometry object.
+ *
+ * @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The wheel positions reported by each module.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ public SwerveDriveOdometry2(
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPose)
+ {
+ super(kinematics, gyroAngle, modulePositions, initialPose);
+ m_kinematics = kinematics;
+ m_poseMeters = initialPose;
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ m_previousAngle = initialPose.getRotation();
+ m_numModules = modulePositions.length;
+
+ m_previousModulePositions = new SwerveModulePosition[m_numModules];
+ for (int index = 0; index < m_numModules; index++)
+ {
+ m_previousModulePositions[index] =
+ new SwerveModulePosition(
+ modulePositions[index].distanceMeters, modulePositions[index].angle);
+ }
+ m_zeroModuleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds(1, 0, 0));
+
+ MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
+ }
+
+ /**
+ * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+ *
+ * @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The wheel positions reported by each module.
+ */
+ public SwerveDriveOdometry2(
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions)
+ {
+ this(kinematics, gyroAngle, modulePositions, new Pose2d());
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ *
The gyroscope angle does not need to be reset here on the user's robot code. The library
+ * automatically takes care of offsetting the gyro angle.
+ *
+ *
Similarly, module positions do not need to be reset in user code.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The wheel positions reported by each module.,
+ * @param pose The position on the field that your robot is at.
+ */
+ public void resetPosition(
+ Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose)
+ {
+ super.resetPosition(gyroAngle, modulePositions, pose);
+ if (modulePositions.length != m_numModules)
+ {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+
+ m_poseMeters = pose;
+ m_previousAngle = pose.getRotation();
+ m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+ for (int index = 0; index < m_numModules; index++)
+ {
+ m_previousModulePositions[index] =
+ new SwerveModulePosition(
+ modulePositions[index].distanceMeters, modulePositions[index].angle);
+ }
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ *
+ * @return The pose of the robot (x and y are in meters).
+ */
+ public Pose2d getPoseMeters()
+ {
+ return m_poseMeters;
+ }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and integration of the pose over time. This
+ * method automatically calculates the current time to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This also takes in an angle parameter which is
+ * used instead of the angular rate that is calculated from forward kinematics. This also takes in pitch and roll to
+ * allow for more accurate pose estimation on angled surfaces using a rotation matrix.
+ *
+ * @param gyroYaw The yaw reported by the gyro.
+ * @param pitch The pitch reported by the gyro.
+ * @param roll The roll reported by the gyro.
+ * @param modulePositions The current position of all swerve modules. Please provide the positions in the same order
+ * in which you instantiated your SwerveDriveKinematics.
+ * @return The new pose of the robot.
+ */
+ public Pose2d update(
+ Rotation2d gyroYaw,
+ Rotation2d pitch,
+ Rotation2d roll,
+ SwerveModulePosition[] modulePositions)
+ {
+ super.update(gyroYaw, modulePositions);
+ if (modulePositions.length != m_numModules)
+ {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+
+ var moduleDeltas = new SwerveModulePosition[m_numModules];
+ var yaw = gyroYaw.plus(m_gyroOffset);
+
+ var rotationMatrix = new SimpleMatrix(3, 3);
+ var gyroZero = new Rotation2d(); // doing dist calcs robot relative
+ rotationMatrix.setRow(
+ 0,
+ 0,
+ gyroZero.getCos() * pitch.getCos(),
+ gyroZero.getCos() * pitch.getSin() * roll.getSin() - gyroZero.getSin() * roll.getCos(),
+ gyroZero.getCos() * pitch.getSin() * roll.getCos() + gyroZero.getSin() * roll.getSin());
+
+ rotationMatrix.setRow(
+ 1,
+ 0,
+ gyroZero.getSin() * pitch.getCos(),
+ gyroZero.getSin() * pitch.getSin() * roll.getSin() + gyroZero.getCos() * roll.getCos(),
+ gyroZero.getSin() * pitch.getSin() * roll.getCos() - gyroZero.getCos() * roll.getSin());
+
+ rotationMatrix.setRow(
+ 2, 0, -pitch.getSin(), pitch.getCos() * roll.getSin(), pitch.getCos() * roll.getCos());
+
+ for (int index = 0; index < m_numModules; index++)
+ {
+ var current = modulePositions[index];
+ var previous = m_previousModulePositions[index];
+
+ var deltaDistanceInitial = current.distanceMeters - previous.distanceMeters;
+ var changedAngle =
+ current.angle.minus(
+ m_zeroModuleStates[index]
+ .angle); // does this return (-pi, pi)? shoudln't matter since Twist2d does sin
+ // cos
+
+ var deltaMatrix = new SimpleMatrix(3, 1);
+ deltaMatrix.setColumn(
+ 0,
+ 0,
+ changedAngle.getCos() * deltaDistanceInitial,
+ changedAngle.getSin() * deltaDistanceInitial,
+ 0);
+
+ var rotatedDeltaMatrix = rotationMatrix.mult(deltaMatrix);
+ var finalDeltaDistance =
+ deltaDistanceInitial >= 0
+ ? Math.sqrt(
+ Math.pow(rotatedDeltaMatrix.get(0, 0), 2)
+ + Math.pow(rotatedDeltaMatrix.get(1, 0), 2))
+ : -Math.sqrt(
+ Math.pow(rotatedDeltaMatrix.get(0, 0), 2)
+ + Math.pow(rotatedDeltaMatrix.get(1, 0), 2));
+
+ var deltaDistance =
+ (roll.getDegrees() > 10 || pitch.getDegrees() > 10)
+ ? finalDeltaDistance
+ : deltaDistanceInitial;
+ var updatedAngle =
+ (roll.getDegrees() > 10 || pitch.getDegrees() > 10)
+ ? Rotation2d.fromRadians(
+ Math.atan2(rotatedDeltaMatrix.get(0, 0), rotatedDeltaMatrix.get(1, 0)))
+ : current.angle;
+
+ moduleDeltas[index] = new SwerveModulePosition(deltaDistance, updatedAngle);
+ previous.distanceMeters = current.distanceMeters;
+ }
+
+ var twist = m_kinematics.toTwist2d(moduleDeltas);
+ twist.dtheta = yaw.minus(m_previousAngle).getRadians();
+
+ var newPose = m_poseMeters.exp(twist);
+
+ m_previousAngle = yaw;
+ m_poseMeters = new Pose2d(newPose.getTranslation(), yaw);
+
+ return m_poseMeters;
+ }
+}
diff --git a/swervelib/math/SwervePoseEstimator2.java b/swervelib/math/SwervePoseEstimator2.java
new file mode 100644
index 0000000..332756c
--- /dev/null
+++ b/swervelib/math/SwervePoseEstimator2.java
@@ -0,0 +1,459 @@
+package swervelib.math;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+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.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.util.Arrays;
+import java.util.Map;
+import java.util.Objects;
+
+/**
+ * Clone of {@link SwerveDrivePoseEstimator} which takes into account gyroscope pitch and roll to achieve a more
+ * accurate estimation, originally made by Team 1466.
+ */
+public class SwervePoseEstimator2 // extends SwerveDrivePoseEstimator
+{
+
+ /**
+ * Swerve drive kinematics.
+ */
+ private final SwerveDriveKinematics m_kinematics;
+ /**
+ * Enhanced swerve drive odometry.
+ */
+ private final SwerveDriveOdometry2 m_odometry;
+ /**
+ * Matrix quotient.
+ */
+ private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1());
+ /**
+ * Number of swerve modules.
+ */
+ private final int m_numModules;
+ /**
+ * Interpolation buffer.
+ */
+ private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5);
+ /**
+ * Vision standard deviations.
+ */
+ private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+ /**
+ * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
+ *
+ *
The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+ * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9
+ * meters for y, and 0.9 radians for heading.
+ *
+ * @param kinematics A correctly-configured kinematics object for your drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param initialPoseMeters The starting pose estimate.
+ */
+ public SwervePoseEstimator2(
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters)
+ {
+ this(
+ kinematics,
+ gyroAngle,
+ modulePositions,
+ initialPoseMeters,
+ VecBuilder.fill(0.1, 0.1, 0.1),
+ VecBuilder.fill(0.9, 0.9, 0.9));
+ }
+
+ /**
+ * Constructs a SwerveDrivePoseEstimator.
+ *
+ * @param kinematics A correctly-configured kinematics object for your drivetrain.
+ * @param gyroAngle The current gyro angle.
+ * @param modulePositions The current distance and rotation measurements of the swerve modules.
+ * @param initialPoseMeters The starting pose estimate.
+ * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position in
+ * meters, and heading in radians). Increase these numbers to trust your state
+ * estimate less.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y
+ * position in meters, and heading in radians). Increase these numbers to trust the
+ * vision pose measurement less.
+ */
+ public SwervePoseEstimator2(
+ SwerveDriveKinematics kinematics,
+ Rotation2d gyroAngle,
+ SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters,
+ Matrix stateStdDevs,
+ Matrix visionMeasurementStdDevs)
+ {
+// super(kinematics, gyroAngle, modulePositions, initialPoseMeters);
+ m_kinematics = kinematics;
+ m_odometry =
+ new SwerveDriveOdometry2(kinematics, gyroAngle, modulePositions, initialPoseMeters);
+
+ for (int i = 0; i < 3; ++i)
+ {
+ m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+ }
+
+ m_numModules = modulePositions.length;
+
+ setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ }
+
+ /**
+ * Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements
+ * after the autonomous period, or to change trust as distance to a vision target increases.
+ *
+ * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these numbers to trust
+ * global measurements from vision less. This matrix is in the form [x, y, theta]ᵀ,
+ * with units in meters and radians.
+ */
+ public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs)
+ {
+// super.setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ var r = new double[3];
+ for (int i = 0; i < 3; ++i)
+ {
+ r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+ }
+
+ // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+ // and C = I. See wpimath/algorithms.md.
+ for (int row = 0; row < 3; ++row)
+ {
+ if (m_q.get(row, 0) == 0.0)
+ {
+ m_visionK.set(row, row, 0.0);
+ } else
+ {
+ m_visionK.set(
+ row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+ }
+ }
+ }
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ *
The gyroscope angle does not need to be reset in the user's robot code. The library
+ * automatically takes care of offsetting the gyro angle.
+ *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param poseMeters The position on the field that your robot is at.
+ */
+ public void resetPosition(
+ Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters)
+ {
+// super.resetPosition(gyroAngle, modulePositions, poseMeters);
+ // Reset state estimate and error covariance
+ m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
+ m_poseBuffer.clear();
+ }
+
+ /**
+ * Gets the estimated robot pose.
+ *
+ * @return The estimated robot pose in meters.
+ */
+ public Pose2d getEstimatedPosition()
+ {
+ return m_odometry.getPoseMeters();
+ }
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
+ * for measurement noise.
+ *
+ *
This method can be called as infrequently as you want, as long as you are calling {@link
+ * SwerveDrivePoseEstimator#update} every loop.
+ *
+ *
To promote stability of the pose estimate and make it robust to bad vision data, we
+ * recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
+ *
+ * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you don't use your
+ * own time source by calling
+ * {@link SwerveDrivePoseEstimator#updateWithTime(double, Rotation2d,
+ * SwerveModulePosition[])} then you must use a timestamp with an epoch since FPGA
+ * startup (i.e., the epoch of this timestamp is the same epoch as
+ * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should
+ * use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync
+ * the epochs.
+ */
+ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)
+ {
+// super.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+ // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+ var sample = m_poseBuffer.getSample(timestampSeconds);
+
+ if (sample.isEmpty())
+ {
+ return;
+ }
+
+ // Step 2: Measure the twist between the odometry pose and the vision pose.
+ var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+ // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+ // gain matrix representing how much we trust vision measurements compared to our current pose.
+ var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+ // Step 4: Convert back to Twist2d.
+ var scaledTwist =
+ new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+ // Step 5: Reset Odometry to state at sample with vision adjustment.
+ m_odometry.resetPosition(
+ sample.get().gyroAngle,
+ sample.get().modulePositions,
+ sample.get().poseMeters.exp(scaledTwist));
+
+ // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
+ // pose buffer and correct odometry.
+ for (Map.Entry entry :
+ m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet())
+ {
+ updateWithTime(
+ entry.getKey(),
+ entry.getValue().gyroAngle,
+ entry.getValue().gyroPitch,
+ entry.getValue().gyroRoll,
+ entry.getValue().modulePositions);
+ }
+ }
+
+ /**
+ * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
+ * for measurement noise.
+ *
+ *
This method can be called as infrequently as you want, as long as you are calling {@link
+ * SwerveDrivePoseEstimator#update} every loop.
+ *
+ *
To promote stability of the pose estimate and make it robust to bad vision data, we
+ * recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
+ *
+ *
Note that the vision measurement standard deviations passed into this method will continue
+ * to apply to future measurements until a subsequent call to
+ * {@link SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+ *
+ * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you don't use your
+ * own time source by calling
+ * {@link SwerveDrivePoseEstimator#updateWithTime(double, Rotation2d,
+ * SwerveModulePosition[])}, then you must use a timestamp with an epoch since FPGA
+ * startup (i.e., the epoch of this timestamp is the same epoch as
+ * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should
+ * use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
+ * this case.
+ * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y
+ * position in meters, and heading in radians). Increase these numbers to trust the
+ * vision pose measurement less.
+ */
+ public void addVisionMeasurement(
+ Pose2d visionRobotPoseMeters,
+ double timestampSeconds,
+ Matrix visionMeasurementStdDevs)
+ {
+ setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+ addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+ }
+
+ /**
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
+ *
+ * @param gyroAngle The current gyro angle.
+ * @param gyroPitch The current gyro pitch.
+ * @param gyroRoll The current gyro roll.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @return The estimated pose of the robot in meters.
+ */
+ public Pose2d update(
+ Rotation2d gyroAngle,
+ Rotation2d gyroPitch,
+ Rotation2d gyroRoll,
+ SwerveModulePosition[] modulePositions)
+ {
+ return updateWithTime(
+ WPIUtilJNI.now() * 1.0e-6, gyroAngle, gyroPitch, gyroRoll, modulePositions);
+ }
+
+ /**
+ * Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.
+ *
+ * @param currentTimeSeconds Time at which this method was called, in seconds.
+ * @param gyroAngle The current gyro angle.
+ * @param gyroPitch The current gyro pitch.
+ * @param gyroRoll The current gyro roll.
+ * @param modulePositions The current distance measurements and rotations of the swerve modules.
+ * @return The estimated pose of the robot in meters.
+ */
+ public Pose2d updateWithTime(
+ double currentTimeSeconds,
+ Rotation2d gyroAngle,
+ Rotation2d gyroPitch,
+ Rotation2d gyroRoll,
+ SwerveModulePosition[] modulePositions)
+ {
+// super.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions);
+ if (modulePositions.length != m_numModules)
+ {
+ throw new IllegalArgumentException(
+ "Number of modules is not consistent with number of wheel locations provided in "
+ + "constructor");
+ }
+
+ var internalModulePositions = new SwerveModulePosition[m_numModules];
+
+ for (int i = 0; i < m_numModules; i++)
+ {
+ internalModulePositions[i] =
+ new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
+ }
+
+ m_odometry.update(gyroAngle, gyroPitch, gyroRoll, internalModulePositions);
+
+ m_poseBuffer.addSample(
+ currentTimeSeconds,
+ new InterpolationRecord(
+ getEstimatedPosition(), gyroAngle, gyroPitch, gyroRoll, internalModulePositions));
+
+ return getEstimatedPosition();
+ }
+
+ /**
+ * Represents an odometry record. The record contains the inputs provided as well as the pose that was observed based
+ * on these inputs, as well as the previous record and its inputs.
+ */
+ private class InterpolationRecord implements Interpolatable
+ {
+
+ // The pose observed given the current sensor inputs and the previous pose.
+ private final Pose2d poseMeters;
+
+ // The current gyro angle.
+ private final Rotation2d gyroAngle;
+
+ // The current gyro pitch.
+ private final Rotation2d gyroPitch;
+
+ // The current gyro roll.
+ private final Rotation2d gyroRoll;
+
+ // The distances and rotations measured at each module.
+ private final SwerveModulePosition[] modulePositions;
+
+ /**
+ * Constructs an Interpolation Record with the specified parameters.
+ *
+ * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
+ * @param gyro The current gyro angle.
+ * @param gyroPitch The current gyro pitch.
+ * @param gyroRoll The current gyro roll.
+ * @param modulePositions The distances and rotations measured at each wheel.
+ */
+ private InterpolationRecord(
+ Pose2d poseMeters,
+ Rotation2d gyro,
+ Rotation2d gyroPitch,
+ Rotation2d gyroRoll,
+ SwerveModulePosition[] modulePositions)
+ {
+ this.poseMeters = poseMeters;
+ this.gyroAngle = gyro;
+ this.gyroPitch = gyroPitch;
+ this.gyroRoll = gyroRoll;
+ this.modulePositions = modulePositions;
+ }
+
+ /**
+ * Return the interpolated record. This object is assumed to be the starting position, or lower bound.
+ *
+ * @param endValue The upper bound, or end.
+ * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+ * @return The interpolated value.
+ */
+ @Override
+ public InterpolationRecord interpolate(InterpolationRecord endValue, double t)
+ {
+ if (t < 0)
+ {
+ return this;
+ } else if (t >= 1)
+ {
+ return endValue;
+ } else
+ {
+ // Find the new wheel distances.
+ var modulePositions = new SwerveModulePosition[m_numModules];
+
+ // Find the distance travelled between this measurement and the interpolated measurement.
+ var moduleDeltas = new SwerveModulePosition[m_numModules];
+
+ for (int i = 0; i < m_numModules; i++)
+ {
+ double ds =
+ MathUtil.interpolate(
+ this.modulePositions[i].distanceMeters,
+ endValue.modulePositions[i].distanceMeters,
+ t);
+ Rotation2d theta =
+ this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
+ modulePositions[i] = new SwerveModulePosition(ds, theta);
+ moduleDeltas[i] =
+ new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
+ }
+
+ // Find the new gyro angle.
+ var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+ var gyroPitch_lerp = gyroPitch.interpolate(endValue.gyroPitch, t);
+ var gyroRoll_lerp = gyroRoll.interpolate(endValue.gyroRoll, t);
+
+ // Create a twist to represent this change based on the interpolated sensor inputs.
+ Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
+ twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
+
+ return new InterpolationRecord(
+ poseMeters.exp(twist), gyro_lerp, gyroPitch_lerp, gyroRoll_lerp, modulePositions);
+ }
+ }
+
+ @Override
+ public boolean equals(Object obj)
+ {
+ if (this == obj)
+ {
+ return true;
+ }
+ if (!(obj instanceof InterpolationRecord))
+ {
+ return false;
+ }
+ InterpolationRecord record = (InterpolationRecord) obj;
+ return Objects.equals(gyroAngle, record.gyroAngle)
+ && Arrays.equals(modulePositions, record.modulePositions)
+ && Objects.equals(poseMeters, record.poseMeters);
+ }
+
+ @Override
+ public int hashCode()
+ {
+ return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
+ }
+ }
+}