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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpimath/src/main/native/include/frc/EigenCore.h Normal file
View File

@@ -0,0 +1,23 @@
// 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 "Eigen/Core"
namespace frc {
template <int Size>
using Vectord = Eigen::Vector<double, Size>;
template <int Rows, int Cols,
int Options = Eigen::AutoAlign |
((Rows == 1 && Cols != 1) ? Eigen::RowMajor
: (Cols == 1 && Rows != 1)
? Eigen::ColMajor
: EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
int MaxRows = Rows, int MaxCols = Cols>
using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
} // namespace frc

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

@@ -13,9 +13,9 @@
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
@@ -64,9 +64,9 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
}
template <int States, int Inputs>
bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A};
for (int i = 0; i < States; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -107,12 +107,12 @@ template <int Rows, int Cols, typename... Ts,
std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
WPI_DEPRECATED(
"Use Eigen::Matrix or Eigen::Vector initializer list constructor")
Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
Matrixd<Rows, Cols> MakeMatrix(Ts... elems) {
static_assert(
sizeof...(elems) == Rows * Cols,
"Number of provided elements doesn't match matrix dimensionality");
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
detail::MatrixImpl<Rows, Cols>(result, elems...);
return result;
}
@@ -132,8 +132,7 @@ Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
Ts... tolerances) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), tolerances...);
return result;
@@ -153,8 +152,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
Ts... stdDevs) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
return result;
@@ -173,7 +171,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
* @return State excursion or control effort cost matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -195,8 +193,7 @@ Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
* @return Process noise or measurement noise covariance matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCovMatrix(
const std::array<double, N>& stdDevs) {
Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -207,8 +204,8 @@ Eigen::Matrix<double, N, N> MakeCovMatrix(
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Eigen::Matrix<double, sizeof...(Ts), 1> result;
Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
return result;
}
@@ -222,12 +219,11 @@ Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
* @return White noise vector.
*/
template <int N>
Eigen::Vector<double, N> MakeWhiteNoiseVector(
const std::array<double, N>& stdDevs) {
Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Eigen::Vector<double, N> result;
Vectord<N> result;
for (int i = 0; i < N; ++i) {
// Passing a standard deviation of 0.0 to std::normal_distribution is
// undefined behavior
@@ -249,7 +245,7 @@ Eigen::Vector<double, N> MakeWhiteNoiseVector(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
Vectord<3> PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -259,7 +255,7 @@ Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
Vectord<4> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -274,8 +270,8 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
* @param B Input matrix.
*/
template <int States, int Inputs>
bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
bool IsStabilizable(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
return detail::IsStabilizableImpl<States, Inputs>(A, B);
}
@@ -292,8 +288,8 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
* @param C Output matrix.
*/
template <int States, int Outputs>
bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, Outputs, States>& C) {
bool IsDetectable(const Matrixd<States, States>& A,
const Matrixd<Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
}
@@ -301,14 +297,14 @@ bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
const Matrixd<1, 1>& B);
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
@@ -318,7 +314,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
Vectord<3> PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.
@@ -330,11 +326,10 @@ Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
* @return Clamped input vector.
*/
template <int Inputs>
Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Inputs>& umin,
const Eigen::Vector<double, Inputs>& umax) {
Eigen::Vector<double, Inputs> result;
Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result(i) = std::clamp(u(i), umin(i), umax(i));
}
@@ -351,8 +346,8 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
* @return The normalizedInput
*/
template <int Inputs>
Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
if (maxValue > maxMagnitude) {

58
wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
@@ -39,6 +39,9 @@ namespace frc {
template <int States, int Inputs>
class ControlAffinePlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with given model dynamics as a function
* of state and input.
@@ -50,15 +53,11 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<StateVector(const StateVector&, const InputVector&)> f,
units::second_t dt)
: m_dt(dt), m_f(f) {
m_B = NumericalJacobianU<States, States, Inputs>(
f, Eigen::Vector<double, States>::Zero(),
Eigen::Vector<double, Inputs>::Zero());
m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
InputVector::Zero());
Reset();
}
@@ -73,14 +72,12 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
f,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
std::function<StateVector(const StateVector&)> f,
const Matrixd<States, Inputs>& B, units::second_t dt)
: m_B(B), m_dt(dt) {
m_f = [=](const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u)
-> Eigen::Vector<double, States> { return f(x); };
m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
return f(x);
};
Reset();
}
@@ -95,7 +92,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -111,7 +108,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -127,7 +124,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -146,15 +143,14 @@ class ControlAffinePlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -166,36 +162,30 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
StateVector rDot = (nextR - r) / m_dt.value();
m_uff = m_B.householderQr().solve(
rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
m_r = nextR;
return m_uff;
}
private:
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
/**
* The model dynamics.
*/
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

