Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

Browse Source
This commit is contained in:
Joseph Eng
2023-06-19 17:10:39 -07:00
committed by GitHub
parent 5c2addda0f
commit 25ad5017a9
41 changed files with 1742 additions and 2177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,13 +7,11 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#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 {
@@ -32,7 +30,9 @@ 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.
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
: public PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions> {
public:
/**
* Constructs a DifferentialDrivePoseEstimator with default standard
@@ -79,19 +79,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision 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 pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*
@@ -101,71 +88,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
* 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.
*
* 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 visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* 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.
*
* 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 SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::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.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
units::meter_t rightDistance, const Pose2d& pose) {
PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::
ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
}
/**
@@ -179,7 +105,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @return The estimated pose of the robot.
*/
Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return PoseEstimator<
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::Update(gyroAngle,
{leftDistance, rightDistance});
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
@@ -195,63 +126,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance);
units::meter_t rightDistance) {
return PoseEstimator<
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
{leftDistance,
rightDistance});
}
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyro angle.
Rotation2d gyroAngle;
// The distance traveled by the left encoder.
units::meter_t leftDistance;
// The distance traveled by the right encoder.
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;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
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{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
DifferentialDriveOdometry m_odometryImpl;
};
} // namespace frc

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

@@ -10,6 +10,7 @@
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,7 +33,9 @@ namespace frc {
* never call it, then this class will behave mostly like regular encoder
* odometry.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
: public PoseEstimator<MecanumDriveWheelSpeeds,
MecanumDriveWheelPositions> {
public:
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations
@@ -76,174 +79,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision 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 pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* 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 wheelPositions The distances measured at each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* 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
* calling 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 visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime()
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp().) This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* 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.
*
* 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 SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::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.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Updates the pose estimator with wheel encoder and gyro 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 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 MecanumDriveWheelPositions& wheelPositions);
private:
struct InterpolationRecord {
// The pose observed given the current sensor inputs and the previous pose.
Pose2d pose;
// The current gyroscope angle.
Rotation2d gyroAngle;
// The distances measured at each wheel.
MecanumDriveWheelPositions wheelPositions;
/**
* 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;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
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{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
MecanumDriveOdometry m_odometryImpl;
};
} // namespace frc

228
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h Normal file
View File

@@ -0,0 +1,228 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "frc/kinematics/Kinematics.h"
#include "frc/kinematics/Odometry.h"
#include "frc/kinematics/WheelPositions.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
/**
* This class wraps odometry to fuse latency-compensated
* vision measurements with encoder measurements. Robot code should not use this
* directly- Instead, use the particular type for your drivetrain (e.g.,
* DifferentialDrivePoseEstimator). It will correct for noisy vision
* measurements and encoder drift. It is intended to be an easy drop-in for
* Odometry.
*
* 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 like regular encoder odometry.
*/
template <typename WheelSpeeds, WheelPositions WheelPositions>
class WPILIB_DLLEXPORT PoseEstimator {
public:
/**
* Constructs a PoseEstimator.
*
* @param kinematics A correctly-configured kinematics object for your
* drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
* @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.
*/
PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* Sets the pose estimator's trust in vision 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 pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
/**
* 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 current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* 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.
*
* 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 visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* 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.
*
* 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 SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::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.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
*
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
/**
* Updates the pose estimator with wheel encoder and gyro information. This
* should be called every loop.
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances traveled by the encoders.
*
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
private:
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 by the wheels.
WheelPositions wheelPositions;
/**
* 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(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
InterpolationRecord endValue, double i) const;
};
static constexpr units::second_t kBufferDuration = 1.5_s;
Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
Odometry<WheelSpeeds, WheelPositions>& m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
};
} // namespace frc
#include "frc/estimator/PoseEstimator.inc"

170
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc Normal file
View File

@@ -0,0 +1,170 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/estimator/PoseEstimator.h"
namespace frc {
template <typename WheelSpeeds, WheelPositions WheelPositions>
PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_kinematics(kinematics), m_odometry(odometry) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& 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]));
}
}
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_poseBuffer.Clear();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {
return m_odometry.GetPose();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (!m_poseBuffer.GetInternalBuffer().empty() &&
m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// 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.
Vectord<3> k_times_twist =
m_visionK *
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().wheelPositions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the
// same timestamp
m_poseBuffer.AddSample(timestamp,
{GetEstimatedPosition(), sample.value().gyroAngle,
sample.value().wheelPositions});
// Step 7: 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.wheelPositions);
}
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
m_odometry.Update(gyroAngle, wheelPositions);
// Copy?
WheelPositions internalWheelPositions = wheelPositions;
m_poseBuffer.AddSample(
currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
return GetEstimatedPosition();
}
template <typename WheelSpeeds, WheelPositions WheelPositions>
typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
InterpolationRecord endValue, double i) const {
if (i < 0) {
return *this;
} else if (i > 1) {
return endValue;
} else {
// Find the new wheel distance measurements.
WheelPositions wheels_lerp =
this->wheelPositions.Interpolate(endValue.wheelPositions, i);
// Find the distance between this measurement and the
// interpolated measurement.
WheelPositions wheels_delta = wheels_lerp - this->wheelPositions;
// 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(wheels_delta);
twist.dtheta = (gyro - gyroAngle).Radians();
return {pose.Exp(twist), gyro, wheels_lerp};
}
}
} // namespace frc

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

@@ -6,19 +6,16 @@
#include <cmath>
#include <fmt/format.h>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "frc/EigenCore.h"
#include "frc/estimator/PoseEstimator.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 "frc/kinematics/SwerveDriveWheelPositions.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -34,7 +31,9 @@ namespace frc {
* odometry.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
class SwerveDrivePoseEstimator
: public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -84,14 +83,10 @@ class SwerveDrivePoseEstimator {
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
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];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
: PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
/**
* Resets the robot's position on the field.
@@ -108,157 +103,11 @@ class SwerveDrivePoseEstimator {
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
m_poseBuffer.Clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
/**
* Sets the pose estimator's trust in vision 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 pose
* measurement (x position in meters, y position in meters, and heading in
* radians). Increase these numbers to trust the vision pose measurement
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& 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]));
}
}
}
/**
* 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.
*
* 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 visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime()
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp().) This means that you should use
* frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (!m_poseBuffer.GetInternalBuffer().empty() &&
m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// 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().modulePositions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the
// same timestamp
m_poseBuffer.AddSample(timestamp,
{GetEstimatedPosition(), sample.value().gyroAngle,
sample.value().modulePositions});
// Step 7: 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.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 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 SetVisionMeasurementStdDevs() or this method.
*
* @param visionRobotPose The pose of the robot as measured by the vision
* camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note
* that if you don't use your own time source by calling UpdateWithTime(),
* then you must use a timestamp with an epoch since FPGA startup (i.e.,
* the epoch of this timestamp is the same epoch as
* frc::Timer::GetFPGATimestamp(). This means that you should use
* frc::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.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
PoseEstimator<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
{modulePositions},
pose);
}
/**
@@ -273,8 +122,10 @@ class SwerveDrivePoseEstimator {
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
modulePositions);
return PoseEstimator<
SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
{modulePositions});
}
/**
@@ -290,111 +141,13 @@ class SwerveDrivePoseEstimator {
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
m_odometry.Update(gyroAngle, modulePositions);
wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
internalModulePositions[i].distance = modulePositions[i].distance;
internalModulePositions[i].angle = modulePositions[i].angle;
}
m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
internalModulePositions});
return GetEstimatedPosition();
return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
SwerveDriveWheelPositions<NumModules>>::
UpdateWithTime(currentTime, gyroAngle, {modulePositions});
}
private:
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 measured at each module.
wpi::array<SwerveModulePosition, NumModules> modulePositions;
/**
* 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->modulePositions[i].distance,
endValue.modulePositions[i].distance, i);
modulePositions[i].angle =
wpi::Lerp(this->modulePositions[i].angle,
endValue.modulePositions[i].angle, i);
modulesDelta[i].distance =
modulePositions[i].distance - this->modulePositions[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};
}
}
};
static constexpr units::second_t kBufferDuration = 1.5_s;
SwerveDriveKinematics<NumModules>& m_kinematics;
SwerveDriveOdometry<NumModules> m_odometry;
wpi::array<double, 3> m_q{wpi::empty_array};
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
kBufferDuration, [this](const InterpolationRecord& start,
const InterpolationRecord& end, double t) {
return start.Interpolate(this->m_kinematics, end, t);
}};
SwerveDriveOdometry<NumModules> m_odometryImpl;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)