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

[wpimath] Make KalmanFilter variant for asymmetric updates (#5951)

Browse Source
This commit is contained in:
Tyler Veness
2023-11-23 10:56:15 -08:00
committed by GitHub
parent ca81ced409
commit dfdea9c992
7 changed files with 708 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -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<States extends Num, Inputs extends Num, Outputs extends Num> {
public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
private final Nat<States> m_states;
private final LinearSystem<States, Inputs, Outputs> m_plant;
/** The steady-state Kalman gain matrix. */
private final Matrix<States, Outputs> m_K;
/** The state estimate. */
private Matrix<States, N1> m_xHat;
private Matrix<States, States> m_P;
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private double m_dtSeconds;
private final Matrix<States, States> m_initP;
/**
* Constructs a state-space observer with the given plant.
* Constructs a Kalman filter with the given plant.
*
* <p>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<States extends Num, Inputs extends Num, Outputs extend
this.m_plant = plant;
var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_dtSeconds = dtSeconds;
var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
// Find discrete A and Q
var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dtSeconds);
var discA = pair.getFirst();
var discQ = pair.getSecond();
var discR = Discretization.discretizeR(contR, dtSeconds);
var discR = Discretization.discretizeR(m_contR, dtSeconds);
var C = plant.getC();
@@ -91,10 +95,139 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
throw new IllegalArgumentException(msg);
}
var P = new Matrix<>(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<States, States> 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<States, States> newP) {
m_P = newP;
}
/**
* Returns the state estimate x-hat.
*
* @return the state estimate x-hat.
*/
@Override
public Matrix<States, N1> 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<States, N1> 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<Inputs, N1> 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<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(u, y, m_contR);
}
/**
* Correct the state estimate x-hat using the measurements in y.
*
* <p>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<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> 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<States extends Num, Inputs extends Num, Outputs extend
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K =
new Matrix<>(
S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
final Matrix<States, Outputs> 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<States, Outputs> 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<States, N1> 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<States, N1> 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<Inputs, N1> 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<Inputs, N1> u, Matrix<Outputs, N1> 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()));
}
}

206
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java Normal file
View File

@@ -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".
*
* <p>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.
*
* <p>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.
*
* <p>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<States extends Num, Inputs extends Num, Outputs extends Num> {
private final Nat<States> m_states;
private final LinearSystem<States, Inputs, Outputs> m_plant;
/** The steady-state Kalman gain matrix. */
private final Matrix<States, Outputs> m_K;
/** The state estimate. */
private Matrix<States, N1> m_xHat;
/**
* Constructs a steady-state Kalman filter with the given plant.
*
* <p>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> states,
Nat<Outputs> outputs,
LinearSystem<States, Inputs, Outputs> plant,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> 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<States, Outputs> 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<States, N1> 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<States, N1> 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<Inputs, N1> 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<Inputs, N1> u, Matrix<Outputs, N1> 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)))));
}
}

14
wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp Normal file
View File

@@ -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

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

@@ -4,7 +4,6 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
@@ -43,8 +42,10 @@ class KalmanFilter {
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
/**
* 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<States, Outputs>& 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<Outputs, Outputs>& R);
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
/**
* The steady-state Kalman gain matrix.
*/
Matrixd<States, Outputs> m_K;
/**
* The state estimate.
*/
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
StateMatrix m_initP;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

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

@@ -4,8 +4,6 @@
#pragma once
#include <frc/fmt/Eigen.h>
#include <cmath>
#include <stdexcept>
#include <string>
@@ -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<States, Inputs, Outputs>::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<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
const auto& C = plant.C();
@@ -48,11 +49,38 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
Reset();
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(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 <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
Matrixd<Outputs, Outputs> 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<States, Inputs, Outputs>::KalmanFilter(
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
Reset();
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(const 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

153
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/array.h>
#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 <int States, int Inputs, int Outputs>
class SteadyStateKalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
/**
* Constructs a 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<States, Inputs, Outputs>& 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<States, Outputs>& 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<States, Inputs, Outputs>* m_plant;
/**
* The steady-state Kalman gain matrix.
*/
Matrixd<States, Outputs> 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"

89
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc Normal file
View File

@@ -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 <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#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 <int States, int Inputs, int Outputs>
SteadyStateKalmanFilter<States, Inputs, Outputs>::SteadyStateKalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(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<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// 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 <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::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