25
wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
View File

@@ -6,8 +6,8 @@
#include <frc/system/LinearSystem.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -27,6 +27,9 @@ namespace frc {
template <int States, int Inputs>
class ImplicitModelFollower {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -47,10 +50,10 @@ class ImplicitModelFollower {
* @param Aref Continuous system matrix whose dynamics should be followed.
* @param Bref Continuous input matrix whose dynamics should be followed.
*/
ImplicitModelFollower(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Aref,
const Eigen::Matrix<double, States, Inputs>& Bref) {
ImplicitModelFollower(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Aref,
const Matrixd<States, Inputs>& Bref) {
// Find u_imf that makes real model match reference model.
//
// dx/dt = Ax + Bu_imf
@@ -79,7 +82,7 @@ class ImplicitModelFollower {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -101,22 +104,20 @@ class ImplicitModelFollower {
* @param x The current state x.
* @param u The current input for the original model.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u) {
InputVector Calculate(const StateVector& x, const InputVector& u) {
m_u = m_A * x + m_B * u;
return m_u;
}
private:
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// State space conversion gain
Eigen::Matrix<double, Inputs, States> m_A;
Matrixd<Inputs, States> m_A;
// Input space conversion gain
Eigen::Matrix<double, Inputs, Inputs> m_B;
Matrixd<Inputs, Inputs> m_B;
};
} // namespace frc

34
wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -31,6 +31,9 @@ namespace frc {
template <int States, int Inputs>
class LinearPlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with the given plant.
*
@@ -50,9 +53,9 @@ class LinearPlantInversionFeedforward {
* @param B Continuous input matrix of the plant being controlled.
* @param dt Discretization timestep.
*/
LinearPlantInversionFeedforward(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
units::second_t dt)
: m_dt(dt) {
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
Reset();
@@ -63,7 +66,7 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -79,7 +82,7 @@ class LinearPlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -95,7 +98,7 @@ class LinearPlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -114,15 +117,14 @@ class LinearPlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -134,25 +136,23 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
m_uff = m_B.householderQr().solve(nextR - (m_A * r));
m_r = nextR;
return m_uff;
}
private:
Eigen::Matrix<double, States, States> m_A;
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, States> m_A;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

151
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -12,9 +12,9 @@
#include <wpi/array.h>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
@@ -39,6 +39,12 @@ namespace detail {
template <int States, int Inputs>
class LinearQuadraticRegulatorImpl {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using StateArray = wpi::array<double, States>;
using InputArray = wpi::array<double, Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -50,8 +56,7 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
LinearQuadraticRegulatorImpl(
const LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems, units::second_t dt)
const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
}
@@ -64,11 +69,10 @@ class LinearQuadraticRegulatorImpl {
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const StateArray& Qelems,
const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -81,13 +85,13 @@ class LinearQuadraticRegulatorImpl {
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
@@ -100,7 +104,7 @@ class LinearQuadraticRegulatorImpl {
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
@@ -121,17 +125,17 @@ class LinearQuadraticRegulatorImpl {
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
@@ -149,7 +153,7 @@ class LinearQuadraticRegulatorImpl {
/**
* Returns the controller matrix K.
*/
const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
const Matrixd<Inputs, States>& K() const { return m_K; }
/**
* Returns an element of the controller matrix K.
@@ -164,7 +168,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -180,7 +184,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -204,8 +208,7 @@ class LinearQuadraticRegulatorImpl {
*
* @param x The current state x.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x) {
InputVector Calculate(const StateVector& x) {
m_u = m_K * (m_r - x);
return m_u;
}
@@ -216,9 +219,7 @@ class LinearQuadraticRegulatorImpl {
* @param x The current state x.
* @param nextR The next reference vector r.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& x, const StateVector& nextR) {
m_r = nextR;
return Calculate(x);
}
@@ -242,8 +243,8 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
@@ -251,13 +252,13 @@ class LinearQuadraticRegulatorImpl {
private:
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// Controller gain
Eigen::Matrix<double, Inputs, States> m_K;
Matrixd<Inputs, States> m_K;
};
} // namespace detail
@@ -291,8 +292,8 @@ class LinearQuadraticRegulator
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
@@ -308,11 +309,10 @@ class LinearQuadraticRegulator
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
units::second_t dt)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
/**
@@ -325,12 +325,11 @@ class LinearQuadraticRegulator
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
units::second_t dt)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
R, N, dt} {}
@@ -351,24 +350,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const wpi::array<double, 1>& Qelems,
const wpi::array<double, 1>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q,
const Eigen::Matrix<double, 1, 1>& R,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
const Matrixd<1, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -387,24 +380,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const wpi::array<double, 2>& Qelems,
const wpi::array<double, 1>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
const Matrixd<2, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -423,24 +410,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const wpi::array<double, 2>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
const Matrixd<2, 2>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;

6
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -6,7 +6,7 @@
#include <wpi/MathExtras.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
@@ -70,8 +70,8 @@ class SimpleMotorFeedforward {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Eigen::Vector<double, 1> r{currentVelocity.value()};
Eigen::Vector<double, 1> nextR{nextVelocity.value()};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};

35
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -6,7 +6,7 @@
#include <wpi/numbers>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/MathUtil.h"
namespace frc {
@@ -21,10 +21,9 @@ namespace frc {
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Vector<double, States> AngleResidual(
const Eigen::Vector<double, States>& a,
const Eigen::Vector<double, States>& b, int angleStateIdx) {
Eigen::Vector<double, States> ret = a - b;
Vectord<States> AngleResidual(const Vectord<States>& a,
const Vectord<States>& b, int angleStateIdx) {
Vectord<States> ret = a - b;
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
return ret;
@@ -38,8 +37,7 @@ Eigen::Vector<double, States> AngleResidual(
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleResidual(int angleStateIdx) {
return [=](auto a, auto b) {
return AngleResidual<States>(a, b, angleStateIdx);
@@ -56,10 +54,9 @@ AngleResidual(int angleStateIdx) {
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
const Eigen::Vector<double, States>& b,
int angleStateIdx) {
Eigen::Vector<double, States> ret = a + b;
Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
int angleStateIdx) {
Vectord<States> ret = a + b;
ret[angleStateIdx] =
InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
return ret;
@@ -73,8 +70,7 @@ Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleAdd(int angleStateIdx) {
return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
}
@@ -91,9 +87,9 @@ AngleAdd(int angleStateIdx) {
* @param angleStatesIdx The row containing the angles.
*/
template <int CovDim, int States>
Eigen::Vector<double, CovDim> AngleMean(
const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm,
int angleStatesIdx) {
double sumSin = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::sin(it); })
.sum();
@@ -101,7 +97,7 @@ Eigen::Vector<double, CovDim> AngleMean(
.unaryExpr([](auto it) { return std::cos(it); })
.sum();
Eigen::Vector<double, CovDim> ret = sigmas * Wm;
Vectord<CovDim> ret = sigmas * Wm;
ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
return ret;
}
@@ -116,9 +112,8 @@ Eigen::Vector<double, CovDim> AngleMean(
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
AngleMean(int angleStateIdx) {
return [=](auto sigmas, auto Wm) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);

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

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
@@ -223,11 +223,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
private:
UnscentedKalmanFilter<5, 3, 3> m_observer;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix<double, 3, 3> m_visionContR;
Matrixd<3, 3> m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
@@ -237,13 +235,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& stdDevs);
const Vectord<Dim>& stdDevs);
static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 3>& u);
static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
units::meter_t leftDistance,
units::meter_t rightDistance);
static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u);
static Vectord<5> FillStateVector(const Pose2d& pose,
units::meter_t leftDistance,
units::meter_t rightDistance);
};
} // namespace frc

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

@@ -8,7 +8,7 @@
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -40,6 +40,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class ExtendedKalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
/**
* Constructs an extended Kalman filter.
*
@@ -51,17 +60,11 @@ class ExtendedKalmanFilter {
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
/**
* Constructs an extended Kalman filter.
@@ -77,30 +80,20 @@ class ExtendedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
* @param dt Nominal discretization timestep.
*/
ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
units::second_t dt);
ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
/**
* Returns the error covariance matrix P.
*/
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
const StateMatrix& P() const { return m_P; }
/**
* Returns an element of the error covariance matrix P.
@@ -115,12 +108,12 @@ class ExtendedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_P = P; }
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -134,7 +127,7 @@ class ExtendedKalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -158,7 +151,7 @@ class ExtendedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -166,19 +159,15 @@ class ExtendedKalmanFilter {
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
}
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -197,46 +186,28 @@ class ExtendedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&,
const Eigen::Vector<double, Rows>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
addFuncX);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
private:
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_h;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>)>
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
m_addFuncX;
Eigen::Vector<double, States> m_xHat;
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
Eigen::Matrix<double, States, States> m_initP;
StateMatrix m_initP;
};
} // namespace frc

138
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -16,72 +16,47 @@ namespace frc {
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt)
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
return a - b;
};
m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
m_dt = dt;
Reset();
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
Eigen::Matrix<double, Outputs, States> C =
NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
m_contQ = MakeCovMatrix(stateStdDevs);
@@ -90,39 +65,36 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Reset();
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
Eigen::Matrix<double, Outputs, States> C =
NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
const InputVector& u, units::second_t dt) {
// Find continuous A
Eigen::Matrix<double, States, States> contA =
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
@@ -136,44 +108,29 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
return a - b;
};
auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Eigen::Matrix<double, Rows, States> C =
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
@@ -187,7 +144,7 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Eigen::Matrix<double, States, Rows> K =
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
@@ -195,9 +152,8 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P *
(Eigen::Matrix<double, States, States>::Identity() - K * C)
.transpose() +
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}

29
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -36,6 +36,13 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class KalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
/**
* Constructs a state-space observer with the given plant.
*
@@ -45,9 +52,8 @@ class KalmanFilter {
* @param dt Nominal discretization timestep.
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs, units::second_t dt);
KalmanFilter(KalmanFilter&&) = default;
KalmanFilter& operator=(KalmanFilter&&) = default;
@@ -55,7 +61,7 @@ class KalmanFilter {
/**
* Returns the steady-state Kalman gain matrix K.
*/
const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
const Matrixd<States, Outputs>& K() const { return m_K; }
/**
* Returns an element of the steady-state Kalman gain matrix K.
@@ -68,7 +74,7 @@ class KalmanFilter {
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -82,7 +88,7 @@ class KalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -103,7 +109,7 @@ class KalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -111,8 +117,7 @@ class KalmanFilter {
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y);
void Correct(const InputVector& u, const OutputVector& y);
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -120,12 +125,12 @@ class KalmanFilter {
/**
* The steady-state Kalman gain matrix.
*/
Eigen::Matrix<double, States, Outputs> m_K;
Matrixd<States, Outputs> m_K;
/**
* The state estimate.
*/
Eigen::Vector<double, States> m_xHat;
StateVector m_xHat;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

24
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -21,15 +21,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt) {
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
@@ -46,12 +46,11 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> P =
drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
@@ -71,15 +70,14 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
const OutputVector& y) {
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
}

27
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -10,7 +10,7 @@
#include <utility>
#include <vector>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/math.h"
#include "units/time.h"
@@ -20,14 +20,13 @@ template <int States, int Inputs, int Outputs, typename KalmanFilterType>
class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
Eigen::Vector<double, States> xHat;
Eigen::Matrix<double, States, States> errorCovariances;
Eigen::Vector<double, Inputs> inputs;
Eigen::Vector<double, Outputs> localMeasurements;
Vectord<States> xHat;
Matrixd<States, States> errorCovariances;
Vectord<Inputs> inputs;
Vectord<Outputs> localMeasurements;
ObserverSnapshot(const KalmanFilterType& observer,
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& localY)
ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
const Vectord<Outputs>& localY)
: xHat(observer.Xhat()),
errorCovariances(observer.P()),
inputs(u),
@@ -47,10 +46,8 @@ class KalmanFilterLatencyCompensator {
* @param localY The local output at the timestamp
* @param timestamp The timesnap of the state.
*/
void AddObserverState(const KalmanFilterType& observer,
Eigen::Vector<double, Inputs> u,
Eigen::Vector<double, Outputs> localY,
units::second_t timestamp) {
void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
Vectord<Outputs> localY, units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -74,10 +71,8 @@ class KalmanFilterLatencyCompensator {
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, units::second_t nominalDt,
Eigen::Vector<double, Rows> y,
std::function<void(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y)>
KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
units::second_t timestamp) {
if (m_pastObserverSnapshots.size() == 0) {

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

@@ -9,7 +9,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
@@ -213,9 +213,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
UnscentedKalmanFilter<3, 3, 1> m_observer;
MecanumDriveKinematics m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;
@@ -227,7 +225,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& vector) {
const Vectord<Dim>& vector) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);

24
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

@@ -7,7 +7,7 @@
#include <cmath>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -62,14 +62,12 @@ class MerweScaledSigmaPoints {
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*
*/
Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
const Eigen::Vector<double, States>& x,
const Eigen::Matrix<double, States, States>& P) {
Matrixd<States, 2 * States + 1> SigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& P) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
Eigen::Matrix<double, States, States> U =
((lambda + States) * P).llt().matrixL();
Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
Eigen::Matrix<double, States, 2 * States + 1> sigmas;
Matrixd<States, 2 * States + 1> sigmas;
sigmas.template block<States, 1>(0, 0) = x;
for (int k = 0; k < States; ++k) {
sigmas.template block<States, 1>(0, k + 1) =
@@ -84,7 +82,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the mean.
*/
const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
/**
* Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +94,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the covariance.
*/
const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
/**
* Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +104,8 @@ class MerweScaledSigmaPoints {
double Wc(int i) const { return m_Wc(i, 0); }
private:
Eigen::Vector<double, 2 * States + 1> m_Wm;
Eigen::Vector<double, 2 * States + 1> m_Wc;
Vectord<2 * States + 1> m_Wm;
Vectord<2 * States + 1> m_Wc;
double m_alpha;
int m_kappa;
@@ -120,8 +118,8 @@ class MerweScaledSigmaPoints {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
double c = 0.5 / (States + lambda);
m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
m_Wm = Vectord<2 * States + 1>::Constant(c);
m_Wc = Vectord<2 * States + 1>::Constant(c);
m_Wm(0) = lambda / (States + lambda);
m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);

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

@@ -10,7 +10,7 @@
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
@@ -83,10 +83,8 @@ class SwerveDrivePoseEstimator {
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s)
: m_observer([](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return u; },
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) {
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs,
@@ -98,12 +96,9 @@ class SwerveDrivePoseEstimator {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return x; },
u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
@@ -190,7 +185,7 @@ class SwerveDrivePoseEstimator {
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -280,10 +275,10 @@ class SwerveDrivePoseEstimator {
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Vectord<3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
Vectord<1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
@@ -298,9 +293,7 @@ class SwerveDrivePoseEstimator {
UnscentedKalmanFilter<3, 3, 1> m_observer;
SwerveDriveKinematics<NumModules>& m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;
@@ -312,7 +305,7 @@ class SwerveDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& vector) {
const Vectord<Dim>& vector) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);

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

@@ -9,7 +9,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "units/time.h"
@@ -42,6 +42,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class UnscentedKalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
/**
* Constructs an unscented Kalman filter.
*
@@ -53,17 +62,11 @@ class UnscentedKalmanFilter {
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
@@ -90,42 +93,27 @@ class UnscentedKalmanFilter {
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
/**
* Returns the error covariance matrix P.
*/
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
const StateMatrix& P() const { return m_P; }
/**
* Returns an element of the error covariance matrix P.
@@ -140,12 +128,12 @@ class UnscentedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_P = P; }
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -159,7 +147,7 @@ class UnscentedKalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -184,7 +172,7 @@ class UnscentedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -192,8 +180,7 @@ class UnscentedKalmanFilter {
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
m_residualFuncX, m_addFuncX);
}
@@ -212,13 +199,10 @@ class UnscentedKalmanFilter {
* @param R Measurement noise covariance matrix (continuous-time).
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -241,64 +225,39 @@ class UnscentedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Matrix<double, Rows, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
meanFuncY,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&,
const Eigen::Vector<double, Rows>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
residualFuncX,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
addFuncX);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
private:
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_h;
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
m_meanFuncX;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
m_meanFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
m_residualFuncX;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
m_addFuncX;
Eigen::Vector<double, States> m_xHat;
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, 2 * States + 1> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;

142
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -16,34 +16,20 @@ namespace frc {
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt)
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
return sigmas * Wm;
};
m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a - b;
};
m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
return a - b;
};
m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
m_dt = dt;
Reset();
@@ -51,36 +37,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f),
m_h(h),
@@ -98,21 +68,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
template <int States, int Inputs, int Outputs>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
Eigen::Matrix<double, States, States> contA =
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
@@ -127,59 +96,38 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a - b;
};
auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
return a - b;
};
auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Matrix<double, Rows, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
// Transform sigma points into measurement space
Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
@@ -191,7 +139,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
Py += discR;
// Compute cross covariance of the state and the measurements
Eigen::Matrix<double, States, Rows> Pxy;
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
@@ -206,7 +154,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
Eigen::Matrix<double, States, Rows> K =
Matrixd<States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)

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

@@ -6,7 +6,7 @@
#include <tuple>
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -31,33 +31,30 @@ namespace frc {
* passing through the transform.
*/
template <int CovDim, int States>
std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Vector<double, 2 * States + 1>& Wm,
const Eigen::Vector<double, 2 * States + 1>& Wc,
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
meanFunc,
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Vector<double, CovDim>&,
const Eigen::Vector<double, CovDim>&)>
residualFunc) {
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFunc,
std::function<Vectord<CovDim>(const Vectord<CovDim>&,
const Vectord<CovDim>&)>
residualFunc) {
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
Vectord<CovDim> 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;
Matrixd<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) =
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
}
Eigen::Matrix<double, CovDim, CovDim> P =
Matrixd<CovDim, CovDim> P =
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
return std::make_tuple(x, P);

8
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -14,8 +14,8 @@
#include <wpi/circular_buffer.h>
#include <wpi/span.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -209,7 +209,7 @@ class LinearFilter {
static_assert(Derivative < Samples,
"Order of derivative must be less than number of samples.");
Eigen::Matrix<double, Samples, Samples> S;
Matrixd<Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
S(row, col) = std::pow(stencil[col], row);
@@ -217,12 +217,12 @@ class LinearFilter {
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
Eigen::Vector<double, Samples> d;
Vectord<Samples> d;
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
}
Eigen::Vector<double, Samples> a =
Vectord<Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list

6
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -6,8 +6,8 @@
#include <wpi/SymbolExports.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
@@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
mutable Matrixd<4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;

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

@@ -9,8 +9,8 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -179,9 +179,8 @@ class SwerveDriveKinematics {
units::meters_per_second_t attainableMaxSpeed);
private:
mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;

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

@@ -25,7 +25,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Eigen::Matrix<double, 2, 3>{
Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
@@ -37,13 +37,13 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
@@ -70,17 +70,16 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
wpi::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStatesMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStatesMatrix);
m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},

