From dfdea9c992e8b36a7b75643942159ebdfcdd02fb Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 23 Nov 2023 10:56:15 -0800 Subject: [PATCH] [wpimath] Make KalmanFilter variant for asymmetric updates (#5951) --- .../first/math/estimator/KalmanFilter.java | 260 +++++++++++------- .../estimator/SteadyStateKalmanFilter.java | 206 ++++++++++++++ .../cpp/estimator/SteadyStateKalmanFilter.cpp | 14 + .../include/frc/estimator/KalmanFilter.h | 65 +++-- .../include/frc/estimator/KalmanFilter.inc | 69 +++-- .../frc/estimator/SteadyStateKalmanFilter.h | 153 +++++++++++ .../frc/estimator/SteadyStateKalmanFilter.inc | 89 ++++++ 7 files changed, 708 insertions(+), 148 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java create mode 100644 wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp create mode 100644 wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h create mode 100644 wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java index 08e3270606..5cf9132cd0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java @@ -29,19 +29,21 @@ import edu.wpi.first.math.system.LinearSystem; * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control * theory". */ -public class KalmanFilter { +public class KalmanFilter + implements KalmanTypeFilter { private final Nat m_states; private final LinearSystem m_plant; - - /** The steady-state Kalman gain matrix. */ - private final Matrix m_K; - - /** The state estimate. */ private Matrix m_xHat; + private Matrix m_P; + private final Matrix m_contQ; + private final Matrix m_contR; + private double m_dtSeconds; + + private final Matrix m_initP; /** - * Constructs a state-space observer with the given plant. + * Constructs a Kalman filter with the given plant. * *

See * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices @@ -66,14 +68,16 @@ public class KalmanFilter(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); + m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); - // S = CPCᵀ + R - var S = C.times(P).times(C.transpose()).plus(discR); + reset(); + } + + /** + * Returns the error covariance matrix P. + * + * @return the error covariance matrix P. + */ + @Override + public Matrix getP() { + return m_P; + } + + /** + * Returns an element of the error covariance matrix P. + * + * @param row Row of P. + * @param col Column of P. + * @return the value of the error covariance matrix P at (i, j). + */ + @Override + public double getP(int row, int col) { + return m_P.get(row, col); + } + + /** + * Sets the entire error covariance matrix P. + * + * @param newP The new value of P to use. + */ + @Override + public void setP(Matrix newP) { + m_P = newP; + } + + /** + * Returns the state estimate x-hat. + * + * @return the state estimate x-hat. + */ + @Override + public Matrix getXhat() { + return m_xHat; + } + + /** + * Returns an element of the state estimate x-hat. + * + * @param row Row of x-hat. + * @return the value of the state estimate x-hat at that row. + */ + @Override + public double getXhat(int row) { + return m_xHat.get(row, 0); + } + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + @Override + public void setXhat(Matrix xHat) { + m_xHat = xHat; + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + @Override + public void setXhat(int row, double value) { + m_xHat.set(row, 0, value); + } + + @Override + public void reset() { + m_xHat = new Matrix<>(m_states, Nat.N1()); + m_P = m_initP; + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + @Override + public void predict(Matrix u, double dtSeconds) { + // Find discrete A and Q + final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dtSeconds); + final var discA = discPair.getFirst(); + final var discQ = discPair.getSecond(); + + m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); + + // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q + m_P = discA.times(m_P).times(discA.transpose()).plus(discQ); + + m_dtSeconds = dtSeconds; + } + + /** + * 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. + */ + @Override + public void correct(Matrix u, Matrix y) { + correct(u, y, m_contR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + *

