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

[wpilib] Add pose estimators (#2867)

Browse Source 
Pose and state estimators can filter latency-compensated global measurements and fuse them with state-space drivetrain model information to estimate robot position. They are drop-in replacements for the existing odometry classes.

Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Declan Freeman-Gleason
2020-11-28 17:35:35 -05:00
committed by GitHub
parent 3413bfc06a
commit bc8f338771
58 changed files with 4958 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -232,6 +232,24 @@ Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
return result;
}
/**
* Converts a Pose2d into a vector of [x, y, theta].
*
* @param pose The pose that is being represented.
*
* @return The vector.
*/
Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
*
* @param pose The pose that is being represented.
*
* @return The vector.
*/
Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
*

128
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h Normal file
View File

@@ -0,0 +1,128 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <wpi/math>
#include "Eigen/Core"
namespace frc {
namespace {
double NormalizeAngle(double angle) {
static constexpr double tau = 2 * wpi::math::pi;
angle -= std::floor(angle / tau) * tau;
if (angle > wpi::math::pi) angle -= tau;
return angle;
}
} // namespace
/**
* Subtracts a and b while normalizing the resulting value in the selected row
* as if it were an angle.
*
* @param a A vector to subtract from.
* @param b A vector to subtract with.
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Matrix<double, States, 1> AngleResidual(
const Eigen::Matrix<double, States, 1>& a,
const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
Eigen::Matrix<double, States, 1> ret = a - b;
ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
return ret;
}
/**
* Returns a function that subtracts two vectors while normalizing the resulting
* value in the selected row as if it were an angle.
*
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<
Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
AngleResidual(int angleStateIdx) {
return [=](auto a, auto b) {
return AngleResidual<States>(a, b, angleStateIdx);
};
}
/**
* Adds a and b while normalizing the resulting value in the selected row as an
* angle.
*
* @param a A vector to add with.
* @param b A vector to add with.
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Matrix<double, States, 1> AngleAdd(
const Eigen::Matrix<double, States, 1>& a,
const Eigen::Matrix<double, States, 1>& b, int angleStateIdx) {
Eigen::Matrix<double, States, 1> ret = a + b;
ret[angleStateIdx] = NormalizeAngle(ret[angleStateIdx]);
return ret;
}
/**
* Returns a function that adds two vectors while normalizing the resulting
* value in the selected row as an angle.
*
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<
Eigen::Matrix<double, States, 1>(const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
AngleAdd(int angleStateIdx) {
return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
}
/**
* Computes the mean of sigmas with the weights Wm while computing a special
* angle mean for a select row.
*
* @param sigmas Sigma points.
* @param Wm Weights for the mean.
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
Eigen::Matrix<double, CovDim, 1> AngleMean(
const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Matrix<double, 2 * States + 1, 1>& Wm, int angleStatesIdx) {
double sumSin = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::sin(it); })
.sum();
double sumCos = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::cos(it); })
.sum();
Eigen::Matrix<double, CovDim, 1> ret = sigmas * Wm;
ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
return ret;
}
/**
* Returns a function that computes the mean of sigmas with the weights Wm while
* computing a special angle mean for a select row.
*
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
std::function<Eigen::Matrix<double, CovDim, 1>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
AngleMean(int angleStateIdx) {
return [=](auto sigmas, auto Wm) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
};
}
} // namespace frc

174
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h Normal file
View File

@@ -0,0 +1,174 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include "Eigen/Core"
#include "frc/estimator/KalmanFilterLatencyCompensator.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/time.h"
namespace frc {
/**
* This class wraps an Unscented Kalman Filter 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
* AddVisionMeasurement(), and only call Update(), this will behave exactly the
* same as DifferentialDriveOdometry.
*
* Update() should be called every robot loop (if your robot loops are faster or
* slower than the default, then you should change the nominal delta time via
* the constructor).
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave like regular encoder odometry.
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field
* coordinate system.
*
* <strong> u = [[d_l, d_r, dtheta]]^T </strong> (robot-relative velocities) --
* 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>y = [[x, y, theta]]^T </strong> from vision,
* or <strong>y = [[dist_l, dist_r, theta]] </strong> from encoders and gyro.
*/
class DifferentialDrivePoseEstimator {
public:
/**
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The gyro angle of the robot.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of the model states.
* Increase these to trust your wheel speeds
* less.
* @param localMeasurementStdDevs Standard deviations of the encoder
* measurements. Increase these to trust
* encoder distances less.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these to trust
* vision less.
* @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const std::array<double, 5>& stateStdDevs,
const std::array<double, 3>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s);
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders to zero when calling this method. 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 pose The estimated pose of the robot on the field.
*@param gyroAngle The current gyro angle.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
/**
* Returns the pose of the robot at the current time as estimated by the
* Unscented Kalman Filter.
*
* @return The estimated robot pose.
*/
Pose2d GetEstimatedPosition() const;
/**
* Adds a vision measurement to the Unscented 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.
*
* @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
* frc2::Timer::GetFPGATimestamp(). This means that
* you should use frc2::Timer::GetFPGATimestamp() as
* your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Updates the Unscented Kalman Filter using only wheel encoder information.
* Note that this should be called every loop iteration.
*
* @param gyroAngle The current gyro angle.
* @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);
/**
* Updates the Unscented Kalman Filter using only wheel encoder information.
* Note that this should be called every loop iteration.
*
* @param currentTime The time at which this method was called.
* @param gyroAngle The current gyro angle.
* @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 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;
KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
m_latencyCompensator;
std::function<void(const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix<double, 3, 3> m_visionDiscR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
template <int Dim>
static std::array<double, Dim> StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& stdDevs);
static Eigen::Matrix<double, 5, 1> F(const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 3, 1>& u);
static Eigen::Matrix<double, 5, 1> FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance);
};
} // namespace frc

2
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -25,7 +25,7 @@ template <int States, int Inputs, int Outputs>
class ExtendedKalmanFilter {
public:
/**
* Constructs an extended Kalman filter.
* Constructs an Extended Kalman filter.
*
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.

129
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h Normal file
View File

@@ -0,0 +1,129 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <algorithm>
#include <array>
#include <functional>
#include <utility>
#include <vector>
#include "Eigen/Core"
#include "units/math.h"
#include "units/time.h"
namespace frc {
template <int States, int Inputs, int Outputs, typename KalmanFilterType>
class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
Eigen::Matrix<double, States, 1> xHat;
Eigen::Matrix<double, States, States> errorCovariances;
Eigen::Matrix<double, Inputs, 1> inputs;
Eigen::Matrix<double, Outputs, 1> localMeasurements;
ObserverSnapshot(const KalmanFilterType& observer,
const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& localY)
: xHat(observer.Xhat()),
errorCovariances(observer.P()),
inputs(u),
localMeasurements(localY) {}
};
void AddObserverState(const KalmanFilterType& observer,
Eigen::Matrix<double, Inputs, 1> u,
Eigen::Matrix<double, Outputs, 1> localY,
units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
// Remove the oldest snapshot if the vector exceeds our maximum size.
if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
m_pastObserverSnapshots.erase(m_pastObserverSnapshots.begin());
}
}
template <int Rows>
void ApplyPastMeasurement(
KalmanFilterType* observer, units::second_t nominalDt,
Eigen::Matrix<double, Rows, 1> y,
std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Rows, 1>& y)>
globalMeasurementCorrect,
units::second_t timestamp) {
if (m_pastObserverSnapshots.size() == 0) {
// State map was empty, which means that we got a measurement right at
// startup. The only thing we can do is ignore the measurement.
return;
}
// We will perform a binary search to find the index of the element in the
// vector that has a timestamp that is equal to or greater than the vision
// measurement timestamp.
auto lowerBoundIter = std::lower_bound(
m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
timestamp,
[](const auto& entry, const auto& ts) { return entry.first < ts; });
int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter);
// High and Low should be the same. The sampled timestamp is greater than or
// equal to the vision pose timestamp. We will now find the entry which is
// closest in time to the requested timestamp.
size_t indexOfClosestEntry =
units::math::abs(
timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) <
units::math::abs(timestamp -
m_pastObserverSnapshots[index].first)
? index - 1
: index;
units::second_t lastTimestamp =
m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;
// We will now go back in time to the state of the system at the time when
// the measurement was captured. We will reset the observer to that state,
// and apply correction based on the measurement. Then, we will go back
// through all observer states until the present and apply past inputs to
// get the present estimated state.
for (size_t i = indexOfClosestEntry; i < m_pastObserverSnapshots.size();
++i) {
auto& [key, snapshot] = m_pastObserverSnapshots[i];
if (i == indexOfClosestEntry) {
observer->SetP(snapshot.errorCovariances);
observer->SetXhat(snapshot.xHat);
}
observer->Predict(snapshot.inputs, key - lastTimestamp);
observer->Correct(snapshot.inputs, snapshot.localMeasurements);
if (i == indexOfClosestEntry) {
// Note that the measurement is at a timestep close but probably not
// exactly equal to the timestep for which we called predict. This makes
// the assumption that the dt is small enough that the difference
// between the measurement time and the time that the inputs were
// captured at is very small.
globalMeasurementCorrect(snapshot.inputs, y);
}
lastTimestamp = key;
snapshot = ObserverSnapshot{*observer, snapshot.inputs,
snapshot.localMeasurements};
}
}
private:
static constexpr size_t kMaxPastObserverStates = 300;
std::vector<std::pair<units::second_t, ObserverSnapshot>>
m_pastObserverSnapshots;
};
} // namespace frc

171
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h Normal file
View File

@@ -0,0 +1,171 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <functional>
#include "Eigen/Core"
#include "frc/estimator/KalmanFilterLatencyCompensator.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "units/time.h"
namespace frc {
/**
* This class wraps an Unscented Kalman Filter 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.
*
* Update() should be called every robot loop. If your loops are faster or
* slower than the default of 0.02s, then you should change the nominal delta
* time by specifying it in the constructor.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave mostly like regular encoder
* odometry.
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta]]^T </strong> in the
* field-coordinate system.
*
* <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
*
* <strong> y = [[x, y, theta]]^T </strong> in field
* coords from vision, or <strong> y = [[theta]]^T
* </strong> from the gyro.
*/
class MecanumDrivePoseEstimator {
public:
/**
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters 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
* wheel and gyro velocities less.
* @param localMeasurementStdDevs Standard deviations of the gyro
* measurement. Increase this number
* to trust gyro angle measurements less.
* @param visionMeasurementStdDevs Standard deviations of the encoder
* measurements. Increase these numbers
* to trust vision less.
* @param nominalDt The time in seconds between each robot
* loop.
*/
MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
MecanumDriveKinematics kinematics,
const std::array<double, 3>& stateStdDevs,
const std::array<double, 1>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s);
/**
* Resets the robot's position on the field.
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
/**
* Gets the pose of the robot at the current time as estimated by the Extended
* Kalman Filter.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
/**
* Add a vision measurement to the Unscented 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.
*
* @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 Timer#GetFPGATimestamp.) This means
* that you should use Timer#GetFPGATimestamp as your
* time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* 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.
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
/**
* 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.
*
* @param currentTimeSeconds 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.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
private:
UnscentedKalmanFilter<3, 3, 1> m_observer;
MecanumDriveKinematics m_kinematics;
KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
m_latencyCompensator;
std::function<void(const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix3d m_visionDiscR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
template <int Dim>
static std::array<double, Dim> StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& vector) {
std::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);
}
return array;
}
};
} // namespace frc

260
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h Normal file
View File

@@ -0,0 +1,260 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <array>
#include <limits>
#include <wpi/timestamp.h>
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/KalmanFilterLatencyCompensator.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/SwerveDriveKinematics.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.
*
* Update() should be called every robot loop. If your loops are faster or
* slower than the default of 0.02s, then you should change the nominal delta
* time by specifying it in the constructor.
*
* AddVisionMeasurement() can be called as infrequently as you want; if you
* never call it, then this class will behave mostly like regular encoder
* odometry.
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta]]^T </strong> in the
* field-coordinate system.
*
* <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
*
* <strong> y = [[x, y, std::theta]]^T </strong> in field
* coords from vision, or <strong> y = [[theta]]^T
* </strong> from the gyro.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
public:
/**
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters 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
* wheel and gyro velocities less.
* @param localMeasurementStdDevs Standard deviations of the gyro
* measurement. Increase this number to
* trust gyro angle measurements less.
* @param visionMeasurementStdDevs Standard deviations of the encoder
* measurements. Increase these numbers to
* trust vision less.
* @param nominalDt The time in seconds between each robot
* loop.
*/
SwerveDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
SwerveDriveKinematics<NumModules>& kinematics,
const std::array<double, 3>& stateStdDevs,
const std::array<double, 1>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s)
: m_observer([](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) { return u; },
[](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs,
frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
frc::AngleAdd<3>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
// Construct R (covariances) matrix for vision measurements.
Eigen::Matrix3d visionContR =
frc::MakeCovMatrix<3>(visionMeasurementStdDevs);
// Create and store discrete covariance matrix for vision measurements.
m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) { return x; },
m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
// Set initial state.
m_observer.SetXhat(PoseTo3dVector(initialPose));
// Calculate offsets.
m_gyroOffset = initialPose.Rotation() - gyroAngle;
m_previousAngle = initialPose.Rotation();
}
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders (to zero) when calling this method.
*
* 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 pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
// Set observer state.
m_observer.SetXhat(PoseTo3dVector(pose));
// Calculate offsets.
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.
*
* @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)}));
}
/**
* Add a vision measurement to the Unscented 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.
*
* @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 Timer#GetFPGATimestamp.) This means
* that you should use Timer#GetFPGATimestamp as your
* time source or sync the epochs.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}
/**
* 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.
*
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @return The estimated pose of the robot in meters.
*/
template <typename... ModuleState>
Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
moduleStates...);
}
/**
* 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.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @return The estimated pose of the robot in meters.
*/
template <typename... ModuleState>
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleState&&... moduleStates) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
auto angle = gyroAngle + m_gyroOffset;
auto omega = (angle - m_previousAngle).Radians() / dt;
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
auto fieldRelativeSpeeds =
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
auto u =
frc::MakeMatrix<3, 1>(fieldRelativeSpeeds.X().template to<double>(),
fieldRelativeSpeeds.Y().template to<double>(),
omega.template to<double>());
auto localY = frc::MakeMatrix<1, 1>(angle.Radians().template to<double>());
m_previousAngle = angle;
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
m_observer.Predict(u, dt);
m_observer.Correct(u, localY);
return GetEstimatedPosition();
}
private:
UnscentedKalmanFilter<3, 3, 1> m_observer;
SwerveDriveKinematics<NumModules>& m_kinematics;
KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
m_latencyCompensator;
std::function<void(const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix3d m_visionDiscR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
template <int Dim>
static std::array<double, Dim> StdDevMatrixToArray(
const Eigen::Matrix<double, Dim, 1>& vector) {
std::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);
}
return array;
}
};
} // namespace frc

