```001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.estimator;
006
007import edu.wpi.first.math.DARE;
008import edu.wpi.first.math.MathSharedStore;
009import edu.wpi.first.math.Matrix;
010import edu.wpi.first.math.Nat;
011import edu.wpi.first.math.Num;
012import edu.wpi.first.math.StateSpaceUtil;
013import edu.wpi.first.math.numbers.N1;
014import edu.wpi.first.math.system.Discretization;
015import edu.wpi.first.math.system.LinearSystem;
016
017/**
018 * A Kalman filter combines predictions from a model and measurements to give an estimate of the
019 * true system state. This is useful because many states cannot be measured directly as a result of
020 * sensor noise, or because the state is "hidden".
021 *
022 * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
023 * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
024 * of squares error in the state estimate. This K gain is used to correct the state estimate by some
025 * amount of the difference between the actual measurements and the measurements predicted by the
026 * model.
027 *
028 * <p>This class assumes predict() and correct() are called in pairs, so the Kalman gain converges
030 *
031 * <p>For more on the underlying math, read <a
032 * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
033 * chapter 9 "Stochastic control theory".
034 *
035 * @param <States> Number of states.
036 * @param <Inputs> Number of inputs.
037 * @param <Outputs> Number of outputs.
038 */
039public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
040  private final Nat<States> m_states;
041
042  private final LinearSystem<States, Inputs, Outputs> m_plant;
043
044  /** The steady-state Kalman gain matrix. */
045  private final Matrix<States, Outputs> m_K;
046
047  /** The state estimate. */
048  private Matrix<States, N1> m_xHat;
049
050  /**
051   * Constructs a steady-state Kalman filter with the given plant.
052   *
053   * <p>See <a
055   * for how to select the standard deviations.
056   *
057   * @param states A Nat representing the states of the system.
058   * @param outputs A Nat representing the outputs of the system.
059   * @param plant The plant used for the prediction step.
060   * @param stateStdDevs Standard deviations of model states.
061   * @param measurementStdDevs Standard deviations of measurements.
062   * @param dtSeconds Nominal discretization timestep.
063   * @throws IllegalArgumentException If the system is unobservable.
064   */
066      Nat<States> states,
067      Nat<Outputs> outputs,
068      LinearSystem<States, Inputs, Outputs> plant,
069      Matrix<States, N1> stateStdDevs,
070      Matrix<Outputs, N1> measurementStdDevs,
071      double dtSeconds) {
072    this.m_states = states;
073
074    this.m_plant = plant;
075
076    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
077    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
078
079    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
080    var discA = pair.getFirst();
081    var discQ = pair.getSecond();
082
083    var discR = Discretization.discretizeR(contR, dtSeconds);
084
085    var C = plant.getC();
086
087    if (!StateSpaceUtil.isDetectable(discA, C)) {
088      var msg =
089          "The system passed to the Kalman filter is unobservable!\n\nA =\n"
090              + discA.getStorage().toString()
091              + "\nC =\n"
092              + C.getStorage().toString()
093              + '\n';
095      throw new IllegalArgumentException(msg);
096    }
097
098    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
099
100    // S = CPCᵀ + R
101    var S = C.times(P).times(C.transpose()).plus(discR);
102
103    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
104    // efficiently.
105    //
106    // K = PCᵀS⁻¹
107    // KS = PCᵀ
108    // (KS)ᵀ = (PCᵀ)ᵀ
109    // SᵀKᵀ = CPᵀ
110    //
111    // The solution of Ax = b can be found via x = A.solve(b).
112    //
113    // Kᵀ = Sᵀ.solve(CPᵀ)
114    // K = (Sᵀ.solve(CPᵀ))ᵀ
115    m_K =
116        new Matrix<>(
117            S.transpose().getStorage().solve(C.times(P.transpose()).getStorage()).transpose());
118
119    reset();
120  }
121
122  /** Resets the observer. */
123  public final void reset() {
124    m_xHat = new Matrix<>(m_states, Nat.N1());
125  }
126
127  /**
128   * Returns the steady-state Kalman gain matrix K.
129   *
130   * @return The steady-state Kalman gain matrix K.
131   */
132  public Matrix<States, Outputs> getK() {
133    return m_K;
134  }
135
136  /**
137   * Returns an element of the steady-state Kalman gain matrix K.
138   *
139   * @param row Row of K.
140   * @param col Column of K.
141   * @return the element (i, j) of the steady-state Kalman gain matrix K.
142   */
143  public double getK(int row, int col) {
144    return m_K.get(row, col);
145  }
146
147  /**
148   * Set initial state estimate x-hat.
149   *
150   * @param xhat The state estimate x-hat.
151   */
152  public void setXhat(Matrix<States, N1> xhat) {
153    this.m_xHat = xhat;
154  }
155
156  /**
157   * Set an element of the initial state estimate x-hat.
158   *
159   * @param row Row of x-hat.
160   * @param value Value for element of x-hat.
161   */
162  public void setXhat(int row, double value) {
163    m_xHat.set(row, 0, value);
164  }
165
166  /**
167   * Returns the state estimate x-hat.
168   *
169   * @return The state estimate x-hat.
170   */
171  public Matrix<States, N1> getXhat() {
172    return m_xHat;
173  }
174
175  /**
176   * Returns an element of the state estimate x-hat.
177   *
178   * @param row Row of x-hat.
179   * @return the state estimate x-hat at that row.
180   */
181  public double getXhat(int row) {
182    return m_xHat.get(row, 0);
183  }
184
185  /**
186   * Project the model into the future with a new control input u.
187   *
188   * @param u New control input from controller.
189   * @param dtSeconds Timestep for prediction.
190   */
191  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
192    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
193  }
194
195  /**
196   * Correct the state estimate x-hat using the measurements in y.
197   *
198   * @param u Same control input used in the last predict step.
199   * @param y Measurement vector.
200   */
201  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
202    final var C = m_plant.getC();
203    final var D = m_plant.getD();
204
205    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
206    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
207  }
208}

```