This is useful for when the measurement noise covariances vary. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param R Continuous measurement noise covariance matrix. + */ + public void correct(Matrix u, Matrix y, Matrix R) { + final var C = m_plant.getC(); + final var D = m_plant.getD(); + + final var discR = Discretization.discretizeR(R, m_dtSeconds); + + final var S = C.times(m_P).times(C.transpose()).plus(discR); // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more // efficiently. @@ -108,95 +241,18 @@ public class KalmanFilter( - S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose()); + final Matrix K = S.transpose().solve(C.times(m_P.transpose())).transpose(); - reset(); - } - - public void reset() { - m_xHat = new Matrix<>(m_states, Nat.N1()); - } - - /** - * Returns the steady-state Kalman gain matrix K. - * - * @return The steady-state Kalman gain matrix K. - */ - public Matrix getK() { - return m_K; - } - - /** - * Returns an element of the steady-state Kalman gain matrix K. - * - * @param row Row of K. - * @param col Column of K. - * @return the element (i, j) of the steady-state Kalman gain matrix K. - */ - public double getK(int row, int col) { - return m_K.get(row, col); - } - - /** - * Set initial state estimate x-hat. - * - * @param xhat The state estimate x-hat. - */ - public void setXhat(Matrix xhat) { - this.m_xHat = xhat; - } - - /** - * Set an element of the initial state estimate x-hat. - * - * @param row Row of x-hat. - * @param value Value for element of x-hat. - */ - public void setXhat(int row, double value) { - m_xHat.set(row, 0, value); - } - - /** - * Returns the state estimate x-hat. - * - * @return The state estimate x-hat. - */ - public Matrix getXhat() { - return m_xHat; - } - - /** - * Returns an element of the state estimate x-hat. - * - * @param row Row of x-hat. - * @return the state estimate x-hat at that row. - */ - public double getXhat(int row) { - return m_xHat.get(row, 0); - } - - /** - * Project the model into the future with a new control input u. - * - * @param u New control input from controller. - * @param dtSeconds Timestep for prediction. - */ - public void predict(Matrix u, double dtSeconds) { - this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); - } - - /** - * 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. - */ - public void correct(Matrix u, Matrix y) { - final var C = m_plant.getC(); - final var D = m_plant.getD(); // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) - m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u))))); + m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u))))); + + // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ + // Use Joseph form for numerical stability + m_P = + Matrix.eye(m_states) + .minus(K.times(C)) + .times(m_P) + .times(Matrix.eye(m_states).minus(K.times(C)).transpose()) + .plus(K.times(discR).times(K.transpose())); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java new file mode 100644 index 0000000000..93ca863663 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java @@ -0,0 +1,206 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.DARE; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; + +/** + * A Kalman filter combines predictions from a model and measurements to give an estimate of the + * true system state. This is useful because many states cannot be measured directly as a result of + * sensor noise, or because the state is "hidden". + * + *

Kalman filters use a K gain matrix to determine whether to trust the model or measurements + * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum + * of squares error in the state estimate. This K gain is used to correct the state estimate by some + * amount of the difference between the actual measurements and the measurements predicted by the + * model. + * + *

This class assumes predict() and correct() are called in pairs, so the Kalman gain converges + * to a steady-state value. If they aren't, use {@link KalmanFilter} instead. + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control + * theory". + */ +public class SteadyStateKalmanFilter { + private final Nat m_states; + + private final LinearSystem m_plant; + + /** The steady-state Kalman gain matrix. */ + private final Matrix m_K; + + /** The state estimate. */ + private Matrix m_xHat; + + /** + * Constructs a steady-state Kalman filter with the given plant. + * + *

See + * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices + * for how to select the standard deviations. + * + * @param states A Nat representing the states of the system. + * @param outputs A Nat representing the outputs of the system. + * @param plant The plant used for the prediction step. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dtSeconds Nominal discretization timestep. + * @throws IllegalArgumentException If the system is unobservable. + */ + public SteadyStateKalmanFilter( + Nat states, + Nat outputs, + LinearSystem plant, + Matrix stateStdDevs, + Matrix measurementStdDevs, + double dtSeconds) { + this.m_states = states; + + this.m_plant = plant; + + var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); + var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); + + var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds); + var discA = pair.getFirst(); + var discQ = pair.getSecond(); + + var discR = Discretization.discretizeR(contR, dtSeconds); + + var C = plant.getC(); + + if (!StateSpaceUtil.isDetectable(discA, C)) { + var builder = + new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n"); + builder + .append(discA.getStorage().toString()) + .append("\nC =\n") + .append(C.getStorage().toString()) + .append('\n'); + + var msg = builder.toString(); + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + + var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); + + // S = CPCᵀ + R + var S = C.times(P).times(C.transpose()).plus(discR); + + // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more + // efficiently. + // + // K = PCᵀS⁻¹ + // KS = PCᵀ + // (KS)ᵀ = (PCᵀ)ᵀ + // SᵀKᵀ = CPᵀ + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // Kᵀ = Sᵀ.solve(CPᵀ) + // K = (Sᵀ.solve(CPᵀ))ᵀ + m_K = + new Matrix<>( + S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose()); + + reset(); + } + + public void reset() { + m_xHat = new Matrix<>(m_states, Nat.N1()); + } + + /** + * Returns the steady-state Kalman gain matrix K. + * + * @return The steady-state Kalman gain matrix K. + */ + public Matrix getK() { + return m_K; + } + + /** + * Returns an element of the steady-state Kalman gain matrix K. + * + * @param row Row of K. + * @param col Column of K. + * @return the element (i, j) of the steady-state Kalman gain matrix K. + */ + public double getK(int row, int col) { + return m_K.get(row, col); + } + + /** + * Set initial state estimate x-hat. + * + * @param xhat The state estimate x-hat. + */ + public void setXhat(Matrix xhat) { + this.m_xHat = xhat; + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + public void setXhat(int row, double value) { + m_xHat.set(row, 0, value); + } + + /** + * Returns the state estimate x-hat. + * + * @return The state estimate x-hat. + */ + public Matrix getXhat() { + return m_xHat; + } + + /** + * Returns an element of the state estimate x-hat. + * + * @param row Row of x-hat. + * @return the state estimate x-hat at that row. + */ + public double getXhat(int row) { + return m_xHat.get(row, 0); + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + public void predict(Matrix u, double dtSeconds) { + this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); + } + + /** + * 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. + */ + public void correct(Matrix u, Matrix y) { + final var C = m_plant.getC(); + final var D = m_plant.getD(); + + // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) + m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u))))); + } +} diff --git a/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp new file mode 100644 index 0000000000..24586232c5 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp @@ -0,0 +1,14 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/estimator/SteadyStateKalmanFilter.h" + +namespace frc { + +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SteadyStateKalmanFilter<1, 1, 1>; +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SteadyStateKalmanFilter<2, 1, 1>; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h index f143493848..3f9443d72b 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include "frc/EigenCore.h" @@ -43,8 +42,10 @@ class KalmanFilter { using StateArray = wpi::array; using OutputArray = wpi::array; + using StateMatrix = Matrixd; + /** - * Constructs a state-space observer with the given plant. + * Constructs a Kalman filter with the given plant. * * See * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices @@ -60,21 +61,25 @@ class KalmanFilter { const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, units::second_t dt); - KalmanFilter(KalmanFilter&&) = default; - KalmanFilter& operator=(KalmanFilter&&) = default; - /** - * Returns the steady-state Kalman gain matrix K. + * Returns the error covariance matrix P. */ - const Matrixd& K() const { return m_K; } + const StateMatrix& P() const { return m_P; } /** - * Returns an element of the steady-state Kalman gain matrix K. + * Returns an element of the error covariance matrix P. * - * @param i Row of K. - * @param j Column of K. + * @param i Row of P. + * @param j Column of P. */ - double K(int i, int j) const { return m_K(i, j); } + 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 StateMatrix& P) { m_P = P; } /** * Returns the state estimate x-hat. @@ -106,7 +111,10 @@ class KalmanFilter { /** * Resets the observer. */ - void Reset() { m_xHat.setZero(); } + void Reset() { + m_xHat.setZero(); + m_P = m_initP; + } /** * Project the model into the future with a new control input u. @@ -119,23 +127,34 @@ class KalmanFilter { /** * Correct the state estimate x-hat using the measurements in y. * - * @param u Same control input used in the last predict step. + * @param u Same control input used in the predict step. * @param y Measurement vector. */ - void Correct(const InputVector& u, const OutputVector& y); + void Correct(const InputVector& u, const OutputVector& y) { + Correct(u, y, m_contR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * This is useful for when the measurement noise covariances vary. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param R Continuous measurement noise covariance matrix. + */ + void Correct(const InputVector& u, const OutputVector& y, + const Matrixd& R); private: LinearSystem* m_plant; - - /** - * The steady-state Kalman gain matrix. - */ - Matrixd m_K; - - /** - * The state estimate. - */ StateVector m_xHat; + StateMatrix m_P; + StateMatrix m_contQ; + Matrixd m_contR; + units::second_t m_dt; + + StateMatrix m_initP; }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc index 7506c0d7b7..cfb8bd9bf9 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc @@ -4,8 +4,6 @@ #pragma once -#include - #include #include #include @@ -15,6 +13,7 @@ #include "frc/DARE.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/KalmanFilter.h" +#include "frc/fmt/Eigen.h" #include "frc/system/Discretization.h" #include "wpimath/MathShared.h" @@ -27,14 +26,16 @@ KalmanFilter::KalmanFilter( units::second_t dt) { m_plant = &plant; - auto contQ = MakeCovMatrix(stateStdDevs); - auto contR = MakeCovMatrix(measurementStdDevs); + m_contQ = MakeCovMatrix(stateStdDevs); + m_contR = MakeCovMatrix(measurementStdDevs); + m_dt = dt; + // Find discrete A and Q Matrixd discA; Matrixd discQ; - DiscretizeAQ(plant.A(), contQ, dt, &discA, &discQ); + DiscretizeAQ(plant.A(), m_contQ, dt, &discA, &discQ); - auto discR = DiscretizeR(contR, dt); + Matrixd discR = DiscretizeR(m_contR, dt); const auto& C = plant.C(); @@ -48,11 +49,38 @@ KalmanFilter::KalmanFilter( throw std::invalid_argument(msg); } - Matrixd P = + m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR); - // S = CPCᵀ + R - Matrixd S = C * P * C.transpose() + discR; + Reset(); +} + +template +void KalmanFilter::Predict(const InputVector& u, + units::second_t dt) { + // Find discrete A and Q + StateMatrix discA; + StateMatrix discQ; + DiscretizeAQ(m_plant->A(), m_contQ, dt, &discA, &discQ); + + m_xHat = m_plant->CalculateX(m_xHat, u, dt); + + // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q + m_P = discA * m_P * discA.transpose() + discQ; + + m_dt = dt; +} + +template +void KalmanFilter::Correct( + const InputVector& u, const OutputVector& y, + const Matrixd& R) { + const auto& C = m_plant->C(); + const auto& D = m_plant->D(); + + const Matrixd discR = DiscretizeR(R, m_dt); + + Matrixd S = C * m_P * C.transpose() + discR; // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more // efficiently. @@ -66,22 +94,17 @@ KalmanFilter::KalmanFilter( // // Kᵀ = Sᵀ.solve(CPᵀ) // K = (Sᵀ.solve(CPᵀ))ᵀ - m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose(); + Matrixd K = + S.transpose().ldlt().solve(C * m_P.transpose()).transpose(); - Reset(); -} - -template -void KalmanFilter::Predict(const InputVector& u, - units::second_t dt) { - m_xHat = m_plant->CalculateX(m_xHat, u, dt); -} - -template -void KalmanFilter::Correct(const InputVector& u, - const OutputVector& y) { // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) - m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u)); + m_xHat += K * (y - (C * m_xHat + D * u)); + + // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ + // Use Joseph form for numerical stability + m_P = (StateMatrix::Identity() - K * C) * m_P * + (StateMatrix::Identity() - K * C).transpose() + + K * discR * K.transpose(); } } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h new file mode 100644 index 0000000000..6548b45660 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h @@ -0,0 +1,153 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/EigenCore.h" +#include "frc/system/LinearSystem.h" +#include "units/time.h" + +namespace frc { + +/** + * A Kalman filter combines predictions from a model and measurements to give an + * estimate of the true system state. This is useful because many states cannot + * be measured directly as a result of sensor noise, or because the state is + * "hidden". + * + * Kalman filters use a K gain matrix to determine whether to trust the model or + * measurements more. Kalman filter theory uses statistics to compute an optimal + * K gain which minimizes the sum of squares error in the state estimate. This K + * gain is used to correct the state estimate by some amount of the difference + * between the actual measurements and the measurements predicted by the model. + * + * This class assumes predict() and correct() are called in pairs, so the Kalman + * gain converges to a steady-state value. If they aren't, use KalmanFilter + * instead. + * + * For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 + * "Stochastic control theory". + * + * @tparam States The number of states. + * @tparam Inputs The number of inputs. + * @tparam Outputs The number of outputs. + */ +template +class SteadyStateKalmanFilter { + public: + using StateVector = Vectord; + using InputVector = Vectord; + using OutputVector = Vectord; + + using StateArray = wpi::array; + using OutputArray = wpi::array; + + /** + * Constructs a staeady-state Kalman filter with the given plant. + * + * See + * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices + * for how to select the standard deviations. + * + * @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. + * @throws std::invalid_argument If the system is unobservable. + */ + SteadyStateKalmanFilter(LinearSystem& plant, + const StateArray& stateStdDevs, + const OutputArray& measurementStdDevs, + units::second_t dt); + + SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default; + SteadyStateKalmanFilter& operator=(SteadyStateKalmanFilter&&) = default; + + /** + * Returns the steady-state Kalman gain matrix K. + */ + const Matrixd& K() const { return m_K; } + + /** + * Returns an element of the steady-state Kalman gain 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 state estimate x-hat. + */ + const StateVector& 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 StateVector& 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 InputVector& u, units::second_t 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 InputVector& u, const OutputVector& y); + + private: + LinearSystem* m_plant; + + /** + * The steady-state Kalman gain matrix. + */ + Matrixd m_K; + + /** + * The state estimate. + */ + StateVector m_xHat; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SteadyStateKalmanFilter<1, 1, 1>; +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SteadyStateKalmanFilter<2, 1, 1>; + +} // namespace frc + +#include "SteadyStateKalmanFilter.inc" diff --git a/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc new file mode 100644 index 0000000000..1f2f2a2ff6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include + +#include "frc/DARE.h" +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/SteadyStateKalmanFilter.h" +#include "frc/fmt/Eigen.h" +#include "frc/system/Discretization.h" +#include "wpimath/MathShared.h" + +namespace frc { + +template +SteadyStateKalmanFilter::SteadyStateKalmanFilter( + LinearSystem& plant, + const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, + units::second_t dt) { + m_plant = &plant; + + auto contQ = MakeCovMatrix(stateStdDevs); + auto contR = MakeCovMatrix(measurementStdDevs); + + Matrixd discA; + Matrixd discQ; + DiscretizeAQ(plant.A(), contQ, dt, &discA, &discQ); + + auto discR = DiscretizeR(contR, dt); + + const auto& C = plant.C(); + + if (!IsDetectable(discA, C)) { + std::string msg = fmt::format( + "The system passed to the Kalman filter is " + "unobservable!\n\nA =\n{}\nC =\n{}\n", + discA, C); + + wpi::math::MathSharedStore::ReportError(msg); + throw std::invalid_argument(msg); + } + + Matrixd P = + DARE(discA.transpose(), C.transpose(), discQ, discR); + + // S = CPCᵀ + R + Matrixd S = C * P * C.transpose() + discR; + + // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more + // efficiently. + // + // K = PCᵀS⁻¹ + // KS = PCᵀ + // (KS)ᵀ = (PCᵀ)ᵀ + // SᵀKᵀ = CPᵀ + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // Kᵀ = Sᵀ.solve(CPᵀ) + // K = (Sᵀ.solve(CPᵀ))ᵀ + m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose(); + + Reset(); +} + +template +void SteadyStateKalmanFilter::Predict( + const InputVector& u, units::second_t dt) { + m_xHat = m_plant->CalculateX(m_xHat, u, dt); +} + +template +void SteadyStateKalmanFilter::Correct( + const InputVector& u, const OutputVector& y) { + const auto& C = m_plant->C(); + const auto& D = m_plant->D(); + + // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) + m_xHat += m_K * (y - (C * m_xHat + D * u)); +} + +} // namespace frc