144
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -50,6 +50,91 @@ class UnscentedKalmanFilter {
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Matrix<double, States, 1> {
return sigmas * Wm;
};
m_meanFuncY = [](auto sigmas,
auto Wc) -> Eigen::Matrix<double, Outputs, 1> {
return sigmas * Wc;
};
m_residualFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
return a - b;
};
m_residualFuncY = [](auto a, auto b) -> Eigen::Matrix<double, Outputs, 1> {
return a - b;
};
m_addFuncX = [](auto a, auto b) -> Eigen::Matrix<double, States, 1> {
return a + b;
};
m_dt = dt;
Reset();
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
* addition functions. Using custom functions for arithmetic can be useful if
* you have angles in the state or measurements, because they allow you to
* correctly account for the modular nature of angle arithmetic.
*
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
* the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param meanFuncX A function that computes the mean of 2 * States +
* 1 state vectors using a given set of weights.
* @param meanFuncY A function that computes the mean of 2 * States +
* 1 measurement vectors using a given set of weights.
* @param residualFuncX A function that computes the residual of two
* state vectors (i.e. it subtracts them.)
* @param residualFuncY A function that computes the residual of two
* measurement vectors (i.e. it subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, Inputs, 1>&)>
f,
std::function<Eigen::Matrix<double, Outputs, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, Inputs, 1>&)>
h,
const std::array<double, States>& stateStdDevs,
const std::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
meanFuncX,
std::function<Eigen::Matrix<double, Outputs, 1>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
meanFuncY,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
residualFuncX,
std::function<Eigen::Matrix<double, Outputs, 1>(
const Eigen::Matrix<double, Outputs, 1>&,
const Eigen::Matrix<double, Outputs, 1>&)>
residualFuncY,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
addFuncX,
units::second_t dt)
: m_f(f),
m_h(h),
m_meanFuncX(meanFuncX),
m_meanFuncY(meanFuncY),
m_residualFuncX(residualFuncX),
m_residualFuncY(residualFuncY),
m_addFuncX(addFuncX) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
@@ -136,8 +221,8 @@ class UnscentedKalmanFilter {
m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
}
auto ret =
UnscentedTransform<States, States>(m_sigmasF, m_pts.Wm(), m_pts.Wc());
auto ret = UnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
m_xHat = std::get<0>(ret);
m_P = std::get<1>(ret);
@@ -152,7 +237,8 @@ class UnscentedKalmanFilter {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
Correct<Outputs>(u, y, m_h, m_contR);
Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
m_residualFuncX, m_addFuncX);
}
/**
@@ -175,7 +261,23 @@ class UnscentedKalmanFilter {
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, Inputs, 1>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Matrix<double, Rows, 1>(
const Eigen::Matrix<double, Rows, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
meanFuncY,
std::function<Eigen::Matrix<double, Rows, 1>(
const Eigen::Matrix<double, Rows, 1>&,
const Eigen::Matrix<double, Rows, 1>&)>
residualFuncY,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
residualFuncX,
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>)>
addFuncX) {
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
// Transform sigma points into measurement space
@@ -188,17 +290,19 @@ class UnscentedKalmanFilter {
}
// Mean and covariance of prediction passed through UT
auto [yHat, Py] =
UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
auto [yHat, Py] = UnscentedTransform<States, Rows>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
Py += discR;
// Compute cross covariance of the state and the measurements
Eigen::Matrix<double, States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
Pxy += m_pts.Wc(i) *
(m_sigmasF.template block<States, 1>(0, i) - m_xHat) *
(sigmasH.template block<Rows, 1>(0, i) - yHat).transpose();
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
(residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
.transpose();
}
// K = P_{xy} Py^-1
@@ -209,7 +313,7 @@ class UnscentedKalmanFilter {
Eigen::Matrix<double, States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
m_xHat += K * (y - yHat);
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
m_P -= K * Py * K.transpose();
}
@@ -222,6 +326,26 @@ class UnscentedKalmanFilter {
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, Inputs, 1>&)>
m_h;
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
m_meanFuncX;
std::function<Eigen::Matrix<double, Outputs, 1>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
m_meanFuncY;
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>&)>
m_residualFuncX;
std::function<Eigen::Matrix<double, Outputs, 1>(
const Eigen::Matrix<double, Outputs, 1>&,
const Eigen::Matrix<double, Outputs, 1>)>
m_residualFuncY;
std::function<Eigen::Matrix<double, States, 1>(
const Eigen::Matrix<double, States, 1>&,
const Eigen::Matrix<double, States, 1>)>
m_addFuncX;
Eigen::Matrix<double, States, 1> m_xHat;
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;

21
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -14,8 +14,8 @@
namespace frc {
/**
* Computes unscented transform of a set of sigma points and weights. CovDimurns
* the mean and covariance in a tuple.
* Computes unscented transform of a set of sigma points and weights. CovDim
* returns the mean and covariance in a tuple.
*
* This works in conjunction with the UnscentedKalmanFilter class.
*
@@ -34,17 +34,26 @@ std::tuple<Eigen::Matrix<double, CovDim, 1>,
Eigen::Matrix<double, CovDim, CovDim>>
UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
const Eigen::Matrix<double, 2 * States + 1, 1>& Wc) {
// New mean is just the sum of the sigmas * weight
const Eigen::Matrix<double, 2 * States + 1, 1>& Wc,
std::function<Eigen::Matrix<double, CovDim, 1>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Matrix<double, 2 * States + 1, 1>&)>
meanFunc,
std::function<Eigen::Matrix<double, CovDim, 1>(
const Eigen::Matrix<double, CovDim, 1>&,
const Eigen::Matrix<double, CovDim, 1>&)>
residualFunc) {
// New mean is usually just the sum of the sigmas * weight:
// dot = \Sigma^n_1 (W[k]*Xi[k])
Eigen::Matrix<double, CovDim, 1> x = sigmas * Wm;
Eigen::Matrix<double, CovDim, 1> x = meanFunc(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Eigen::Matrix<double, CovDim, 2 * States + 1> y;
for (int i = 0; i < 2 * States + 1; ++i) {
// y[:, i] = sigmas[:, i] - x;
y.template block<CovDim, 1>(0, i) =
sigmas.template block<CovDim, 1>(0, i) - x;
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
}
Eigen::Matrix<double, CovDim, CovDim> P =
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();

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

@@ -76,6 +76,23 @@ class SwerveDriveKinematics {
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
explicit SwerveDriveKinematics(
const std::array<Translation2d, NumModules>& wheels)
: m_modules{wheels} {
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
1, 0, (-m_modules[i].Y()).template to<double>(),
0, 1, (+m_modules[i].X()).template to<double>();
// clang-format on
}
m_forwardKinematics = m_inverseKinematics.householderQr();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
/**