Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

Browse Source 
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2022-12-02 11:36:10 -05:00
committed by GitHub
parent 68dba92630
commit e22d8cc343
35 changed files with 2288 additions and 1884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -12,12 +12,14 @@
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
* This class wraps an Unscented Kalman Filter to fuse latency-compensated
* This class wraps Differential Drive Odometry to fuse latency-compensated
* vision measurements with differential drive encoder measurements. It will
* correct for noisy vision measurements and encoder drift. It is intended to be
* an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
@@ -31,30 +33,22 @@ namespace frc {
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
* The state-space system used internally has the following states (x) and
* outputs (y):
*
* <strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate
* system containing x position, y position, heading, left encoder distance,
* and right encoder distance.
*
* <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
* and angular velocity in the field coordinate system.
*
* NB: Using velocities make things considerably easier, because it means that
* teams don't have to worry about getting an accurate model. Basically, we
* suspect that it's easier for teams to get good encoder data than it is for
* them to perform system identification well enough to get a good model.
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong>y = [dist_l, dist_r, theta] </strong>
* containing left encoder position, right encoder position, and gyro heading.
* position, and heading.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
public:
/**
* Constructs a DifferentialDrivePoseEstimator.
*
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param gyroAngle The gyro angle of the robot.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
@@ -65,28 +59,18 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* is in the form
* [x, y, theta, dist_l, dist_r]ᵀ,
* with units in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
* trust sensor readings from
* encoders and gyros less.
* This matrix is in the form
* [dist_l, dist_r, theta]ᵀ, with units in
* meters and radians.
* @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.
* @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose,
const wpi::array<double, 5>& stateStdDevs,
const wpi::array<double, 3>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms);
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -106,11 +90,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero). 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.
*
* @param gyroAngle The current gyro angle.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
@@ -120,15 +99,14 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
units::meter_t rightDistance, const Pose2d& pose);
/**
* Returns the pose of the robot at the current time as estimated by the
* Unscented Kalman Filter.
* Gets the estimated robot pose.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
* Adds a vision measurement to the Unscented Kalman Filter. This will correct
* 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
@@ -153,7 +131,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
units::second_t timestamp);
/**
* Adds a vision measurement to the Unscented Kalman Filter. This will correct
* 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
@@ -199,15 +177,13 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* Note that this should be called every loop iteration.
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
* @return The estimated pose of the robot.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance, units::meter_t rightDistance);
Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
/**
* Updates the Unscented Kalman Filter using only wheel encoder information.
@@ -215,7 +191,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The velocities of the wheels in meters per second.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*
@@ -223,27 +198,62 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds,
units::meter_t leftDistance,
units::meter_t rightDistance);
private:
UnscentedKalmanFilter<5, 3, 3> m_observer;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
Matrixd<3, 3> m_visionContR;
// The current gyro angle.
Rotation2d gyroAngle;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
// The distance traveled by the left encoder.
units::meter_t leftDistance;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
// The distance traveled by the right encoder.
units::meter_t rightDistance;
static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u);
static Vectord<5> FillStateVector(const Pose2d& pose,
units::meter_t leftDistance,
units::meter_t rightDistance);
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
InterpolationRecord endValue,
double i) const;
};
DifferentialDriveKinematics& m_kinematics;
DifferentialDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};
} // namespace frc

130
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -15,14 +15,15 @@
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/kinematics/MecanumDriveOdometry.h"
#include "units/time.h"
namespace frc {
/**
* This class wraps an Unscented Kalman Filter to fuse latency-compensated
* This class wraps Mecanum Drive Odometry to fuse latency-compensated
* vision measurements with mecanum drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an
* easy but more accurate drop-in for MecanumDriveOdometry.
* easy drop-in for MecanumDriveOdometry.
*
* Update() should be called every robot loop. If your loops are faster or
* slower than the default of 20 ms, then you should change the nominal delta
@@ -32,63 +33,43 @@ namespace frc {
* never call it, then this class will behave mostly like regular encoder
* odometry.
*
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
* The state-space system used internally has the following states (x) and
* outputs (y):
*
* <strong> x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> in the field
* coordinate system containing x position, y position, and heading, followed
* by the distance driven by the front left, front right, rear left, and rear
* right wheels.
*
* <strong> u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ </strong> containing
* x velocity, y velocity, and angular rate in the field coordinate system,
* followed by the velocity of the front left, front right, rear left, and rear
* right wheels.
* <strong> x = [x, y, theta]ᵀ </strong> in the field
* coordinate system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ
* </strong> containing gyro heading, followed by the distance driven by the
* front left, front right, rear left, and rear right wheels.
* position, and heading.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
public:
/**
* Constructs a MecanumDrivePoseEstimator.
*
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distance measured by each wheel.
* @param initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta, s_fl, s_fr,
* s_rl, s_rr]ᵀ, with units in meters and
* radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro
* measurement. Increase this number to trust
* sensor readings from the gyro less. This
* matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians,
* followed by meters.
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* @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.
* @param nominalDt The time in seconds between each robot
* loop.
*/
MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle,
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose, MecanumDriveKinematics kinematics,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms);
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -108,9 +89,6 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* IF wheelPositions are unspecified,
* You NEED to reset your encoders (to zero).
*
* 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.
*
@@ -123,15 +101,14 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const Pose2d& pose);
/**
* Gets the pose of the robot at the current time as estimated by the Extended
* Kalman Filter.
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct
* Add 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
@@ -156,7 +133,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
units::second_t timestamp);
/**
* Adds a vision measurement to the Unscented Kalman Filter. This will correct
* 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
@@ -198,48 +175,79 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
* Updates the the Kalman Filter using only wheel encoder
* information. This should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
* Updates the the Kalman Filter using only wheel encoder
* information. This should be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
private:
UnscentedKalmanFilter<7, 7, 5> m_observer;
MecanumDriveKinematics m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<7>& u, const Vectord<3>& y)> m_visionCorrect;
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
Eigen::Matrix3d m_visionContR;
// The current gyroscope angle.
Rotation2d gyroAngle;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
// The distances measured at each wheel.
MecanumDriveWheelPositions wheelPositions;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
InterpolationRecord endValue,
double i) const;
};
MecanumDriveKinematics& m_kinematics;
MecanumDriveOdometry m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};
} // namespace frc

354
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -4,143 +4,85 @@
#pragma once
#include <limits>
#include <cmath>
#include <fmt/format.h>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
#include "units/time.h"
namespace frc {
/**
* This class wraps an Unscented Kalman Filter to fuse latency-compensated
* vision measurements with swerve drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an
* easy but more accurate drop-in for SwerveDriveOdometry.
* This class wraps Swerve Drive Odometry to fuse latency-compensated
* vision measurements with swerve drive encoder distance measurements. It is
* intended to be a drop-in for SwerveDriveOdometry.
*
* Update() should be called every robot loop. If your loops are faster or
* slower than the default of 20 ms, then you should change the nominal delta
* time by specifying it in the constructor.
* Update() should be called every robot loop.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave mostly like regular encoder
* never call it, then this class will behave as regular encoder
* odometry.
*
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
* The state-space system used internally has the following states (x) and
* outputs (y):
*
* <strong> x = [x, y, theta, s_0, ..., s_n]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading, followed by the
* distance travelled by each wheel.
*
* <strong> u = [v_x, v_y, omega, v_0, ... v_n]ᵀ </strong> containing x
* velocity, y velocity, and angular velocity in the field coordinate system,
* followed by the velocity measured at each wheel.
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta, s_0, ..., s_n]ᵀ </strong>
* containing gyro heading, followed by the distance travelled by each wheel.
* position, and heading.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
public:
static constexpr size_t States = 3 + NumModules;
static constexpr size_t Inputs = 3 + NumModules;
static constexpr size_t Outputs = 1 + NumModules;
/**
* 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 initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta, s_0, ...
* s_n]ᵀ, with units in meters and radians, then meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement.
* Increase this number to trust sensor
* readings from the gyro less. This matrix is
* in the form [theta, s_0, ... s_n], with
* units in radians followed by meters.
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* @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.
* @param nominalDt The time in seconds between each robot
* loop.
*/
SwerveDrivePoseEstimator(
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, SwerveDriveKinematics<NumModules>& kinematics,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms)
: m_observer([](const Vectord<States>& x,
const Vectord<Inputs>& u) { return u; },
[](const Vectord<States>& x, const Vectord<Inputs>& u) {
return x.template block<States - 2, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs,
frc::AngleMean<States, States>(2),
frc::AngleMean<Outputs, States>(0),
frc::AngleResidual<States>(2),
frc::AngleResidual<Outputs>(0), frc::AngleAdd<States>(2),
nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Vectord<Inputs>& u, const Vectord<3>& y) {
m_observer.template Correct<3>(
u, y,
[](const Vectord<States>& x, const Vectord<Inputs>& u) {
return x.template block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, States>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<States>(2), frc::AngleAdd<States>(2));
};
// Set initial state.
Vectord<States> xhat;
auto poseVec = PoseTo3dVector(initialPose);
xhat(0) = poseVec(0);
xhat(1) = poseVec(1);
xhat(2) = poseVec(2);
for (size_t i = 0; i < NumModules; i++) {
xhat(3 + i) = modulePositions[i].distance.value();
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics{kinematics},
m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
m_observer.SetXhat(xhat);
// Calculate offsets.
m_gyroOffset = initialPose.Rotation() - gyroAngle;
m_previousAngle = initialPose.Rotation();
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Resets the robot's position on the field.
*
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero).
*
* 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.
*
@@ -154,35 +96,16 @@ class SwerveDrivePoseEstimator {
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
m_poseBuffer.Clear();
Vectord<States> xhat;
auto poseVec = PoseTo3dVector(pose);
xhat(0) = poseVec(0);
xhat(1) = poseVec(1);
xhat(2) = poseVec(2);
for (size_t i = 0; i < NumModules; i++) {
xhat(3 + i) = modulePositions[i].distance.value();
}
m_observer.SetXhat(xhat);
m_prevTime = -1_s;
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
/**
* Gets the pose of the robot at the current time as estimated by the Extended
* Kalman Filter.
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const {
return Pose2d{m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
Rotation2d{units::radian_t{m_observer.Xhat(2)}}};
}
Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
* Sets the pose estimator's trust of global measurements. This might be used
@@ -198,13 +121,26 @@ class SwerveDrivePoseEstimator {
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
// Create R (covariances) for vision measurements.
m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs);
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
* 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 Update() every loop.
@@ -226,16 +162,50 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Vectord<States>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
// Step 1: Get the estimated pose from when the vision measurement was made.
auto sample = m_poseBuffer.Sample(timestamp);
if (!sample.has_value()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose
auto twist = sample.value().pose.Log(visionRobotPose);
// 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.
frc::Vectord<3> k_times_twist =
m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 4: Convert back to Twist2d
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.ResetPosition(sample.value().gyroAngle,
sample.value().modulePostions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Replay odometry inputs between sample time and latest recorded
// sample to update the pose buffer and correct odometry.
auto internal_buf = m_poseBuffer.GetInternalBuffer();
auto upper_bound = std::lower_bound(
internal_buf.begin(), internal_buf.end(), timestamp,
[](const auto& pair, auto t) { return t > pair.first; });
for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
UpdateWithTime(entry->first, entry->second.gyroAngle,
entry->second.modulePostions);
}
}
/**
* Adds a vision measurement to the Unscented Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.
* 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 Update() every loop.
@@ -276,91 +246,137 @@ class SwerveDrivePoseEstimator {
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
* Updates the Kalman Filter using only wheel encoder information. This should
* be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
* @return The estimated pose of the robot in meters.
* @return The estimated robot pose in meters.
*/
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules>& moduleStates,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
moduleStates, modulePositions);
modulePositions);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
* Updates the Kalman Filter using only wheel encoder information. This should
* be called every loop.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param modulePositions The current distance travelled and rotations of
* @param modulePositions The current distance traveled and rotations of
* the swerve modules.
* @return The estimated pose of the robot in meters.
* @return The estimated robot pose in meters.
*/
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules>& moduleStates,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
m_odometry.Update(gyroAngle, modulePositions);
auto angle = gyroAngle + m_gyroOffset;
auto omega = (angle - m_previousAngle).Radians() / dt;
wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
wpi::empty_array};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates);
auto fieldRelativeSpeeds =
Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
angle);
Vectord<Inputs> u;
u(0) = fieldRelativeSpeeds.X().value();
u(1) = fieldRelativeSpeeds.Y().value();
u(2) = omega.value();
for (size_t i = 0; i < NumModules; i++) {
u(3 + i) = moduleStates[i].speed.value();
internalModulePositions[i].distance = modulePositions[i].distance;
internalModulePositions[i].angle = modulePositions[i].angle;
}
Vectord<Outputs> localY;
localY(0) = angle.Radians().value();
for (size_t i = 0; i < NumModules; i++) {
localY(1 + i) = modulePositions[i].distance.value();
}
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
m_observer.Predict(u, dt);
m_observer.Correct(u, localY);
m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
internalModulePositions});
return GetEstimatedPosition();
}
private:
UnscentedKalmanFilter<States, Inputs, Outputs> m_observer;
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyroscope angle.
Rotation2d gyroAngle;
// The distances traveled and rotations meaured at each module.
wpi::array<SwerveModulePosition, NumModules> modulePostions;
/**
* Checks equality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const InterpolationRecord& other) const = default;
/**
* Checks inequality between this InterpolationRecord and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const InterpolationRecord& other) const = default;
/**
* Interpolates between two InterpolationRecords.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
*
* @return The interpolated state.
*/
InterpolationRecord Interpolate(
SwerveDriveKinematics<NumModules>& kinematics,
InterpolationRecord endValue, double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the new module distances.
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wpi::empty_array};
// Find the distance between this measurement and the
// interpolated measurement.
wpi::array<SwerveModulePosition, NumModules> modulesDelta{
wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
modulePositions[i].distance =
wpi::Lerp(this->modulePostions[i].distance,
endValue.modulePostions[i].distance, i);
modulePositions[i].angle =
wpi::Lerp(this->modulePostions[i].angle,
endValue.modulePostions[i].angle, i);
modulesDelta[i].distance =
modulePositions[i].distance - this->modulePostions[i].distance;
modulesDelta[i].angle = modulePositions[i].angle;
}
// Find the new gyro angle.
auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
// Create a twist to represent this changed based on the interpolated
// sensor inputs.
auto twist = kinematics.ToTwist2d(modulesDelta);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, modulePositions};
}
}
};
SwerveDriveKinematics<NumModules>& m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<Inputs>& u, const Vectord<3>& y)>
m_visionCorrect;
SwerveDriveOdometry<NumModules> m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
Eigen::Matrix3d m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
1.5_s, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