19
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 4> m_coefficients =
Eigen::Matrix<double, 6, 4>::Zero();
Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
static Matrixd<4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
@@ -74,10 +71,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
static const Eigen::Matrix<double, 4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
return basis;
}

22
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 6> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 6> m_coefficients =
Eigen::Matrix<double, 6, 6>::Zero();
Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
static Matrixd<6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
@@ -81,7 +78,7 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
static const Eigen::Matrix<double, 6, 6> basis{
static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
@@ -100,11 +97,10 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector<double, 6> ControlVectorFromArrays(
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
initialVector[2], finalVector[0],
finalVector[1], finalVector[2]};
static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
wpi::array<double, 3> finalVector) {
return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]};
}
};
} // namespace frc

8
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -9,7 +9,7 @@
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
#include "units/length.h"
@@ -55,7 +55,7 @@ class Spline {
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
Eigen::Vector<double, Degree + 1> polynomialBases;
Vectord<Degree + 1> polynomialBases;
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@ class Spline {
// This simply multiplies by the coefficients. We need to divide out t some
// n number of times where n is the derivative we want to take.
Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
Vectord<6> combined = Coefficients() * polynomialBases;
double dx, dy, ddx, ddy;
@@ -100,7 +100,7 @@ class Spline {
*
* @return The coefficients of the spline.
*/
virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
/**
* Converts a Translation2d into a vector that is compatible with Eigen.

62
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "unsupported/Eigen/MatrixFunctions"
@@ -19,9 +19,8 @@ namespace frc {
* @param discA Storage for discrete system matrix.
*/
template <int States>
void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA) {
void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
Matrixd<States, States>* discA) {
*discA = (contA * dt.value()).exp();
}
@@ -37,19 +36,18 @@ void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
* @param discB Storage for discrete input matrix.
*/
template <int States, int Inputs>
void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, Inputs>& contB,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, Inputs>* discB) {
void DiscretizeAB(const Matrixd<States, States>& contA,
const Matrixd<States, Inputs>& contB, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, Inputs>* discB) {
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Matrixd<States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
Matrixd<States + Inputs, States + Inputs> Mdisc = Mcont.exp();
*discA = Mdisc.template block<States, States>(0, 0);
*discB = Mdisc.template block<States, Inputs>(0, States);
}
@@ -65,29 +63,26 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
void DiscretizeAQ(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
// Set up the matrix M = [[-A, Q], [0, A.T]]
Eigen::Matrix<double, 2 * States, 2 * States> M;
Matrixd<2 * States, 2 * States> M;
M.template block<States, States>(0, 0) = -contA;
M.template block<States, States>(0, States) = Q;
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
Eigen::Matrix<double, States, States> phi12 =
phi.block(0, States, States, States);
Eigen::Matrix<double, States, States> phi22 =
phi.block(States, States, States, States);
Matrixd<States, States> phi12 = phi.block(0, States, States, States);
Matrixd<States, States> phi22 = phi.block(States, States, States, States);
*discA = phi22.transpose();
@@ -117,21 +112,20 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ,
units::second_t dt, Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
Matrixd<States, States> lastTerm = Q;
double lastCoeff = dt.value();
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
Matrixd<States, States> Atn = contA.transpose();
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
Matrixd<States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
@@ -159,8 +153,8 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
* @param dt Discretization timestep.
*/
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
units::second_t dt) {
return R / dt.value();
}

44
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -8,7 +8,7 @@
#include <functional>
#include <stdexcept>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
@@ -30,6 +30,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystem {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a discrete plant with the given continuous system coefficients.
*
@@ -39,10 +43,10 @@ class LinearSystem {
* @param D Feedthrough matrix.
* @throws std::domain_error if any matrix element isn't finite.
*/
LinearSystem(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, Outputs, States>& C,
const Eigen::Matrix<double, Outputs, Inputs>& D) {
LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
throw std::domain_error(
"Elements of A aren't finite. This is usually due to model "
@@ -78,7 +82,7 @@ class LinearSystem {
/**
* Returns the system matrix A.
*/
const Eigen::Matrix<double, States, States>& A() const { return m_A; }
const Matrixd<States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
@@ -91,7 +95,7 @@ class LinearSystem {
/**
* Returns the input matrix B.
*/
const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
const Matrixd<States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
@@ -104,7 +108,7 @@ class LinearSystem {
/**
* Returns the output matrix C.
*/
const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
const Matrixd<Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
@@ -117,7 +121,7 @@ class LinearSystem {
/**
* Returns the feedthrough matrix D.
*/
const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
const Matrixd<Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
@@ -137,11 +141,10 @@ class LinearSystem {
* @param clampedU The control input.
* @param dt Timestep for model update.
*/
Eigen::Vector<double, States> CalculateX(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
StateVector CalculateX(const StateVector& x, const InputVector& clampedU,
units::second_t dt) const {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
return discA * x + discB * clampedU;
@@ -156,9 +159,8 @@ class LinearSystem {
* @param x The current state.
* @param clampedU The control input.
*/
Eigen::Vector<double, Outputs> CalculateY(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU) const {
OutputVector CalculateY(const StateVector& x,
const InputVector& clampedU) const {
return m_C * x + m_D * clampedU;
}
@@ -166,22 +168,22 @@ class LinearSystem {
/**
* Continuous system matrix.
*/
Eigen::Matrix<double, States, States> m_A;
Matrixd<States, States> m_A;
/**
* Continuous input matrix.
*/
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
/**
* Output matrix.
*/
Eigen::Matrix<double, Outputs, States> m_C;
Matrixd<Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
Eigen::Matrix<double, Outputs, Inputs> m_D;
Matrixd<Outputs, Inputs> m_D;
};
} // namespace frc

62
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -35,6 +35,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystemLoop {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
@@ -53,7 +57,7 @@ class LinearSystemLoop {
units::volt_t maxVoltage, units::second_t dt)
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
[=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -73,9 +77,7 @@ class LinearSystemLoop {
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
clampFunction,
std::function<InputVector(const InputVector&)> clampFunction,
units::second_t dt)
: LinearSystemLoop(
controller,
@@ -97,11 +99,10 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}
: LinearSystemLoop(
controller, feedforward, observer, [=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
}) {}
/**
* Constructs a state-space loop with the given controller, feedforward,
@@ -117,9 +118,7 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<
Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
clampFunction)
std::function<InputVector(const InputVector&)> clampFunction)
: m_controller(&controller),
m_feedforward(feedforward),
m_observer(&observer),
@@ -134,9 +133,7 @@ class LinearSystemLoop {
/**
* Returns the observer's state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const {
return m_observer->Xhat();
}
const StateVector& Xhat() const { return m_observer->Xhat(); }
/**
* Returns an element of the observer's state estimate x-hat.
@@ -148,7 +145,7 @@ class LinearSystemLoop {
/**
* Returns the controller's next reference r.
*/
const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
const StateVector& NextR() const { return m_nextR; }
/**
* Returns an element of the controller's next reference r.
@@ -160,7 +157,7 @@ class LinearSystemLoop {
/**
* Returns the controller's calculated control input u.
*/
Eigen::Vector<double, Inputs> U() const {
InputVector U() const {
return ClampInput(m_controller->U() + m_feedforward.Uff());
}
@@ -176,9 +173,7 @@ class LinearSystemLoop {
*
* @param xHat The initial state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) {
m_observer->SetXhat(xHat);
}
void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); }
/**
* Set an element of the initial state estimate x-hat.
@@ -193,7 +188,7 @@ class LinearSystemLoop {
*
* @param nextR Next reference.
*/
void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
void SetNextR(const StateVector& nextR) { m_nextR = nextR; }
/**
* Return the controller used internally.
@@ -225,7 +220,7 @@ class LinearSystemLoop {
*
* @param initialState The initial state.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_nextR.setZero();
m_controller->Reset();
m_feedforward.Reset(initialState);
@@ -235,18 +230,14 @@ class LinearSystemLoop {
/**
* Returns difference between reference r and current state x-hat.
*/
Eigen::Vector<double, States> Error() const {
return m_controller->R() - m_observer->Xhat();
}
StateVector Error() const { return m_controller->R() - m_observer->Xhat(); }
/**
* Correct the state estimate x-hat using the measurements in y.
*
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Outputs>& y) {
m_observer->Correct(U(), y);
}
void Correct(const OutputVector& y) { m_observer->Correct(U(), y); }
/**
* Sets new controller output, projects model forward, and runs observer
@@ -258,7 +249,7 @@ class LinearSystemLoop {
* @param dt Timestep for model update.
*/
void Predict(units::second_t dt) {
Eigen::Vector<double, Inputs> u =
InputVector u =
ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
m_feedforward.Calculate(m_nextR));
m_observer->Predict(u, dt);
@@ -270,10 +261,7 @@ class LinearSystemLoop {
* @param u Input vector to clamp.
* @return Clamped input vector.
*/
Eigen::Vector<double, Inputs> ClampInput(
const Eigen::Vector<double, Inputs>& u) const {
return m_clampFunc(u);
}
InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); }
protected:
LinearQuadraticRegulator<States, Inputs>* m_controller;
@@ -283,12 +271,10 @@ class LinearSystemLoop {
/**
* Clamping function.
*/
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
m_clampFunc;
std::function<InputVector(const InputVector&)> m_clampFunc;
// Reference to go to in the next cycle (used by feedforward controller).
Eigen::Vector<double, States> m_nextR;
StateVector m_nextR;
// These are accessible from non-templated subclasses.
static constexpr int kStates = States;

26
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -17,16 +17,16 @@ namespace frc {
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Eigen::Vector<double, Cols> dX_plus = x;
Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::Vector<double, Cols> dX_minus = x;
Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
@@ -48,12 +48,10 @@ auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianX(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
x);
[&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
@@ -70,12 +68,10 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianU(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
u);
[&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
} // namespace frc

88
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -56,15 +56,13 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * r * m)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -90,12 +88,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -134,10 +132,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<1, 1> A{-kV.value() / kA.value()};
Matrixd<1, 1> B{1.0 / kA.value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
@@ -176,10 +174,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -224,12 +222,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Eigen::Matrix<double, 2, 2> A =
0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
Eigen::Matrix<double, 2, 2> B =
0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
@@ -311,11 +307,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{
Matrixd<1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
@@ -342,12 +338,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
@@ -391,18 +387,16 @@ class WPILIB_DLLEXPORT LinearSystemId {
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
Eigen::Matrix<double, 2, 2> A{
{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Eigen::Matrix<double, 2, 2> B{
{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}