mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +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
@@ -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
|
||||
Reference in New Issue
Block a user