8
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -120,6 +120,14 @@ class TimeInterpolatableBuffer {
return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
}
/**
* Grant access to the internal sample buffer. Used in Pose Estimation to
* replay odometry inputs stored within this buffer.
*/
std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
return m_pastSnapshots;
}
private:
units::second_t m_historySize;
std::vector<std::pair<units::second_t, T>> m_pastSnapshots;

15
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
View File

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/angle.h"
@@ -64,6 +65,20 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
/**
* Returns a twist from left and right distance deltas using
* forward kinematics.
*
* @param leftDistance The distance measured by the left encoder.
* @param rightDistance The distance measured by the right encoder.
* @return The resulting Twist2d.
*/
constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
const units::meter_t rightDistance) const {
return {(leftDistance + rightDistance) / 2, 0_m,
(rightDistance - leftDistance) / trackWidth * 1_rad};
}
units::meter_t trackWidth;
};
} // namespace frc

4
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -60,8 +60,8 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = 0_m;
m_prevRightDistance = 0_m;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
}
/**

17
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
View File

@@ -32,5 +32,22 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
* Distance driven by the rear-right wheel.
*/
units::meter_t rearRight = 0_m;
/**
* Checks equality between this MecanumDriveWheelPositions and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const MecanumDriveWheelPositions& other) const = default;
/**
* Checks inequality between this MecanumDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const MecanumDriveWheelPositions& other) const = default;
};
} // namespace frc

11
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -66,11 +66,9 @@ class SwerveDriveOdometry {
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter 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.
* integration of the pose over time. This also takes in an angle parameter
* which is used instead of the angular rate that is calculated from forward
* kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current position of all swerve modules. Please
@@ -90,7 +88,8 @@ class SwerveDriveOdometry {
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions;
wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
wpi::empty_array};
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

20
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -13,11 +13,15 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousModulePositions(modulePositions) {
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
for (size_t i = 0; i < NumModules; i++) {
m_previousModulePositions[i] = {modulePositions[i].distance,
modulePositions[i].angle};
}
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
}
@@ -30,7 +34,10 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousModulePositions = modulePositions;
for (size_t i = 0; i < NumModules; i++) {
m_previousModulePositions[i].distance = modulePositions[i].distance;
}
}
template <size_t NumModules>
@@ -39,11 +46,13 @@ const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto moduleDeltas =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t index = 0; index < modulePositions.size(); index++) {
for (size_t index = 0; index < NumModules; index++) {
auto lastPosition = m_previousModulePositions[index];
auto currentPosition = modulePositions[index];
moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
currentPosition.angle};
m_previousModulePositions[index].distance = modulePositions[index].distance;
}
auto angle = gyroAngle + m_gyroOffset;
@@ -55,7 +64,6 @@ const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
m_previousModulePositions = modulePositions;
return m_pose;
}