Skip to content

Time varying qr pose estimator #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mcm001 wants to merge 23 commits into
base: time-varying-qr-pose-estimator
Choose a base branch
from
Open

Time varying qr pose estimator #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
8bd8022
Start kinda doing stuff
mcm001 Oct 18, 2020
3cb52f3
aaah
mcm001 Oct 18, 2020
6ff2022
Finally
mcm001 Oct 18, 2020
037d5c4
Remove uses of Vector
mcm001 Oct 18, 2020
1fe89f9
Update DifferentialDrivePoseEstimatorTest.cpp
mcm001 Oct 18, 2020
2f8e849
Fix infine loop bug in latency comp
mcm001 Oct 18, 2020
b13945f
Run wpiformat
mcm001 Oct 18, 2020
4cd86f8
Add KF latency test
mcm001 Oct 19, 2020
60dfa6a
Cleanup cos/sin
mcm001 Oct 19, 2020
3602d0c
address reviewaaaah
mcm001 Oct 20, 2020
41d3083
whoop
mcm001 Oct 20, 2020
6de5ee6
Fix rotation2d bug in diff drive pose estimator
mcm001 Oct 21, 2020
a58bbae
Update swerve to have time-varying Q and R
mcm001 Oct 21, 2020
157012f
Run format
mcm001 Oct 21, 2020
02ad673
Make mecanum pose q and r time-varying
mcm001 Oct 22, 2020
527bc54
Switch java pose estiamtor to time varying q/r
mcm001 Oct 22, 2020
b690d4f
timicbwdgtstiol
mcm001 Oct 23, 2020
2a592f7
Start adding time varying q/r support to latency comp
mcm001 Oct 23, 2020
c617d2a
Whoops
mcm001 Oct 23, 2020
3048f95
Update build.gradle
mcm001 Oct 23, 2020
cd12928
Add JNI for llt
mcm001 Oct 23, 2020
8946867
Adding JNI llt decomposition with Eigen.
cttdev Oct 24, 2020
de330b4
Add naive test that basically recreates swerve
mcm001 Oct 24, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
131 changes: 90 additions & 41 deletions wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,36 +7,45 @@

#include "frc/estimator/DifferentialDrivePoseEstimator.h"

#include "frc/StateSpaceUtil.h"
#include <frc/StateSpaceUtil.h>

#include "frc2/Timer.h"

using namespace frc;

DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const Vector<5>& stateStdDevs, const Vector<3>& localMeasurementStdDevs,
const Vector<3>& visionMeasurmentStdDevs, units::second_t nominalDt)
: m_observer(
const std::array<double, 5>& stateStdDevs,
const std::array<double, 3>& localMeasurementStdDevs,
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_stateStdDevs(stateStdDevs),
m_localMeasurementStdDevs(localMeasurementStdDevs),
m_observer(
&DifferentialDrivePoseEstimator::F,
[](const Vector<5>& x, const Vector<3>& u) {
return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0));
},
StdDevMatrixToArray<5>(stateStdDevs),
StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt),
&DifferentialDrivePoseEstimator::LocalMeasurementModel,
MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)),
MakeRDiagonals(localMeasurementStdDevs,
FillStateVector(initialPose, 0_m, 0_m)),
nominalDt),
m_nominalDt(nominalDt) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurmentStdDevs));
m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
frc::MakeCovMatrix<3>(visionMeasurementStdDevs);

// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Vector<3>& u, const Vector<3>& y) {
m_observer.Correct<3>(
m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
const Eigen::Matrix<double, 4, 1>& y) {
m_observer.Correct<4>(
u, y,
[](const Vector<5>& x, const Vector<3>&) {
return x.block<3, 1>(0, 0);
[](const Eigen::Matrix<double, 6, 1>& x_,
const Eigen::Matrix<double, 3, 1>& u_) {
return x_.block<4, 1>(0, 0);
},
m_visionDiscR);
DiscretizeR<4>(
MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals(
visionMeasurementStdDevs, m_observer.Xhat())),
m_nominalDt));
};

m_gyroOffset = initialPose.Rotation() - gyroAngle;
Expand All @@ -54,13 +63,13 @@ void DifferentialDrivePoseEstimator::ResetPosition(
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))));
Rotation2d(m_observer.Xhat(2), m_observer.Xhat(3)));
}

void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
PoseTo3dVector(visionRobotPose),
m_latencyCompensator.ApplyPastMeasurement<4>(&m_observer, m_nominalDt,
PoseTo4dVector(visionRobotPose),
m_visionCorrect, timestamp);
}

Expand All @@ -82,54 +91,94 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto angle = gyroAngle + m_gyroOffset;
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;

