mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add pose estimators (#2867)
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
committed by
GitHub
GitHub
parent
3413bfc06a
commit
bc8f338771
@@ -9,6 +9,18 @@
|
||||
|
||||
namespace frc {
|
||||
|
||||
Eigen::Matrix<double, 3, 1> PoseTo3dVector(const Pose2d& pose) {
|
||||
return frc::MakeMatrix<3, 1>(pose.Translation().X().to<double>(),
|
||||
pose.Translation().Y().to<double>(),
|
||||
pose.Rotation().Radians().to<double>());
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose) {
|
||||
return frc::MakeMatrix<4, 1>(pose.Translation().X().to<double>(),
|
||||
pose.Translation().Y().to<double>(),
|
||||
pose.Rotation().Cos(), pose.Rotation().Sin());
|
||||
}
|
||||
|
||||
template <>
|
||||
bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B) {
|
||||
|
||||
@@ -0,0 +1,143 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
|
||||
|
||||
#include <wpi/timestamp.h>
|
||||
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/AngleStatistics.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
|
||||
const Rotation2d& gyroAngle, const Pose2d& initialPose,
|
||||
const std::array<double, 5>& stateStdDevs,
|
||||
const std::array<double, 3>& localMeasurementStdDevs,
|
||||
const std::array<double, 3>& visionMeasurmentStdDevs,
|
||||
units::second_t nominalDt)
|
||||
: m_observer(
|
||||
&DifferentialDrivePoseEstimator::F,
|
||||
[](const Eigen::Matrix<double, 5, 1>& x,
|
||||
const Eigen::Matrix<double, 3, 1>& u) {
|
||||
return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
|
||||
},
|
||||
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
|
||||
frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
|
||||
frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
|
||||
m_nominalDt(nominalDt) {
|
||||
// Create R (covariances) for vision measurements.
|
||||
Eigen::Matrix<double, 3, 3> visionContR =
|
||||
frc::MakeCovMatrix(visionMeasurmentStdDevs);
|
||||
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, 5, 1>& x,
|
||||
const Eigen::Matrix<double, 3, 1>&) { return x.block<3, 1>(0, 0); },
|
||||
m_visionDiscR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
|
||||
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
|
||||
};
|
||||
|
||||
m_gyroOffset = initialPose.Rotation() - gyroAngle;
|
||||
m_previousAngle = initialPose.Rotation();
|
||||
m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
|
||||
}
|
||||
|
||||
void DifferentialDrivePoseEstimator::ResetPosition(
|
||||
const Pose2d& pose, const Rotation2d& gyroAngle) {
|
||||
m_previousAngle = pose.Rotation();
|
||||
m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
|
||||
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
|
||||
}
|
||||
|
||||
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
|
||||
return Pose2d(units::meter_t(m_observer.Xhat(0)),
|
||||
units::meter_t(m_observer.Xhat(1)),
|
||||
Rotation2d(units::radian_t(m_observer.Xhat(2))));
|
||||
}
|
||||
|
||||
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
|
||||
const Pose2d& visionRobotPose, units::second_t timestamp) {
|
||||
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
|
||||
PoseTo3dVector(visionRobotPose),
|
||||
m_visionCorrect, timestamp);
|
||||
}
|
||||
|
||||
Pose2d DifferentialDrivePoseEstimator::Update(
|
||||
const Rotation2d& gyroAngle,
|
||||
const DifferentialDriveWheelSpeeds& wheelSpeeds,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance) {
|
||||
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
|
||||
wheelSpeeds, leftDistance, rightDistance);
|
||||
}
|
||||
|
||||
Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
|
||||
units::second_t currentTime, const Rotation2d& gyroAngle,
|
||||
const DifferentialDriveWheelSpeeds& wheelSpeeds,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance) {
|
||||
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
|
||||
m_prevTime = currentTime;
|
||||
|
||||
auto angle = gyroAngle + m_gyroOffset;
|
||||
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
|
||||
|
||||
auto u = frc::MakeMatrix<3, 1>(
|
||||
(wheelSpeeds.left + wheelSpeeds.right).to<double>() / 2.0, 0.0,
|
||||
omega.to<double>());
|
||||
|
||||
m_previousAngle = angle;
|
||||
|
||||
auto localY = frc::MakeMatrix<3, 1>(leftDistance.to<double>(),
|
||||
rightDistance.to<double>(),
|
||||
angle.Radians().to<double>());
|
||||
|
||||
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
|
||||
m_observer.Predict(u, dt);
|
||||
m_observer.Correct(u, localY);
|
||||
|
||||
return GetEstimatedPosition();
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
|
||||
const Eigen::Matrix<double, 5, 1>& x,
|
||||
const Eigen::Matrix<double, 3, 1>& u) {
|
||||
// Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
|
||||
// that for us.
|
||||
auto& theta = x(2, 0);
|
||||
Eigen::Matrix<double, 5, 5> toFieldRotation = frc::MakeMatrix<5, 5>(
|
||||
// clang-format off
|
||||
std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0,
|
||||
std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 1.0); // clang-format on
|
||||
return toFieldRotation *
|
||||
frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0));
|
||||
}
|
||||
|
||||
template <int Dim>
|
||||
std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
|
||||
const Eigen::Matrix<double, Dim, 1>& stdDevs) {
|
||||
std::array<double, Dim> array;
|
||||
for (size_t i = 0; i < Dim; ++i) {
|
||||
array[i] = stdDevs(i);
|
||||
}
|
||||
return array;
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::FillStateVector(
|
||||
const Pose2d& pose, units::meter_t leftDistance,
|
||||
units::meter_t rightDistance) {
|
||||
return frc::MakeMatrix<5, 1>(
|
||||
pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
|
||||
pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
|
||||
rightDistance.to<double>());
|
||||
}
|
||||
@@ -0,0 +1,115 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/estimator/MecanumDrivePoseEstimator.h"
|
||||
|
||||
#include <wpi/timestamp.h>
|
||||
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/AngleStatistics.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
frc::MecanumDrivePoseEstimator::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)
|
||||
: 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 vision correction mechanism.
|
||||
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>&) { 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();
|
||||
}
|
||||
|
||||
void frc::MecanumDrivePoseEstimator::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();
|
||||
}
|
||||
|
||||
Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
|
||||
return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
|
||||
Rotation2d(units::radian_t{m_observer.Xhat(2)}));
|
||||
}
|
||||
|
||||
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
|
||||
const Pose2d& visionRobotPose, units::second_t timestamp) {
|
||||
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
|
||||
PoseTo3dVector(visionRobotPose),
|
||||
m_visionCorrect, timestamp);
|
||||
}
|
||||
|
||||
Pose2d frc::MecanumDrivePoseEstimator::Update(
|
||||
const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
|
||||
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
|
||||
wheelSpeeds);
|
||||
}
|
||||
|
||||
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
|
||||
units::second_t currentTime, const Rotation2d& gyroAngle,
|
||||
const MecanumDriveWheelSpeeds& wheelSpeeds) {
|
||||
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(wheelSpeeds);
|
||||
auto fieldRelativeVelocities =
|
||||
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
|
||||
.RotateBy(angle);
|
||||
|
||||
auto u = frc::MakeMatrix<3, 1>(fieldRelativeVelocities.X().to<double>(),
|
||||
fieldRelativeVelocities.Y().to<double>(),
|
||||
omega.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();
|
||||
}
|
||||
Reference in New Issue
Block a user