mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-29 02:21:44 +00:00
[wpimath] Add core State-space classes (#2614)
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
303
wpimath/src/main/native/include/frc/StateSpaceUtil.h
Normal file
303
wpimath/src/main/native/include/frc/StateSpaceUtil.h
Normal file
@@ -0,0 +1,303 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <random>
|
||||
#include <type_traits>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "Eigen/QR"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
|
||||
namespace frc {
|
||||
namespace detail {
|
||||
|
||||
template <int Rows, int Cols, typename Matrix, typename T, typename... Ts>
|
||||
void MatrixImpl(Matrix& result, T elem, Ts... elems) {
|
||||
constexpr int count = Rows * Cols - (sizeof...(Ts) + 1);
|
||||
|
||||
result(count / Cols, count % Cols) = elem;
|
||||
if constexpr (sizeof...(Ts) > 0) {
|
||||
MatrixImpl<Rows, Cols>(result, elems...);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Matrix, typename T, typename... Ts>
|
||||
void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
|
||||
result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
|
||||
if constexpr (sizeof...(Ts) > 0) {
|
||||
CostMatrixImpl(result, elems...);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Matrix, typename T, typename... Ts>
|
||||
void CovMatrixImpl(Matrix& result, T elem, Ts... elems) {
|
||||
result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2);
|
||||
if constexpr (sizeof...(Ts) > 0) {
|
||||
CovMatrixImpl(result, elems...);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Matrix, typename T, typename... Ts>
|
||||
void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
|
||||
std::random_device rd;
|
||||
std::mt19937 gen{rd()};
|
||||
std::normal_distribution<> distr{0.0, elem};
|
||||
|
||||
result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen);
|
||||
if constexpr (sizeof...(Ts) > 0) {
|
||||
WhiteNoiseVectorImpl(result, 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);
|
||||
|
||||
for (int i = 0; i < States; ++i) {
|
||||
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
|
||||
es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
|
||||
1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
|
||||
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
|
||||
States>::Identity() -
|
||||
A,
|
||||
B;
|
||||
|
||||
Eigen::ColPivHouseholderQR<
|
||||
Eigen::Matrix<std::complex<double>, States, States + Inputs>>
|
||||
qr(E);
|
||||
if (qr.rank() < States) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
/**
|
||||
* Creates a matrix from the given list of elements.
|
||||
*
|
||||
* The elements of the matrix are filled in in row-major order.
|
||||
*
|
||||
* @param elems An array of elements in the matrix.
|
||||
* @return A matrix containing the given elements.
|
||||
*/
|
||||
template <int Rows, int Cols, typename... Ts,
|
||||
typename =
|
||||
std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
|
||||
Eigen::Matrix<double, 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;
|
||||
detail::MatrixImpl<Rows, Cols>(result, elems...);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a cost matrix from the given vector for use with LQR.
|
||||
*
|
||||
* The cost matrix is constructed using Bryson's rule. The inverse square of
|
||||
* each element in the input is taken and placed on the cost matrix diagonal.
|
||||
*
|
||||
* @param costs An array. For a Q matrix, its elements are the maximum allowed
|
||||
* excursions of the states from the reference. For an R matrix,
|
||||
* its elements are the maximum allowed excursions of the control
|
||||
* inputs from no actuation.
|
||||
* @return State excursion or control effort cost matrix.
|
||||
*/
|
||||
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... costs) {
|
||||
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
|
||||
detail::CostMatrixImpl(result.diagonal(), costs...);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a covariance matrix from the given vector for use with Kalman
|
||||
* filters.
|
||||
*
|
||||
* Each element is squared and placed on the covariance matrix diagonal.
|
||||
*
|
||||
* @param stdDevs An array. For a Q matrix, its elements are the standard
|
||||
* deviations of each state from how the model behaves. For an R
|
||||
* matrix, its elements are the standard deviations for each
|
||||
* output measurement.
|
||||
* @return Process noise or measurement noise covariance matrix.
|
||||
*/
|
||||
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) {
|
||||
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
|
||||
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a cost matrix from the given vector for use with LQR.
|
||||
*
|
||||
* The cost matrix is constructed using Bryson's rule. The inverse square of
|
||||
* each element in the input is taken and placed on the cost matrix diagonal.
|
||||
*
|
||||
* @param costs An array. For a Q matrix, its elements are the maximum allowed
|
||||
* excursions of the states from the reference. For an R matrix,
|
||||
* its elements are the maximum allowed excursions of the control
|
||||
* inputs from no actuation.
|
||||
* @return State excursion or control effort cost matrix.
|
||||
*/
|
||||
template <size_t N>
|
||||
Eigen::Matrix<double, 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) {
|
||||
diag(i) = 1.0 / std::pow(costs[i], 2);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a covariance matrix from the given vector for use with Kalman
|
||||
* filters.
|
||||
*
|
||||
* Each element is squared and placed on the covariance matrix diagonal.
|
||||
*
|
||||
* @param stdDevs An array. For a Q matrix, its elements are the standard
|
||||
* deviations of each state from how the model behaves. For an R
|
||||
* matrix, its elements are the standard deviations for each
|
||||
* output measurement.
|
||||
* @return Process noise or measurement noise covariance matrix.
|
||||
*/
|
||||
template <size_t N>
|
||||
Eigen::Matrix<double, 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) {
|
||||
diag(i) = std::pow(stdDevs[i], 2);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
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;
|
||||
detail::WhiteNoiseVectorImpl(result, stdDevs...);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a vector of normally distributed white noise with the given noise
|
||||
* intensities for each element.
|
||||
*
|
||||
* @param stdDevs An array whose elements are the standard deviations of each
|
||||
* element of the noise vector.
|
||||
* @return White noise vector.
|
||||
*/
|
||||
template <int N>
|
||||
Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
|
||||
const std::array<double, N>& stdDevs) {
|
||||
std::random_device rd;
|
||||
std::mt19937 gen{rd()};
|
||||
|
||||
Eigen::Matrix<double, N, 1> result;
|
||||
for (int i = 0; i < N; ++i) {
|
||||
std::normal_distribution<> distr{0.0, stdDevs[i]};
|
||||
result(i) = distr(gen);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if (A, B) is a stabilizable pair.
|
||||
*
|
||||
* (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
|
||||
* any, have absolute values less than one, where an eigenvalue is
|
||||
* uncontrollable if rank(lambda * I - A, B) < n where n is number of states.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @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) {
|
||||
return detail::IsStabilizableImpl<States, Inputs>(A, B);
|
||||
}
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B);
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B);
|
||||
|
||||
/**
|
||||
* Converts a Pose2d into a vector of [x, y, theta].
|
||||
*
|
||||
* @param pose The pose that is being represented.
|
||||
*
|
||||
* @return The vector.
|
||||
*/
|
||||
Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
|
||||
|
||||
/**
|
||||
* Clamps input vector between system's minimum and maximum allowable input.
|
||||
*
|
||||
* @param u Input vector to clamp.
|
||||
* @return Clamped input vector.
|
||||
*/
|
||||
template <int Inputs>
|
||||
Eigen::Matrix<double, Inputs, 1> ClampInputMaxMagnitude(
|
||||
const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Inputs, 1>& umin,
|
||||
const Eigen::Matrix<double, Inputs, 1>& umax) {
|
||||
Eigen::Matrix<double, Inputs, 1> result;
|
||||
for (int i = 0; i < Inputs; ++i) {
|
||||
result(i) = std::clamp(u(i), umin(i), umax(i));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems
|
||||
* such as differential drivetrains.
|
||||
*
|
||||
* @param u The input vector.
|
||||
* @param maxMagnitude The maximum magnitude any input can have.
|
||||
* @param <I> The number of inputs.
|
||||
* @return The normalizedInput
|
||||
*/
|
||||
template <int Inputs>
|
||||
Eigen::Matrix<double, Inputs, 1> NormalizeInputVector(
|
||||
const Eigen::Matrix<double, Inputs, 1>& u, double maxMagnitude) {
|
||||
double maxValue = u.template lpNorm<Eigen::Infinity>();
|
||||
|
||||
if (maxValue > maxMagnitude) {
|
||||
return u * maxMagnitude / maxValue;
|
||||
}
|
||||
return u;
|
||||
}
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,202 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Constructs a control-affine plant inversion model-based feedforward from
|
||||
* given model dynamics.
|
||||
*
|
||||
* If given the vector valued function as f(x, u) where x is the state
|
||||
* vector and u is the input vector, the B matrix(continuous input matrix)
|
||||
* is calculated through a NumericalJacobian. In this case f has to be
|
||||
* control-affine (of the form f(x) + Bu).
|
||||
*
|
||||
* The feedforward is calculated as
|
||||
* <strong> u_ff = B<sup>+</sup> (rDot - f(x)) </strong>, where <strong>
|
||||
* B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* This feedforward does not account for a dynamic B matrix, B is either
|
||||
* determined or supplied when the feedforward is created and remains constant.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class ControlAffinePlantInversionFeedforward {
|
||||
public:
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function
|
||||
* of state and input.
|
||||
*
|
||||
* @param f A vector-valued function of x, the state, and
|
||||
* u, the input, that returns the derivative of
|
||||
* the state vector. HAS to be control-affine
|
||||
* (of the form f(x) + Bu).
|
||||
* @param dt The timestep between calls of calculate().
|
||||
*/
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
f,
|
||||
units::second_t dt)
|
||||
: m_dt(dt), m_f(f) {
|
||||
m_B = NumericalJacobianU<States, States, Inputs>(
|
||||
f, Eigen::Matrix<double, States, 1>::Zero(),
|
||||
Eigen::Matrix<double, Inputs, 1>::Zero());
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function of state,
|
||||
* and the plant's B matrix(continuous input matrix).
|
||||
*
|
||||
* @param f A vector-valued function of x, the state,
|
||||
* that returns the derivative of the state vector.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dt The timestep between calls of calculate().
|
||||
*/
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&)>
|
||||
f,
|
||||
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
|
||||
: m_B(B), m_dt(dt) {
|
||||
m_f = [=](const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& u)
|
||||
-> Eigen::Matrix<double, States, 1> { return f(x); };
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
ControlAffinePlantInversionFeedforward(
|
||||
ControlAffinePlantInversionFeedforward&&) = default;
|
||||
ControlAffinePlantInversionFeedforward& operator=(
|
||||
ControlAffinePlantInversionFeedforward&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
double Uff(int i) const { return m_uff(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* If this method is used the initial state of the system is the one
|
||||
* set using Reset(const Eigen::Matrix<double, States, 1>&).
|
||||
* 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::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
return Calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& r,
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
Eigen::Matrix<double, States, 1> rDot = (nextR - r) / m_dt.to<double>();
|
||||
|
||||
m_uff = m_B.householderQr().solve(
|
||||
rDot - m_f(r, Eigen::Matrix<double, Inputs, 1>::Zero()));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
private:
|
||||
Eigen::Matrix<double, States, Inputs> m_B;
|
||||
|
||||
units::second_t m_dt;
|
||||
|
||||
/**
|
||||
* The model dynamics.
|
||||
*/
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_f;
|
||||
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed feedforward
|
||||
Eigen::Matrix<double, Inputs, 1> m_uff;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,162 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Constructs a plant inversion model-based feedforward from a LinearSystem.
|
||||
*
|
||||
* The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A
|
||||
* r_k) </strong>, where <strong> B<sup>+</sup> </strong> is the pseudoinverse
|
||||
* of B.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearPlantInversionFeedforward {
|
||||
public:
|
||||
/**
|
||||
* Constructs a feedforward with the given plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearPlantInversionFeedforward(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt)
|
||||
: LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given coefficients.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
LinearPlantInversionFeedforward(
|
||||
const Eigen::Matrix<double, States, States>& A,
|
||||
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
|
||||
: m_dt(dt) {
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
|
||||
|
||||
m_r.setZero();
|
||||
Reset(m_r);
|
||||
}
|
||||
|
||||
LinearPlantInversionFeedforward(LinearPlantInversionFeedforward&&) = default;
|
||||
LinearPlantInversionFeedforward& operator=(
|
||||
LinearPlantInversionFeedforward&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
double Uff(int i) const { return m_uff(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_uff.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* If this method is used the initial state of the system is the one
|
||||
* set using Reset(const Eigen::Matrix<double, States, 1>&).
|
||||
* 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::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
return Calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& r,
|
||||
const Eigen::Matrix<double, States, 1>& 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;
|
||||
|
||||
units::second_t m_dt;
|
||||
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed feedforward
|
||||
Eigen::Matrix<double, Inputs, 1> m_uff;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,422 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/QR"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
namespace detail {
|
||||
|
||||
/**
|
||||
* Contains the controller coefficients and logic for a linear-quadratic
|
||||
* regulator (LQR).
|
||||
* LQRs use the control law u = K(r - x).
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
template <int States, int Inputs>
|
||||
class LinearQuadraticRegulatorImpl {
|
||||
public:
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulatorImpl(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems, units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulatorImpl(
|
||||
const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems, const double rho,
|
||||
const std::array<double, Inputs>& Relems, units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @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 std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(A, B, Qelems, 1.0, Relems, dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @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 std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems) * rho,
|
||||
MakeCostMatrix(Relems), dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @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,
|
||||
units::second_t dt) {
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, Inputs> discB;
|
||||
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
|
||||
|
||||
Eigen::Matrix<double, States, States> S =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
Eigen::Matrix<double, Inputs, Inputs> tmp =
|
||||
discB.transpose() * S * discB + R;
|
||||
m_K = tmp.llt().solve(discB.transpose() * S * discA);
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
|
||||
LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
|
||||
default;
|
||||
|
||||
/**
|
||||
* Returns the controller matrix K.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
|
||||
|
||||
/**
|
||||
* Returns an element of the controller matrix K.
|
||||
*
|
||||
* @param i Row of K.
|
||||
* @param j Column of K.
|
||||
*/
|
||||
double K(int i, int j) const { return m_K(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the reference vector r.
|
||||
*
|
||||
* @return The reference vector.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*
|
||||
* @return The row of the reference vector.
|
||||
*/
|
||||
double R(int i) const { return m_r(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the control input vector u.
|
||||
*
|
||||
* @return The control input.
|
||||
*/
|
||||
const Eigen::Matrix<double, Inputs, 1>& U() const { return m_u; }
|
||||
|
||||
/**
|
||||
* Returns an element of the control input vector u.
|
||||
*
|
||||
* @param i Row of u.
|
||||
*
|
||||
* @return The row of the control input vector.
|
||||
*/
|
||||
double U(int i) const { return m_u(i, 0); }
|
||||
|
||||
/**
|
||||
* Resets the controller.
|
||||
*/
|
||||
void Reset() {
|
||||
m_r.setZero();
|
||||
m_u.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& x) {
|
||||
m_u = m_K * (m_r - x);
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
* @param nextR The next reference vector r.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> Calculate(
|
||||
const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
m_r = nextR;
|
||||
return Calculate(x);
|
||||
}
|
||||
|
||||
private:
|
||||
// Current reference
|
||||
Eigen::Matrix<double, States, 1> m_r;
|
||||
|
||||
// Computed controller output
|
||||
Eigen::Matrix<double, Inputs, 1> m_u;
|
||||
|
||||
// Controller gain
|
||||
Eigen::Matrix<double, Inputs, States> m_K;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
template <int States, int Inputs>
|
||||
class LinearQuadraticRegulator
|
||||
: public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param system The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param system The plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @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,
|
||||
const std::array<double, States>& Qelems,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state
|
||||
* excursion. Greater values penalize state excursion more heavily. 1 is a
|
||||
* good starting value.
|
||||
* @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,
|
||||
const std::array<double, States>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, Inputs>& Relems,
|
||||
units::second_t dt)
|
||||
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{
|
||||
A, B, Qelems, rho, Relems, dt} {}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @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)
|
||||
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
class LinearQuadraticRegulator<1, 1>
|
||||
: public detail::LinearQuadraticRegulatorImpl<1, 1> {
|
||||
public:
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
|
||||
const Eigen::Matrix<double, 1, 1>& B,
|
||||
const std::array<double, 1>& Qelems,
|
||||
const double rho,
|
||||
const std::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,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input pairs
|
||||
// compile faster.
|
||||
template <>
|
||||
class LinearQuadraticRegulator<2, 1>
|
||||
: public detail::LinearQuadraticRegulatorImpl<2, 1> {
|
||||
public:
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems,
|
||||
dt) {}
|
||||
|
||||
template <int Outputs>
|
||||
LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const double rho,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt)
|
||||
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems,
|
||||
dt) {}
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const std::array<double, 1>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
|
||||
const Eigen::Matrix<double, 2, 1>& B,
|
||||
const std::array<double, 2>& Qelems,
|
||||
const double rho,
|
||||
const std::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,
|
||||
units::second_t dt);
|
||||
|
||||
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
|
||||
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,230 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "frc/system/RungeKutta.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class ExtendedKalmanFilter {
|
||||
public:
|
||||
/**
|
||||
* Constructs an extended Kalman filter.
|
||||
*
|
||||
* @param f A vector-valued function of x and u that returns
|
||||
* the derivative of the state vector.
|
||||
* @param h A vector-valued function of x and u that returns
|
||||
* the measurement vector.
|
||||
* @param stateStdDevs Standard deviations of model states.
|
||||
* @param measurementStdDevs Standard deviations of measurements.
|
||||
* @param dt Nominal discretization timestep.
|
||||
*/
|
||||
ExtendedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
f,
|
||||
std::function<Eigen::Matrix<double, Outputs, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
h,
|
||||
const std::array<double, States>& stateStdDevs,
|
||||
const std::array<double, Outputs>& measurementStdDevs,
|
||||
units::second_t dt)
|
||||
: m_f(f), m_h(h) {
|
||||
m_contQ = MakeCovMatrix(stateStdDevs);
|
||||
m_contR = MakeCovMatrix(measurementStdDevs);
|
||||
|
||||
Reset();
|
||||
|
||||
Eigen::Matrix<double, States, States> contA =
|
||||
NumericalJacobianX<States, States, Inputs>(
|
||||
m_f, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
|
||||
Eigen::Matrix<double, Outputs, States> C =
|
||||
NumericalJacobianX<Outputs, States, Inputs>(
|
||||
m_h, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
|
||||
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
|
||||
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
|
||||
bool isObservable =
|
||||
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
|
||||
if (isObservable && Outputs <= States) {
|
||||
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, m_discR);
|
||||
} else {
|
||||
m_initP = Eigen::Matrix<double, States, States>::Zero();
|
||||
}
|
||||
m_P = m_initP;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the error covariance matrix P.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
|
||||
|
||||
/**
|
||||
* Returns an element of the error covariance matrix P.
|
||||
*
|
||||
* @param i Row of P.
|
||||
* @param j Column of P.
|
||||
*/
|
||||
double P(int i, int j) const { return m_P(i, j); }
|
||||
|
||||
/**
|
||||
* Set the current error covariance matrix P.
|
||||
*
|
||||
* @param P The error covariance matrix P.
|
||||
*/
|
||||
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
|
||||
|
||||
/**
|
||||
* Returns the state estimate x-hat.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
|
||||
|
||||
/**
|
||||
* Returns an element of the state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
*/
|
||||
double Xhat(int i) const { return m_xHat(i, 0); }
|
||||
|
||||
/**
|
||||
* Set initial state estimate x-hat.
|
||||
*
|
||||
* @param xHat The state estimate x-hat.
|
||||
*/
|
||||
void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
|
||||
|
||||
/**
|
||||
* Set an element of the initial state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
* @param value Value for element of x-hat.
|
||||
*/
|
||||
void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
|
||||
|
||||
/**
|
||||
* Resets the observer.
|
||||
*/
|
||||
void Reset() {
|
||||
m_xHat.setZero();
|
||||
m_P = m_initP;
|
||||
}
|
||||
|
||||
/**
|
||||
* Project the model into the future with a new control input u.
|
||||
*
|
||||
* @param u New control input from controller.
|
||||
* @param dt Timestep for prediction.
|
||||
*/
|
||||
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
|
||||
// Find continuous A
|
||||
Eigen::Matrix<double, States, States> 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;
|
||||
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
|
||||
|
||||
m_xHat = RungeKutta(m_f, m_xHat, u, dt);
|
||||
m_P = discA * m_P * discA.transpose() + discQ;
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* @param u Same control input used in the predict step.
|
||||
* @param y Measurement vector.
|
||||
*/
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Outputs, 1>& y) {
|
||||
Correct<Outputs>(u, y, m_h, m_discR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* This is useful for when the measurements available during a timestep's
|
||||
* Correct() call vary. The h(x, u) passed to the constructor is used if one
|
||||
* is not provided (the two-argument version of this function).
|
||||
*
|
||||
* @param u Same control input used in the predict step.
|
||||
* @param y Measurement vector.
|
||||
* @param h A vector-valued function of x and u that returns
|
||||
* the measurement vector.
|
||||
* @param R Discrete measurement noise covariance matrix.
|
||||
*/
|
||||
template <int Rows>
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Rows, 1>& y,
|
||||
std::function<Eigen::Matrix<double, Rows, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
h,
|
||||
const Eigen::Matrix<double, Rows, Rows>& R) {
|
||||
const Eigen::Matrix<double, Rows, States> C =
|
||||
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
|
||||
|
||||
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
|
||||
// efficiently.
|
||||
//
|
||||
// K = PC^T S^-1
|
||||
// KS = PC^T
|
||||
// (KS)^T = (PC^T)^T
|
||||
// S^T K^T = CP^T
|
||||
//
|
||||
// The solution of Ax = b can be found via x = A.solve(b).
|
||||
//
|
||||
// K^T = S^T.solve(CP^T)
|
||||
// K = (S^T.solve(CP^T))^T
|
||||
Eigen::Matrix<double, States, Rows> K =
|
||||
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
|
||||
|
||||
m_xHat += K * (y - h(m_xHat, u));
|
||||
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
|
||||
}
|
||||
|
||||
private:
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_f;
|
||||
std::function<Eigen::Matrix<double, Outputs, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_h;
|
||||
Eigen::Matrix<double, States, 1> 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, Outputs, Outputs> m_discR;
|
||||
|
||||
Eigen::Matrix<double, States, States> m_initP;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
291
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
Normal file
291
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
Normal file
@@ -0,0 +1,291 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "drake/math/discrete_algebraic_riccati_equation.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
namespace frc {
|
||||
namespace detail {
|
||||
|
||||
/**
|
||||
* Luenberger observers combine predictions from a model and measurements to
|
||||
* give an estimate of the true system state.
|
||||
*
|
||||
* Luenberger observers use an L gain matrix to determine whether to trust the
|
||||
* model or measurements more. Kalman filter theory uses statistics to compute
|
||||
* an optimal L gain (alternatively called the Kalman gain, K) which minimizes
|
||||
* the sum of squares error in the state estimate.
|
||||
*
|
||||
* Luenberger observers run the prediction and correction steps simultaneously
|
||||
* while Kalman filters run them sequentially. To implement a discrete-time
|
||||
* Kalman filter as a Luenberger observer, use the following mapping:
|
||||
* <pre>C = H, L = A * K</pre>
|
||||
* (H is the measurement matrix).
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/state-space-guide.pdf.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class KalmanFilterImpl {
|
||||
public:
|
||||
/**
|
||||
* Constructs a state-space observer with the given plant.
|
||||
*
|
||||
* @param plant The plant used for the prediction step.
|
||||
* @param stateStdDevs Standard deviations of model states.
|
||||
* @param measurementStdDevs Standard deviations of measurements.
|
||||
* @param dt Nominal discretization timestep.
|
||||
*/
|
||||
KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& stateStdDevs,
|
||||
const std::array<double, Outputs>& measurementStdDevs,
|
||||
units::second_t dt) {
|
||||
m_plant = &plant;
|
||||
|
||||
m_contQ = MakeCovMatrix(stateStdDevs);
|
||||
m_contR = MakeCovMatrix(measurementStdDevs);
|
||||
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(plant.A(), m_contQ, dt, &discA, &discQ);
|
||||
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
|
||||
bool isObservable = IsStabilizable<States, Outputs>(discA.transpose(),
|
||||
plant.C().transpose());
|
||||
if (isObservable) {
|
||||
if (Outputs <= States) {
|
||||
m_P = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), plant.C().transpose(), discQ, m_discR);
|
||||
} else {
|
||||
m_P.setZero();
|
||||
}
|
||||
} else {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"The system passed to the Kalman Filter is not observable!");
|
||||
throw std::invalid_argument(
|
||||
"The system passed to the Kalman Filter is not observable!");
|
||||
}
|
||||
}
|
||||
|
||||
KalmanFilterImpl(KalmanFilterImpl&&) = default;
|
||||
KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the error covariance matrix P.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
|
||||
|
||||
/**
|
||||
* Returns an element of the error covariance matrix P.
|
||||
*
|
||||
* @param i Row of P.
|
||||
* @param j Column of P.
|
||||
*/
|
||||
double P(int i, int j) const { return m_P(i, j); }
|
||||
|
||||
/**
|
||||
* Set the current error covariance matrix P.
|
||||
*
|
||||
* @param P The error covariance matrix P.
|
||||
*/
|
||||
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
|
||||
|
||||
/**
|
||||
* Returns the state estimate x-hat.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
|
||||
|
||||
/**
|
||||
* Returns an element of the state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
*/
|
||||
double Xhat(int i) const { return m_xHat(i); }
|
||||
|
||||
/**
|
||||
* Set initial state estimate x-hat.
|
||||
*
|
||||
* @param xHat The state estimate x-hat.
|
||||
*/
|
||||
void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
|
||||
|
||||
/**
|
||||
* Set an element of the initial state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
* @param value Value for element of x-hat.
|
||||
*/
|
||||
void SetXhat(int i, double value) { m_xHat(i) = value; }
|
||||
|
||||
/**
|
||||
* Resets the observer.
|
||||
*/
|
||||
void Reset() { m_xHat.setZero(); }
|
||||
|
||||
/**
|
||||
* Project the model into the future with a new control input u.
|
||||
*
|
||||
* @param u New control input from controller.
|
||||
* @param dt Timestep for prediction.
|
||||
*/
|
||||
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
|
||||
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
|
||||
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
|
||||
|
||||
m_P = discA * m_P * discA.transpose() + discQ;
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* @param u Same control input used in the last predict step.
|
||||
* @param y Measurement vector.
|
||||
*/
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Outputs, 1>& y) {
|
||||
Correct<Outputs>(u, y, m_plant->C(), m_plant->D(), m_discR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* This is useful for when the measurements available during a timestep's
|
||||
* Correct() call vary. The C matrix passed to the constructor is used if one
|
||||
* is not provided (the two-argument version of this function).
|
||||
*
|
||||
* @param u Same control input used in the predict step.
|
||||
* @param y Measurement vector.
|
||||
* @param C Output matrix.
|
||||
* @param D Feedthrough matrix.
|
||||
* @param R Measurement noise covariance matrix.
|
||||
*/
|
||||
template <int Rows>
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Rows, 1>& y,
|
||||
const Eigen::Matrix<double, Rows, States>& C,
|
||||
const Eigen::Matrix<double, Rows, Inputs>& D,
|
||||
const Eigen::Matrix<double, Rows, Rows>& R) {
|
||||
const auto& x = m_xHat;
|
||||
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
|
||||
// efficiently.
|
||||
//
|
||||
// K = PC^T S^-1
|
||||
// KS = PC^T
|
||||
// (KS)^T = (PC^T)^T
|
||||
// S^T K^T = CP^T
|
||||
//
|
||||
// The solution of Ax = b can be found via x = A.solve(b).
|
||||
//
|
||||
// K^T = S^T.solve(CP^T)
|
||||
// K = (S^T.solve(CP^T))^T
|
||||
Eigen::Matrix<double, States, Rows> K =
|
||||
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
|
||||
|
||||
m_xHat = x + K * (y - (C * x + D * u));
|
||||
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
|
||||
}
|
||||
|
||||
private:
|
||||
LinearSystem<States, Inputs, Outputs>* m_plant;
|
||||
|
||||
/**
|
||||
* Error covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, States> m_P;
|
||||
|
||||
/**
|
||||
* Continuous process noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, States> m_contQ;
|
||||
|
||||
/**
|
||||
* Continuous measurement noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, Outputs> m_contR;
|
||||
|
||||
/**
|
||||
* Discrete measurement noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, Outputs> m_discR;
|
||||
|
||||
/**
|
||||
* State estimate x-hat.
|
||||
*/
|
||||
Eigen::Matrix<double, States, 1> m_xHat;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a state-space observer with the given plant.
|
||||
*
|
||||
* @param plant The plant used for the prediction step.
|
||||
* @param stateStdDevs Standard deviations of model states.
|
||||
* @param measurementStdDevs Standard deviations of measurements.
|
||||
* @param dt Nominal discretization timestep.
|
||||
*/
|
||||
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
|
||||
const std::array<double, States>& stateStdDevs,
|
||||
const std::array<double, Outputs>& measurementStdDevs,
|
||||
units::second_t dt)
|
||||
: detail::KalmanFilterImpl<States, Inputs, Outputs>{
|
||||
plant, stateStdDevs, measurementStdDevs, dt} {}
|
||||
|
||||
KalmanFilter(KalmanFilter&&) = default;
|
||||
KalmanFilter& operator=(KalmanFilter&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input-output
|
||||
// triplets compile faster.
|
||||
template <>
|
||||
class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> {
|
||||
public:
|
||||
KalmanFilter(LinearSystem<1, 1, 1>& plant,
|
||||
const std::array<double, 1>& stateStdDevs,
|
||||
const std::array<double, 1>& measurementStdDevs,
|
||||
units::second_t dt);
|
||||
|
||||
KalmanFilter(KalmanFilter&&) = default;
|
||||
KalmanFilter& operator=(KalmanFilter&&) = default;
|
||||
};
|
||||
|
||||
// Template specializations are used here to make common state-input-output
|
||||
// triplets compile faster.
|
||||
template <>
|
||||
class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> {
|
||||
public:
|
||||
KalmanFilter(LinearSystem<2, 1, 1>& plant,
|
||||
const std::array<double, 2>& stateStdDevs,
|
||||
const std::array<double, 1>& measurementStdDevs,
|
||||
units::second_t dt);
|
||||
|
||||
KalmanFilter(KalmanFilter&&) = default;
|
||||
KalmanFilter& operator=(KalmanFilter&&) = default;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,134 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Generates sigma points and weights according to Van der Merwe's 2004
|
||||
* dissertation[1] for the UnscentedKalmanFilter class.
|
||||
*
|
||||
* It parametrizes the sigma points using alpha, beta, kappa terms, and is the
|
||||
* version seen in most publications. Unless you know better, this should be
|
||||
* your default choice.
|
||||
*
|
||||
* @tparam States The dimensionality of the state. 2*States+1 weights will be
|
||||
* generated.
|
||||
*
|
||||
* [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
|
||||
* Inference in Dynamic State-Space Models" (Doctoral dissertation)
|
||||
*/
|
||||
template <int States>
|
||||
class MerweScaledSigmaPoints {
|
||||
public:
|
||||
/**
|
||||
* Constructs a generator for Van der Merwe scaled sigma points.
|
||||
*
|
||||
* @param alpha Determines the spread of the sigma points around the mean.
|
||||
* Usually a small positive value (1e-3).
|
||||
* @param beta Incorporates prior knowledge of the distribution of the mean.
|
||||
* For Gaussian distributions, beta = 2 is optimal.
|
||||
* @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
|
||||
*/
|
||||
MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
|
||||
int kappa = 3 - States) {
|
||||
m_alpha = alpha;
|
||||
m_kappa = kappa;
|
||||
|
||||
ComputeWeights(beta);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns number of sigma points for each variable in the state x.
|
||||
*/
|
||||
int NumSigmas() { return 2 * States + 1; }
|
||||
|
||||
/**
|
||||
* Computes the sigma points for an unscented Kalman filter given the mean
|
||||
* (x) and covariance(P) of the filter.
|
||||
*
|
||||
* @param x An array of the means.
|
||||
* @param P Covariance of the filter.
|
||||
*
|
||||
* @return Two dimensional array of sigma points. Each column contains all of
|
||||
* the sigmas for one dimension in the problem space. Ordered by
|
||||
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
|
||||
*
|
||||
*/
|
||||
Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
|
||||
const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, 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();
|
||||
|
||||
Eigen::Matrix<double, 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) =
|
||||
x + U.template block<States, 1>(0, k);
|
||||
sigmas.template block<States, 1>(0, States + k + 1) =
|
||||
x - U.template block<States, 1>(0, k);
|
||||
}
|
||||
|
||||
return sigmas;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the weight for each sigma point for the mean.
|
||||
*/
|
||||
const Eigen::Matrix<double, 2 * States + 1, 1>& Wm() const { return m_Wm; }
|
||||
|
||||
/**
|
||||
* Returns an element of the weight for each sigma point for the mean.
|
||||
*
|
||||
* @param i Element of vector to return.
|
||||
*/
|
||||
double Wm(int i) const { return m_Wm(i, 0); }
|
||||
|
||||
/**
|
||||
* Returns the weight for each sigma point for the covariance.
|
||||
*/
|
||||
const Eigen::Matrix<double, 2 * States + 1, 1>& Wc() const { return m_Wc; }
|
||||
|
||||
/**
|
||||
* Returns an element of the weight for each sigma point for the covariance.
|
||||
*
|
||||
* @param i Element of vector to return.
|
||||
*/
|
||||
double Wc(int i) const { return m_Wc(i, 0); }
|
||||
|
||||
private:
|
||||
Eigen::Matrix<double, 2 * States + 1, 1> m_Wm;
|
||||
Eigen::Matrix<double, 2 * States + 1, 1> m_Wc;
|
||||
double m_alpha;
|
||||
int m_kappa;
|
||||
|
||||
/**
|
||||
* Computes the weights for the scaled unscented Kalman filter.
|
||||
*
|
||||
* @param beta Incorporates prior knowledge of the distribution of the mean.
|
||||
*/
|
||||
void ComputeWeights(double beta) {
|
||||
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
|
||||
|
||||
double c = 0.5 / (States + lambda);
|
||||
m_Wm = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
|
||||
m_Wc = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
|
||||
|
||||
m_Wm(0) = lambda / (States + lambda);
|
||||
m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,233 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/MerweScaledSigmaPoints.h"
|
||||
#include "frc/estimator/UnscentedTransform.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "frc/system/RungeKutta.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class UnscentedKalmanFilter {
|
||||
public:
|
||||
/**
|
||||
* Constructs an unscented Kalman filter.
|
||||
*
|
||||
* @param f A vector-valued function of x and u that returns
|
||||
* the derivative of the state vector.
|
||||
* @param h A vector-valued function of x and u that returns
|
||||
* the measurement vector.
|
||||
* @param stateStdDevs Standard deviations of model states.
|
||||
* @param measurementStdDevs Standard deviations of measurements.
|
||||
* @param dt Nominal discretization timestep.
|
||||
*/
|
||||
UnscentedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
f,
|
||||
std::function<Eigen::Matrix<double, Outputs, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
h,
|
||||
const std::array<double, States>& stateStdDevs,
|
||||
const std::array<double, Outputs>& measurementStdDevs,
|
||||
units::second_t dt)
|
||||
: m_f(f), m_h(h) {
|
||||
m_contQ = MakeCovMatrix(stateStdDevs);
|
||||
m_contR = MakeCovMatrix(measurementStdDevs);
|
||||
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the error covariance matrix P.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
|
||||
|
||||
/**
|
||||
* Returns an element of the error covariance matrix P.
|
||||
*
|
||||
* @param i Row of P.
|
||||
* @param j Column of P.
|
||||
*/
|
||||
double P(int i, int j) const { return m_P(i, j); }
|
||||
|
||||
/**
|
||||
* Set the current error covariance matrix P.
|
||||
*
|
||||
* @param P The error covariance matrix P.
|
||||
*/
|
||||
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
|
||||
|
||||
/**
|
||||
* Returns the state estimate x-hat.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
|
||||
|
||||
/**
|
||||
* Returns an element of the state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
*/
|
||||
double Xhat(int i) const { return m_xHat(i, 0); }
|
||||
|
||||
/**
|
||||
* Set initial state estimate x-hat.
|
||||
*
|
||||
* @param xHat The state estimate x-hat.
|
||||
*/
|
||||
void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
|
||||
|
||||
/**
|
||||
* Set an element of the initial state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
* @param value Value for element of x-hat.
|
||||
*/
|
||||
void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
|
||||
|
||||
/**
|
||||
* Resets the observer.
|
||||
*/
|
||||
void Reset() {
|
||||
m_xHat.setZero();
|
||||
m_P.setZero();
|
||||
m_sigmasF.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Project the model into the future with a new control input u.
|
||||
*
|
||||
* @param u New control input from controller.
|
||||
* @param dt Timestep for prediction.
|
||||
*/
|
||||
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
|
||||
// Discretize Q before projecting mean and covariance forward
|
||||
Eigen::Matrix<double, States, States> contA =
|
||||
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
|
||||
|
||||
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
|
||||
m_pts.SigmaPoints(m_xHat, m_P);
|
||||
|
||||
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
|
||||
Eigen::Matrix<double, States, 1> x =
|
||||
sigmas.template block<States, 1>(0, i);
|
||||
m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
|
||||
}
|
||||
|
||||
auto ret =
|
||||
UnscentedTransform<States, States>(m_sigmasF, m_pts.Wm(), m_pts.Wc());
|
||||
m_xHat = std::get<0>(ret);
|
||||
m_P = std::get<1>(ret);
|
||||
|
||||
m_P += discQ;
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* @param u Same control input used in the predict step.
|
||||
* @param y Measurement vector.
|
||||
*/
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Outputs, 1>& y) {
|
||||
Correct<Outputs>(u, y, m_h, m_discR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* This is useful for when the measurements available during a timestep's
|
||||
* Correct() call vary. The h(x, u) passed to the constructor is used if one
|
||||
* is not provided (the two-argument version of this function).
|
||||
*
|
||||
* @param u Same control input used in the predict step.
|
||||
* @param y Measurement vector.
|
||||
* @param h A vector-valued function of x and u that returns
|
||||
* the measurement vector.
|
||||
* @param R Measurement noise covariance matrix.
|
||||
*/
|
||||
template <int Rows>
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Rows, 1>& y,
|
||||
std::function<Eigen::Matrix<double, Rows, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
h,
|
||||
const Eigen::Matrix<double, Rows, Rows>& R) {
|
||||
// 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);
|
||||
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);
|
||||
}
|
||||
|
||||
// Mean and covariance of prediction passed through UT
|
||||
auto [yHat, Py] =
|
||||
UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
|
||||
Py += R;
|
||||
|
||||
// Compute cross covariance of the state and the measurements
|
||||
Eigen::Matrix<double, States, Rows> Pxy;
|
||||
Pxy.setZero();
|
||||
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
|
||||
Pxy += m_pts.Wc(i) *
|
||||
(m_sigmasF.template block<States, 1>(0, i) - m_xHat) *
|
||||
(sigmasH.template block<Rows, 1>(0, i) - yHat).transpose();
|
||||
}
|
||||
|
||||
// K = P_{xy} Py^-1
|
||||
// K^T = P_y^T^-1 P_{xy}^T
|
||||
// P_y^T K^T = P_{xy}^T
|
||||
// K^T = P_y^T.solve(P_{xy}^T)
|
||||
// K = (P_y^T.solve(P_{xy}^T)^T
|
||||
Eigen::Matrix<double, States, Rows> K =
|
||||
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
|
||||
|
||||
m_xHat += K * (y - yHat);
|
||||
m_P -= K * Py * K.transpose();
|
||||
}
|
||||
|
||||
private:
|
||||
std::function<Eigen::Matrix<double, States, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_f;
|
||||
std::function<Eigen::Matrix<double, Outputs, 1>(
|
||||
const Eigen::Matrix<double, States, 1>&,
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_h;
|
||||
Eigen::Matrix<double, States, 1> 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, Outputs, Outputs> m_discR;
|
||||
Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
|
||||
|
||||
MerweScaledSigmaPoints<States> m_pts;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,55 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <tuple>
|
||||
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Computes unscented transform of a set of sigma points and weights. CovDimurns
|
||||
* the mean and covariance in a tuple.
|
||||
*
|
||||
* This works in conjunction with the UnscentedKalmanFilter class.
|
||||
*
|
||||
* @tparam States Number of states.
|
||||
* @tparam CovDim Dimension of covariance of sigma points after passing through
|
||||
* the transform.
|
||||
* @param sigmas List of sigma points.
|
||||
* @param Wm Weights for the mean.
|
||||
* @param Wc Weights for the covariance.
|
||||
*
|
||||
* @return Tuple of x, mean of sigma points; P, covariance of sigma points after
|
||||
* passing through the transform.
|
||||
*/
|
||||
template <int States, int CovDim>
|
||||
std::tuple<Eigen::Matrix<double, CovDim, 1>,
|
||||
Eigen::Matrix<double, CovDim, CovDim>>
|
||||
UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
|
||||
const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
|
||||
const Eigen::Matrix<double, 2 * States + 1, 1>& Wc) {
|
||||
// New mean is just the sum of the sigmas * weight
|
||||
// dot = \Sigma^n_1 (W[k]*Xi[k])
|
||||
Eigen::Matrix<double, CovDim, 1> x = sigmas * Wm;
|
||||
|
||||
// New covariance is the sum of the outer product of the residuals times the
|
||||
// weights
|
||||
Eigen::Matrix<double, CovDim, 2 * States + 1> y;
|
||||
for (int i = 0; i < 2 * States + 1; ++i) {
|
||||
y.template block<CovDim, 1>(0, i) =
|
||||
sigmas.template block<CovDim, 1>(0, i) - x;
|
||||
}
|
||||
Eigen::Matrix<double, CovDim, CovDim> P =
|
||||
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
|
||||
|
||||
return std::make_tuple(x, P);
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
165
wpimath/src/main/native/include/frc/system/Discretization.h
Normal file
165
wpimath/src/main/native/include/frc/system/Discretization.h
Normal file
@@ -0,0 +1,165 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "units/time.h"
|
||||
#include "unsupported/Eigen/MatrixFunctions"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A matrix.
|
||||
*
|
||||
* @param contA Continuous system matrix.
|
||||
* @param dt Discretization timestep.
|
||||
* @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) {
|
||||
*discA = (contA * dt.to<double>()).exp();
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and B matrices.
|
||||
*
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contB Continuous input matrix.
|
||||
* @param dt Discretization timestep.
|
||||
* @param discA Storage for discrete system matrix.
|
||||
* @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) {
|
||||
// Matrices are blocked here to minimize matrix exponentiation calculations
|
||||
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
|
||||
Mcont.setZero();
|
||||
Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
|
||||
Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
|
||||
|
||||
// Discretize A and B with the given timestep
|
||||
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
|
||||
*discA = Mdisc.template block<States, States>(0, 0);
|
||||
*discB = Mdisc.template block<States, Inputs>(0, States);
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and Q matrices.
|
||||
*
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contQ Continuous process noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
* @param discA Storage for discrete system matrix.
|
||||
* @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) {
|
||||
// Make continuous Q symmetric if it isn't already
|
||||
Eigen::Matrix<double, 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;
|
||||
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.to<double>()).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);
|
||||
|
||||
*discA = phi22.transpose();
|
||||
|
||||
Q = *discA * phi12;
|
||||
|
||||
// Make discrete Q symmetric if it isn't already
|
||||
*discQ = (Q + Q.transpose()) / 2.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Discretizes the given continuous A and Q matrices.
|
||||
*
|
||||
* Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
|
||||
* (which is expensive), we take advantage of the structure of the block matrix
|
||||
* of A and Q.
|
||||
*
|
||||
* 1) The exponential of A*t, which is only N x N, is relatively cheap.
|
||||
* 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
|
||||
* using a taylor series to several terms and still be substantially cheaper
|
||||
* than taking the big exponential.
|
||||
*
|
||||
* @param contA Continuous system matrix.
|
||||
* @param contQ Continuous process noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
* @param discA Storage for discrete system matrix.
|
||||
* @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) {
|
||||
// Make continuous Q symmetric if it isn't already
|
||||
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
|
||||
|
||||
Eigen::Matrix<double, States, States> lastTerm = Q;
|
||||
double lastCoeff = dt.to<double>();
|
||||
|
||||
// A^T^n
|
||||
Eigen::Matrix<double, States, States> Atn = contA.transpose();
|
||||
|
||||
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
|
||||
|
||||
// i = 6 i.e. 5th order should be enough precision
|
||||
for (int i = 2; i < 6; ++i) {
|
||||
lastTerm = -contA * lastTerm + Q * Atn;
|
||||
lastCoeff *= dt.to<double>() / static_cast<double>(i);
|
||||
|
||||
phi12 += lastTerm * lastCoeff;
|
||||
|
||||
Atn *= contA.transpose();
|
||||
}
|
||||
|
||||
DiscretizeA<States>(contA, dt, discA);
|
||||
Q = *discA * phi12;
|
||||
|
||||
// Make discrete Q symmetric if it isn't already
|
||||
*discQ = (Q + Q.transpose()) / 2.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a discretized version of the provided continuous measurement noise
|
||||
* covariance matrix.
|
||||
*
|
||||
* @param R Continuous measurement noise covariance matrix.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
template <int Outputs>
|
||||
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
|
||||
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
|
||||
return R / dt.to<double>();
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
164
wpimath/src/main/native/include/frc/system/LinearSystem.h
Normal file
164
wpimath/src/main/native/include/frc/system/LinearSystem.h
Normal file
@@ -0,0 +1,164 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* A plant defined using state-space notation.
|
||||
*
|
||||
* A plant is a mathematical model of a system's dynamics.
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/state-space-guide.pdf.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class LinearSystem {
|
||||
public:
|
||||
/**
|
||||
* Constructs a discrete plant with the given continuous system coefficients.
|
||||
*
|
||||
* @param A System matrix.
|
||||
* @param B Input matrix.
|
||||
* @param C Output matrix.
|
||||
* @param D Feedthrough matrix.
|
||||
*/
|
||||
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) {
|
||||
m_A = A;
|
||||
m_B = B;
|
||||
m_C = C;
|
||||
m_D = D;
|
||||
}
|
||||
|
||||
LinearSystem(const LinearSystem&) = default;
|
||||
LinearSystem& operator=(const LinearSystem&) = default;
|
||||
LinearSystem(LinearSystem&&) = default;
|
||||
LinearSystem& operator=(LinearSystem&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the system matrix A.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, States>& A() const { return m_A; }
|
||||
|
||||
/**
|
||||
* Returns an element of the system matrix A.
|
||||
*
|
||||
* @param i Row of A.
|
||||
* @param j Column of A.
|
||||
*/
|
||||
double A(int i, int j) const { return m_A(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the input matrix B.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
|
||||
|
||||
/**
|
||||
* Returns an element of the input matrix B.
|
||||
*
|
||||
* @param i Row of B.
|
||||
* @param j Column of B.
|
||||
*/
|
||||
double B(int i, int j) const { return m_B(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the output matrix C.
|
||||
*/
|
||||
const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
|
||||
|
||||
/**
|
||||
* Returns an element of the output matrix C.
|
||||
*
|
||||
* @param i Row of C.
|
||||
* @param j Column of C.
|
||||
*/
|
||||
double C(int i, int j) const { return m_C(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the feedthrough matrix D.
|
||||
*/
|
||||
const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
|
||||
|
||||
/**
|
||||
* Returns an element of the feedthrough matrix D.
|
||||
*
|
||||
* @param i Row of D.
|
||||
* @param j Column of D.
|
||||
*/
|
||||
double D(int i, int j) const { return m_D(i, j); }
|
||||
|
||||
/**
|
||||
* Computes the new x given the old x and the control input.
|
||||
*
|
||||
* This is used by state observers directly to run updates based on state
|
||||
* estimate.
|
||||
*
|
||||
* @param x The current state.
|
||||
* @param u The control input.
|
||||
* @param dt Timestep for model update.
|
||||
*/
|
||||
Eigen::Matrix<double, States, 1> CalculateX(
|
||||
const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& clampedU,
|
||||
units::second_t dt) const {
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, Inputs> discB;
|
||||
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
|
||||
|
||||
return discA * x + discB * clampedU;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the new y given the control input.
|
||||
*
|
||||
* This is used by state observers directly to run updates based on state
|
||||
* estimate.
|
||||
*
|
||||
* @param x The current state.
|
||||
* @param clampedU The control input.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, 1> CalculateY(
|
||||
const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& clampedU) const {
|
||||
return m_C * x + m_D * clampedU;
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* Continuous system matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, States> m_A;
|
||||
|
||||
/**
|
||||
* Continuous input matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, Inputs> m_B;
|
||||
|
||||
/**
|
||||
* Output matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, States> m_C;
|
||||
|
||||
/**
|
||||
* Feedthrough matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, Inputs> m_D;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
262
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
Normal file
262
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
Normal file
@@ -0,0 +1,262 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "frc/controller/LinearPlantInversionFeedforward.h"
|
||||
#include "frc/controller/LinearQuadraticRegulator.h"
|
||||
#include "frc/estimator/KalmanFilter.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "units/time.h"
|
||||
#include "units/voltage.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Combines a plant, controller, and observer for controlling a mechanism with
|
||||
* full state feedback.
|
||||
*
|
||||
* For everything in this file, "inputs" and "outputs" are defined from the
|
||||
* perspective of the plant. This means U is an input and Y is an output
|
||||
* (because you give the plant U (powers) and it gives you back a Y (sensor
|
||||
* values). This is the opposite of what they mean from the perspective of the
|
||||
* controller (U is an output because that's what goes to the motors and Y is an
|
||||
* input because that's what comes back from the sensors).
|
||||
*
|
||||
* For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/state-space-guide.pdf.
|
||||
*/
|
||||
template <int States, int Inputs, int Outputs>
|
||||
class LinearSystemLoop {
|
||||
public:
|
||||
/**
|
||||
* Constructs a state-space loop with the given plant, controller, and
|
||||
* observer. By default, the initial reference is all zeros. Users should
|
||||
* call reset with the initial system state before enabling the loop.
|
||||
*
|
||||
* @param plant State-space plant.
|
||||
* @param controller State-space controller.
|
||||
* @param feedforward Plant inversion feedforward.
|
||||
* @param observer State-space observer.
|
||||
* @param maxVoltageVolts The maximum voltage that can be applied. Assumes
|
||||
* that the inputs are voltages.
|
||||
*/
|
||||
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
|
||||
LinearQuadraticRegulator<States, Inputs>& controller,
|
||||
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
|
||||
KalmanFilter<States, Inputs, Outputs>& observer,
|
||||
units::volt_t maxVoltage)
|
||||
: LinearSystemLoop(plant, controller, feedforward, observer,
|
||||
[=](Eigen::Matrix<double, Inputs, 1> u) {
|
||||
return frc::NormalizeInputVector<Inputs>(
|
||||
u, maxVoltage.template to<double>());
|
||||
}) {}
|
||||
|
||||
/**
|
||||
* Constructs a state-space loop with the given plant, controller, and
|
||||
* observer.
|
||||
*
|
||||
* @param plant State-space plant.
|
||||
* @param controller State-space controller.
|
||||
* @param feedforward Plant-inversion feedforward.
|
||||
* @param observer State-space observer.
|
||||
*/
|
||||
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
|
||||
LinearQuadraticRegulator<States, Inputs>& controller,
|
||||
LinearPlantInversionFeedforward<States, Inputs>& feedforward,
|
||||
KalmanFilter<States, Inputs, Outputs>& observer,
|
||||
std::function<Eigen::Matrix<double, Inputs, 1>(
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
clampFunction)
|
||||
: m_plant(plant),
|
||||
m_controller(controller),
|
||||
m_feedforward(feedforward),
|
||||
m_observer(observer),
|
||||
m_clampFunc(clampFunction) {
|
||||
m_nextR.setZero();
|
||||
Reset(m_nextR);
|
||||
}
|
||||
|
||||
virtual ~LinearSystemLoop() = default;
|
||||
|
||||
LinearSystemLoop(LinearSystemLoop&&) = default;
|
||||
LinearSystemLoop& operator=(LinearSystemLoop&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the observer's state estimate x-hat.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& Xhat() const {
|
||||
return m_observer.Xhat();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the observer's state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
*/
|
||||
double Xhat(int i) const { return m_observer.Xhat(i); }
|
||||
|
||||
/**
|
||||
* Returns the controller's next reference r.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
|
||||
|
||||
/**
|
||||
* Returns an element of the controller's next reference r.
|
||||
*
|
||||
* @param i Row of r.
|
||||
*/
|
||||
double NextR(int i) const { return NextR()(i); }
|
||||
|
||||
/**
|
||||
* Returns the controller's calculated control input u.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> U() const {
|
||||
return ClampInput(m_controller.U() + m_feedforward.Uff());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the controller's calculated control input u.
|
||||
*
|
||||
* @param i Row of u.
|
||||
*/
|
||||
double U(int i) const { return U()(i); }
|
||||
|
||||
/**
|
||||
* Set the initial state estimate x-hat.
|
||||
*
|
||||
* @param xHat The initial state estimate x-hat.
|
||||
*/
|
||||
void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
|
||||
m_observer.SetXhat(xHat);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set an element of the initial state estimate x-hat.
|
||||
*
|
||||
* @param i Row of x-hat.
|
||||
* @param value Value for element of x-hat.
|
||||
*/
|
||||
void SetXhat(int i, double value) { m_observer.SetXhat(i, value); }
|
||||
|
||||
/**
|
||||
* Set the next reference r.
|
||||
*
|
||||
* @param nextR Next reference.
|
||||
*/
|
||||
void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
|
||||
m_nextR = nextR;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the plant used internally.
|
||||
*/
|
||||
const LinearSystem<States, Inputs, Outputs>& Plant() const { return m_plant; }
|
||||
|
||||
/**
|
||||
* Return the controller used internally.
|
||||
*/
|
||||
const LinearQuadraticRegulator<States, Inputs>& Controller() const {
|
||||
return m_controller;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the feedforward used internally.
|
||||
*
|
||||
* @return the feedforward used internally.
|
||||
*/
|
||||
const LinearPlantInversionFeedforward<States, Inputs> Feedforward() const {
|
||||
return m_feedforward;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the observer used internally.
|
||||
*/
|
||||
const KalmanFilter<States, Inputs, Outputs>& Observer() const {
|
||||
return m_observer;
|
||||
}
|
||||
|
||||
/**
|
||||
* Zeroes reference r, controller output u and plant output y.
|
||||
* The previous reference for PlantInversionFeedforward is set to the
|
||||
* initial reference.
|
||||
* @param initialReference The initial reference.
|
||||
*/
|
||||
void Reset(Eigen::Matrix<double, States, 1> initialState) {
|
||||
m_controller.Reset();
|
||||
m_feedforward.Reset(initialState);
|
||||
m_observer.Reset();
|
||||
m_nextR.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns difference between reference r and x-hat.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, 1> 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::Matrix<double, Outputs, 1>& y) {
|
||||
m_observer.Correct(U(), y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets new controller output, projects model forward, and runs observer
|
||||
* prediction.
|
||||
*
|
||||
* After calling this, the user should send the elements of u to the
|
||||
* actuators.
|
||||
*
|
||||
* @param dt Timestep for model update.
|
||||
*/
|
||||
void Predict(units::second_t dt) {
|
||||
Eigen::Matrix<double, Inputs, 1> u =
|
||||
ClampInput(m_controller.Calculate(m_observer.Xhat(), m_nextR) +
|
||||
m_feedforward.Calculate(m_nextR));
|
||||
m_observer.Predict(u, dt);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clamps input vector between system's minimum and maximum allowable input.
|
||||
*
|
||||
* @param u Input vector to clamp.
|
||||
* @return Clamped input vector.
|
||||
*/
|
||||
Eigen::Matrix<double, Inputs, 1> ClampInput(
|
||||
const Eigen::Matrix<double, Inputs, 1>& u) const {
|
||||
return m_clampFunc(u);
|
||||
}
|
||||
|
||||
protected:
|
||||
LinearSystem<States, Inputs, Outputs>& m_plant;
|
||||
LinearQuadraticRegulator<States, Inputs>& m_controller;
|
||||
LinearPlantInversionFeedforward<States, Inputs>& m_feedforward;
|
||||
KalmanFilter<States, Inputs, Outputs>& m_observer;
|
||||
|
||||
/**
|
||||
* Clamping function.
|
||||
*/
|
||||
std::function<Eigen::Matrix<double, Inputs, 1>(
|
||||
const Eigen::Matrix<double, Inputs, 1>&)>
|
||||
m_clampFunc;
|
||||
|
||||
// Reference to go to in the next cycle (used by feedforward controller).
|
||||
Eigen::Matrix<double, States, 1> m_nextR;
|
||||
|
||||
// These are accessible from non-templated subclasses.
|
||||
static constexpr int kStates = States;
|
||||
static constexpr int kInputs = Inputs;
|
||||
static constexpr int kOutputs = Outputs;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,80 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Core"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Returns numerical Jacobian with respect to x for f(x).
|
||||
*
|
||||
* @tparam Rows Number of rows in result of f(x).
|
||||
* @tparam Cols Number of columns in result of f(x).
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x Vector argument.
|
||||
*/
|
||||
template <int Rows, int Cols, typename F>
|
||||
auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
|
||||
constexpr double kEpsilon = 1e-5;
|
||||
Eigen::Matrix<double, Rows, Cols> result;
|
||||
result.setZero();
|
||||
|
||||
// It's more expensive, but +- epsilon will be more accurate
|
||||
for (int i = 0; i < Cols; ++i) {
|
||||
Eigen::Matrix<double, Cols, 1> dX_plus = x;
|
||||
dX_plus(i) += kEpsilon;
|
||||
Eigen::Matrix<double, Cols, 1> dX_minus = x;
|
||||
dX_minus(i) -= kEpsilon;
|
||||
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns numerical Jacobian with respect to x for f(x, u, ...).
|
||||
*
|
||||
* @tparam Rows Number of rows in result of f(x, u, ...).
|
||||
* @tparam States Number of rows in x.
|
||||
* @tparam Inputs Number of rows in u.
|
||||
* @tparam F Function object type.
|
||||
* @tparam Args... Remaining arguments to f(x, u, ...).
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
*/
|
||||
template <int Rows, int States, int Inputs, typename F, typename... Args>
|
||||
auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
Args&&... args) {
|
||||
return NumericalJacobian<Rows, States>(
|
||||
[&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns numerical Jacobian with respect to u for f(x, u, ...).
|
||||
*
|
||||
* @tparam Rows Number of rows in result of f(x, u, ...).
|
||||
* @tparam States Number of rows in x.
|
||||
* @tparam Inputs Number of rows in u.
|
||||
* @tparam F Function object type.
|
||||
* @tparam Args... Remaining arguments to f(x, u, ...).
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
*/
|
||||
template <int Rows, int States, int Inputs, typename F, typename... Args>
|
||||
auto NumericalJacobianU(F&& f, const Eigen::Matrix<double, States, 1>& x,
|
||||
const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
Args&&... args) {
|
||||
return NumericalJacobian<Rows, Inputs>(
|
||||
[&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
69
wpimath/src/main/native/include/frc/system/RungeKutta.h
Normal file
69
wpimath/src/main/native/include/frc/system/RungeKutta.h
Normal file
@@ -0,0 +1,69 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
|
||||
*
|
||||
* @param f The function to integrate. It must take one argument x.
|
||||
* @param x The initial value of x.
|
||||
* @param dt The time over which to integrate.
|
||||
*/
|
||||
template <typename F, typename T>
|
||||
T RungeKutta(F&& f, T x, units::second_t dt) {
|
||||
const auto halfDt = 0.5 * dt;
|
||||
T k1 = f(x);
|
||||
T k2 = f(x + k1 * halfDt.to<double>());
|
||||
T k3 = f(x + k2 * halfDt.to<double>());
|
||||
T k4 = f(x + k3 * dt.to<double>());
|
||||
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
|
||||
*
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param dt The time over which to integrate.
|
||||
*/
|
||||
template <typename F, typename T, typename U>
|
||||
T RungeKutta(F&& f, T x, U u, units::second_t dt) {
|
||||
const auto halfDt = 0.5 * dt;
|
||||
T k1 = f(x, u);
|
||||
T k2 = f(x + k1 * halfDt.to<double>(), u);
|
||||
T k3 = f(x + k2 * halfDt.to<double>(), u);
|
||||
T k4 = f(x + k3 * dt.to<double>(), u);
|
||||
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
|
||||
*
|
||||
* @param f The function to integrate. It must take two arguments x and t.
|
||||
* @param x The initial value of x.
|
||||
* @param t The intial value of t.
|
||||
* @param dt The time over which to integrate.
|
||||
*/
|
||||
template <typename F, typename T>
|
||||
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
|
||||
const auto halfDt = 0.5 * dt;
|
||||
T k1 = f(t, x);
|
||||
T k2 = f(t + halfDt, x + k1 / 2.0);
|
||||
T k3 = f(t + halfDt, x + k2 / 2.0);
|
||||
T k4 = f(t + dt, x + k3);
|
||||
|
||||
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
} // namespace frc
|
||||
157
wpimath/src/main/native/include/frc/system/plant/DCMotor.h
Normal file
157
wpimath/src/main/native/include/frc/system/plant/DCMotor.h
Normal file
@@ -0,0 +1,157 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/current.h"
|
||||
#include "units/impedance.h"
|
||||
#include "units/torque.h"
|
||||
#include "units/voltage.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Holds the constants for a DC motor.
|
||||
*/
|
||||
class DCMotor {
|
||||
public:
|
||||
using radians_per_second_per_volt_t =
|
||||
units::unit_t<units::compound_unit<units::radians_per_second,
|
||||
units::inverse<units::volt>>>;
|
||||
using newton_meters_per_ampere_t =
|
||||
units::unit_t<units::compound_unit<units::newton_meters,
|
||||
units::inverse<units::ampere>>>;
|
||||
|
||||
units::volt_t nominalVoltage;
|
||||
units::newton_meter_t stallTorque;
|
||||
units::ampere_t stallCurrent;
|
||||
units::ampere_t freeCurrent;
|
||||
units::radians_per_second_t freeSpeed;
|
||||
|
||||
// Resistance of motor
|
||||
units::ohm_t R;
|
||||
|
||||
// Motor velocity constant
|
||||
radians_per_second_per_volt_t Kv;
|
||||
|
||||
// Torque constant
|
||||
newton_meters_per_ampere_t Kt;
|
||||
|
||||
/**
|
||||
* Constructs a DC motor.
|
||||
*
|
||||
* @param nominalVoltage Voltage at which the motor constants were measured.
|
||||
* @param stallTorque Current draw when stalled.
|
||||
* @param stallCurrent Current draw when stalled.
|
||||
* @param freeCurrent Current draw under no load.
|
||||
* @param freeSpeed Angular velocity under no load.
|
||||
* @param numMotors Number of motors in a gearbox.
|
||||
*/
|
||||
constexpr DCMotor(units::volt_t nominalVoltage,
|
||||
units::newton_meter_t stallTorque,
|
||||
units::ampere_t stallCurrent, units::ampere_t freeCurrent,
|
||||
units::radians_per_second_t freeSpeed, int numMotors = 1)
|
||||
: nominalVoltage(nominalVoltage),
|
||||
stallTorque(stallTorque * numMotors),
|
||||
stallCurrent(stallCurrent),
|
||||
freeCurrent(freeCurrent),
|
||||
freeSpeed(freeSpeed),
|
||||
R(nominalVoltage / stallCurrent),
|
||||
Kv(freeSpeed / (nominalVoltage - R * freeCurrent)),
|
||||
Kt(stallTorque * numMotors / stallCurrent) {}
|
||||
|
||||
/**
|
||||
* Returns current drawn by motor with given speed and input voltage.
|
||||
*
|
||||
* @param speed The current angular velocity of the motor.
|
||||
* @param inputVoltage The voltage being applied to the motor.
|
||||
*/
|
||||
constexpr units::ampere_t Current(units::radians_per_second_t speed,
|
||||
units::volt_t inputVoltage) const {
|
||||
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of CIM.
|
||||
*/
|
||||
static constexpr DCMotor CIM(int numMotors = 1) {
|
||||
return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of MiniCIM.
|
||||
*/
|
||||
static constexpr DCMotor MiniCIM(int numMotors = 1) {
|
||||
return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Bag motor.
|
||||
*/
|
||||
static constexpr DCMotor Bag(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Vex 775 Pro.
|
||||
*/
|
||||
static constexpr DCMotor Vex775Pro(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Andymark RS 775-125.
|
||||
*/
|
||||
static constexpr DCMotor RS775_125(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Banebots RS 775.
|
||||
*/
|
||||
static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Andymark 9015.
|
||||
*/
|
||||
static constexpr DCMotor Andymark9015(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Banebots RS 550.
|
||||
*/
|
||||
static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of NEO brushless motor.
|
||||
*/
|
||||
static constexpr DCMotor NEO(int numMotors = 1) {
|
||||
return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of NEO 550 brushless motor.
|
||||
*/
|
||||
static constexpr DCMotor NEO550(int numMotors = 1) {
|
||||
return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns instance of Falcon 500 brushless motor.
|
||||
*/
|
||||
static constexpr DCMotor Falcon500(int numMotors = 1) {
|
||||
return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,213 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "frc/system/plant/DCMotor.h"
|
||||
#include "units/moment_of_inertia.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
class LinearSystemId {
|
||||
public:
|
||||
/**
|
||||
* Constructs the state-space model for an elevator.
|
||||
*
|
||||
* States: [[position], [velocity]]
|
||||
* Inputs: [[voltage]]
|
||||
* Outputs: [[position]]
|
||||
*
|
||||
* @param motor Instance of DCMotor.
|
||||
* @param m Carriage mass.
|
||||
* @param r Pulley radius.
|
||||
* @param G Gear ratio from motor to carriage.
|
||||
*/
|
||||
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
|
||||
units::kilogram_t m,
|
||||
units::meter_t r, double G) {
|
||||
auto A = frc::MakeMatrix<2, 2>(
|
||||
0.0, 1.0, 0.0,
|
||||
(-std::pow(G, 2) * motor.Kt /
|
||||
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
|
||||
.to<double>());
|
||||
auto B = frc::MakeMatrix<2, 1>(
|
||||
0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
|
||||
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
|
||||
auto D = frc::MakeMatrix<1, 1>(0.0);
|
||||
|
||||
return LinearSystem<2, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a single-jointed arm.
|
||||
*
|
||||
* States: [[angle], [angular velocity]]
|
||||
* Inputs: [[voltage]]
|
||||
* Outputs: [[angle]]
|
||||
*
|
||||
* @param motor Instance of DCMotor.
|
||||
* @param J Moment of inertia.
|
||||
* @param G Gear ratio from motor to carriage.
|
||||
*/
|
||||
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
|
||||
DCMotor motor, units::kilogram_square_meter_t J, double G) {
|
||||
auto A = frc::MakeMatrix<2, 2>(
|
||||
0.0, 1.0, 0.0,
|
||||
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
|
||||
auto B =
|
||||
frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
|
||||
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
|
||||
auto D = frc::MakeMatrix<1, 1>(0.0);
|
||||
|
||||
return LinearSystem<2, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a 1 DOF velocity-only system from
|
||||
* system identification data.
|
||||
*
|
||||
* States: [[velocity]]
|
||||
* Inputs: [[voltage]]
|
||||
* Outputs: [[velocity]]
|
||||
*
|
||||
* The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volt seconds per distance.
|
||||
* @param kA The acceleration gain, in volt seconds^2 per distance.
|
||||
*/
|
||||
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) {
|
||||
auto A = frc::MakeMatrix<1, 1>(-kV / kA);
|
||||
auto B = frc::MakeMatrix<1, 1>(1.0 / kA);
|
||||
auto C = frc::MakeMatrix<1, 1>(1.0);
|
||||
auto D = frc::MakeMatrix<1, 1>(0.0);
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a 1 DOF position system from system
|
||||
* identification data.
|
||||
*
|
||||
* States: [[position], [velocity]]
|
||||
* Inputs: [[voltage]]
|
||||
* Outputs: [[position]]
|
||||
*
|
||||
* The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volt seconds per distance.
|
||||
* @param kA The acceleration gain, in volt seconds^2 per distance.
|
||||
*/
|
||||
static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) {
|
||||
auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA);
|
||||
auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA);
|
||||
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
|
||||
auto D = frc::MakeMatrix<1, 1>(0.0);
|
||||
|
||||
return LinearSystem<2, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a 2 DOF drivetrain velocity system
|
||||
* from system identification data.
|
||||
*
|
||||
* States: [[left velocity], [right velocity]]
|
||||
* Inputs: [[left voltage], [right voltage]]
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
*
|
||||
* @param kVlinear The linear velocity gain, in volt seconds per distance.
|
||||
* @param kAlinear The linear acceleration gain, in volt seconds^2 per
|
||||
* distance.
|
||||
* @param kVangular The angular velocity gain, in volt seconds per angle.
|
||||
* @param kAangular The angular acceleration gain, in volt seconds^2 per
|
||||
* angle.
|
||||
*/
|
||||
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear,
|
||||
double kAlinear,
|
||||
double kVangular,
|
||||
double kAangular) {
|
||||
double c = 0.5 / (kAlinear * kAangular);
|
||||
double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular);
|
||||
double A2 = c * (kAlinear * kVangular - kVlinear * kAangular);
|
||||
double B1 = c * (kAlinear + kAangular);
|
||||
double B2 = c * (kAangular - kAlinear);
|
||||
|
||||
auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
|
||||
auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
|
||||
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
|
||||
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
|
||||
|
||||
return LinearSystem<2, 2, 2>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a flywheel.
|
||||
*
|
||||
* States: [[angular velocity]]
|
||||
* Inputs: [[voltage]]
|
||||
* Outputs: [[angular velocity]]
|
||||
*
|
||||
* @param motor Instance of DCMotor.
|
||||
* @param J Moment of inertia.
|
||||
* @param G Gear ratio from motor to carriage.
|
||||
*/
|
||||
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
|
||||
units::kilogram_square_meter_t J,
|
||||
double G) {
|
||||
auto A = frc::MakeMatrix<1, 1>(
|
||||
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
|
||||
auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
|
||||
auto C = frc::MakeMatrix<1, 1>(1.0);
|
||||
auto D = frc::MakeMatrix<1, 1>(0.0);
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the state-space model for a drivetrain.
|
||||
*
|
||||
* States: [[left velocity], [right velocity]]
|
||||
* Inputs: [[left voltage], [right voltage]]
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
*
|
||||
* @param motor Instance of DCMotor.
|
||||
* @param m Drivetrain mass.
|
||||
* @param r Wheel radius.
|
||||
* @param rb Robot radius.
|
||||
* @param G Gear ratio from motor to wheel.
|
||||
* @param J Moment of inertia.
|
||||
*/
|
||||
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
|
||||
DCMotor motor, units::kilogram_t m, units::meter_t r, units::meter_t rb,
|
||||
units::kilogram_square_meter_t J, double G) {
|
||||
auto C1 = -std::pow(G, 2) * motor.Kt /
|
||||
(motor.Kv * motor.R * units::math::pow<2>(r));
|
||||
auto C2 = G * motor.Kt / (motor.R * r);
|
||||
|
||||
auto A = frc::MakeMatrix<2, 2>(
|
||||
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
|
||||
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
|
||||
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
|
||||
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
|
||||
auto B = frc::MakeMatrix<2, 2>(
|
||||
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
|
||||
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
|
||||
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
|
||||
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
|
||||
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
|
||||
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
|
||||
|
||||
return LinearSystem<2, 2, 2>(A, B, C, D);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user