auto u = frc::MakeMatrix<3, 1>(
Eigen::Matrix<double, 3, 1> 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>());
Eigen::Matrix<double, 4, 1> localY = frc::MakeMatrix<4, 1>(
leftDistance.to<double>(), rightDistance.to<double>(), angle.Cos(),
angle.Sin());

m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
m_observer.Predict(u, dt);
m_observer.Correct(u, localY);
m_observer.Predict(
u,
frc::MakeCovMatrix<6>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())),
dt);
m_observer.Correct<4>(u, localY, &LocalMeasurementModel,
frc::MakeCovMatrix<4>(MakeRDiagonals(
m_localMeasurementStdDevs, m_observer.Xhat())));

return GetEstimatedPosition();
}

Vector<5> DifferentialDrivePoseEstimator::F(const Vector<5>& x,
const Vector<3>& u) {
Eigen::Matrix<double, 6, 1> DifferentialDrivePoseEstimator::F(
const Eigen::Matrix<double, 6, 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));
double cosTheta = x(2);
double sinTheta = x(3);

// We want vx and vy to be in the field frame, so we apply a rotation matrix.
// to u_Field = u = [[vx_field, vy_field, omega]]^T
Eigen::Matrix<double, 2, 1> chassisVel_local =
MakeMatrix<2, 1>((u(0, 0) + u(1, 0)) / 2.0, 0.0);
Eigen::Matrix<double, 2, 2> toFieldRotation =
MakeMatrix<2, 2>(cosTheta, -sinTheta, sinTheta, cosTheta);
Eigen::Matrix<double, 2, 1> chassisVel_field =
toFieldRotation * chassisVel_local;

// dcos(theta)/dt = -std::sin(theta) * dtheta/dt = -std::sin(theta) * omega
double dcosTheta = -sinTheta * u(2, 0);
// dsin(theta)/dt = std::cos(theta) * omega
double dsinTheta = cosTheta * u(2, 0);

// As x = [[x_field, y_field, std::cos(theta), std::sin(theta), dist_l,
// dist_r]]^T, we need to return x-dot = [[vx_field, vy_field, d/dt
// std::cos(theta), d/dt sin(theta), vel_left, vel_right]]^T Assuming no
// wheel slip, vx = (v_left + v_right) / 2, and vy = 0;

return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), dcosTheta,
dsinTheta, u(0), u(1));
}

Eigen::Matrix<double, 4, 1>
DifferentialDrivePoseEstimator::LocalMeasurementModel(
const Eigen::Matrix<double, 6, 1>& x,
const Eigen::Matrix<double, 3, 1>& u) {
return frc::MakeMatrix<4, 1>(x(4), x(5), x(2), x(3));
}

template <int Dim>
std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
const Vector<Dim>& stdDevs) {
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;
}

Vector<5> DifferentialDrivePoseEstimator::FillStateVector(
Eigen::Matrix<double, 6, 1> DifferentialDrivePoseEstimator::FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance) {
return frc::MakeMatrix<5, 1>(
return frc::MakeMatrix<6, 1>(
pose.Translation().X().to<double>(), pose.Translation().Y().to<double>(),
pose.Rotation().Radians().to<double>(), leftDistance.to<double>(),
pose.Rotation().Cos(), pose.Rotation().Sin(), leftDistance.to<double>(),
rightDistance.to<double>());
}

std::array<double, 6> DifferentialDrivePoseEstimator::MakeQDiagonals(
const std::array<double, 5>& stdDevs,
const Eigen::Matrix<double, 6, 1>& x) {
return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2),
stdDevs[2] * x(3), stdDevs[3], stdDevs[4]};
}

std::array<double, 4> DifferentialDrivePoseEstimator::MakeRDiagonals(
const std::array<double, 3>& stdDevs,
const Eigen::Matrix<double, 6, 1>& x) {
return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)};
}
58 changes: 34 additions & 24 deletions wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,35 +13,37 @@
using namespace frc;

DifferentialDriveStateEstimator::DifferentialDriveStateEstimator(
const LinearSystem<2, 2, 2>& plant, const Eigen::Matrix<double, 10, 1>& initialState,
const Eigen::Matrix<double, 10, 1>& stateStdDevs, const Eigen::Matrix<double, 3, 1>& localMeasurementStdDevs,
const Eigen::Matrix<double, 3, 1>& globalMeasurementStdDevs,
const LinearSystem<2, 2, 2>& plant,
const std::array<double, 10>& initialState,
const std::array<double, 10>& stateStdDevs,
const std::array<double, 3>& localMeasurementStdDevs,
const std::array<double, 3>& globalMeasurementStdDevs,
const DifferentialDriveKinematics& kinematics, units::second_t nominalDt)
: m_plant(plant),
m_rb(kinematics.trackWidth / 2.0),
m_observer([this](auto& x, auto& u) { return Dynamics(x, u); },
&DifferentialDriveStateEstimator::LocalMeasurementModel,
StdDevMatrixToArray<10>(stateStdDevs),
StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt),
stateStdDevs, localMeasurementStdDevs, nominalDt),
m_nominalDt(nominalDt) {
m_localY.setZero();
m_globalY.setZero();

// Create R (covariances) for global measurements.
Eigen::Matrix<double, 3, 3> globalContR =
frc::MakeCovMatrix(StdDevMatrixToArray<3>(globalMeasurementStdDevs));
frc::MakeCovMatrix(globalMeasurementStdDevs);

Eigen::Matrix<double, 3, 3> globalDiscR =
frc::DiscretizeR<3>(globalContR, m_nominalDt);

// Create correction mechanism for global measurements.
m_globalCorrect = [&](const Eigen::Matrix<double, 2, 1>& u, const Eigen::Matrix<double, 3, 1>& y) {
m_globalCorrect = [&](const Eigen::Matrix<double, 2, 1>& u,
const Eigen::Matrix<double, 3, 1>& y) {
m_observer.Correct<3>(
u, y, &DifferentialDriveStateEstimator::GlobalMeasurementModel,
globalDiscR);
};

Reset(initialState);
Reset(ArrayToVector<10>(initialState));
}

void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement(
Expand All @@ -51,28 +53,30 @@ void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement(
m_globalCorrect, timestamp);
}

Vector<10> DifferentialDriveStateEstimator::GetEstimatedState() const {
Eigen::Matrix<double, 10, 1>
DifferentialDriveStateEstimator::GetEstimatedState() const {
return m_observer.Xhat();
}

frc::Pose2d DifferentialDriveStateEstimator::GetEstimatedPosition() {
Vector<10> xHat = GetEstimatedState();
Eigen::Matrix<double, 10, 1> xHat = GetEstimatedState();
return frc::Pose2d(
units::meter_t(xHat(State::kX, 0)),
units::meter_t(xHat(State::kY, 0)),
frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0))));
units::meter_t(xHat(State::kX, 0)), units::meter_t(xHat(State::kY, 0)),
frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0))));
}

Vector<10> DifferentialDriveStateEstimator::Update(
Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::Update(
units::radian_t heading, units::meter_t leftPosition,
units::meter_t rightPosition, const Eigen::Matrix<double, 2, 1>& controlInput) {
units::meter_t rightPosition,
const Eigen::Matrix<double, 2, 1>& controlInput) {
return UpdateWithTime(heading, leftPosition, rightPosition, controlInput,
frc2::Timer::GetFPGATimestamp());
}

Vector<10> DifferentialDriveStateEstimator::UpdateWithTime(
Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::UpdateWithTime(
units::radian_t heading, units::meter_t leftPosition,
units::meter_t rightPosition, const Eigen::Matrix<double, 2, 1>& controlInput,
units::meter_t rightPosition,
const Eigen::Matrix<double, 2, 1>& controlInput,
units::second_t currentTime) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
Expand All @@ -90,16 +94,18 @@ Vector<10> DifferentialDriveStateEstimator::UpdateWithTime(
return GetEstimatedState();
}

void DifferentialDriveStateEstimator::Reset(const Eigen::Matrix<double, 10, 1>& initialState) {
void DifferentialDriveStateEstimator::Reset(
const Eigen::Matrix<double, 10, 1>& initialState) {
m_observer.Reset();

m_observer.SetXhat(initialState);
}

void DifferentialDriveStateEstimator::Reset() { m_observer.Reset(); }

Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix<double, 10, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Matrix<double, 10, 1> DifferentialDriveStateEstimator::Dynamics(
const Eigen::Matrix<double, 10, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Matrix<double, 4, 2> B;
B.block<2, 2>(0, 0) = m_plant.B();
B.block<2, 2>(2, 0).setZero();
Expand All @@ -124,8 +130,10 @@ Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix<double,
return result;
}

Vector<3> DifferentialDriveStateEstimator::LocalMeasurementModel(
const Eigen::Matrix<double, 10, 1>& x, const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Matrix<double, 3, 1>
DifferentialDriveStateEstimator::LocalMeasurementModel(
const Eigen::Matrix<double, 10, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);

Eigen::Matrix<double, 3, 1> y;
Expand All @@ -134,8 +142,10 @@ Vector<3> DifferentialDriveStateEstimator::LocalMeasurementModel(
return y;
}

Vector<3> DifferentialDriveStateEstimator::GlobalMeasurementModel(
const Eigen::Matrix<double, 10, 1>& x, const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Matrix<double, 3, 1>
DifferentialDriveStateEstimator::GlobalMeasurementModel(
const Eigen::Matrix<double, 10, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);

Eigen::Matrix<double, 3, 1> y;
Expand Down
Loading