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 029 * to a steady-state value. If they aren't, use {@link KalmanFilter} instead. 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 */ 035public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> { 036 private final Nat<States> m_states; 037 038 private final LinearSystem<States, Inputs, Outputs> m_plant; 039 040 /** The steady-state Kalman gain matrix. */ 041 private final Matrix<States, Outputs> m_K; 042 043 /** The state estimate. */ 044 private Matrix<States, N1> m_xHat; 045 046 /** 047 * Constructs a steady-state Kalman filter with the given plant. 048 * 049 * <p>See <a 050 * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices</a> 051 * for how to select the standard deviations. 052 * 053 * @param states A Nat representing the states of the system. 054 * @param outputs A Nat representing the outputs of the system. 055 * @param plant The plant used for the prediction step. 056 * @param stateStdDevs Standard deviations of model states. 057 * @param measurementStdDevs Standard deviations of measurements. 058 * @param dtSeconds Nominal discretization timestep. 059 * @throws IllegalArgumentException If the system is unobservable. 060 */ 061 public SteadyStateKalmanFilter( 062 Nat<States> states, 063 Nat<Outputs> outputs, 064 LinearSystem<States, Inputs, Outputs> plant, 065 Matrix<States, N1> stateStdDevs, 066 Matrix<Outputs, N1> measurementStdDevs, 067 double dtSeconds) { 068 this.m_states = states; 069 070 this.m_plant = plant; 071 072 var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); 073 var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); 074 075 var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds); 076 var discA = pair.getFirst(); 077 var discQ = pair.getSecond(); 078 079 var discR = Discretization.discretizeR(contR, dtSeconds); 080 081 var C = plant.getC(); 082 083 if (!StateSpaceUtil.isDetectable(discA, C)) { 084 var msg = 085 "The system passed to the Kalman filter is unobservable!\n\nA =\n" 086 + discA.getStorage().toString() 087 + "\nC =\n" 088 + C.getStorage().toString() 089 + '\n'; 090 MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); 091 throw new IllegalArgumentException(msg); 092 } 093 094 var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); 095 096 // S = CPCᵀ + R 097 var S = C.times(P).times(C.transpose()).plus(discR); 098 099 // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more 100 // efficiently. 101 // 102 // K = PCᵀS⁻¹ 103 // KS = PCᵀ 104 // (KS)ᵀ = (PCᵀ)ᵀ 105 // SᵀKᵀ = CPᵀ 106 // 107 // The solution of Ax = b can be found via x = A.solve(b). 108 // 109 // Kᵀ = Sᵀ.solve(CPᵀ) 110 // K = (Sᵀ.solve(CPᵀ))ᵀ 111 m_K = 112 new Matrix<>( 113 S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose()); 114 115 reset(); 116 } 117 118 public final void reset() { 119 m_xHat = new Matrix<>(m_states, Nat.N1()); 120 } 121 122 /** 123 * Returns the steady-state Kalman gain matrix K. 124 * 125 * @return The steady-state Kalman gain matrix K. 126 */ 127 public Matrix<States, Outputs> getK() { 128 return m_K; 129 } 130 131 /** 132 * Returns an element of the steady-state Kalman gain matrix K. 133 * 134 * @param row Row of K. 135 * @param col Column of K. 136 * @return the element (i, j) of the steady-state Kalman gain matrix K. 137 */ 138 public double getK(int row, int col) { 139 return m_K.get(row, col); 140 } 141 142 /** 143 * Set initial state estimate x-hat. 144 * 145 * @param xhat The state estimate x-hat. 146 */ 147 public void setXhat(Matrix<States, N1> xhat) { 148 this.m_xHat = xhat; 149 } 150 151 /** 152 * Set an element of the initial state estimate x-hat. 153 * 154 * @param row Row of x-hat. 155 * @param value Value for element of x-hat. 156 */ 157 public void setXhat(int row, double value) { 158 m_xHat.set(row, 0, value); 159 } 160 161 /** 162 * Returns the state estimate x-hat. 163 * 164 * @return The state estimate x-hat. 165 */ 166 public Matrix<States, N1> getXhat() { 167 return m_xHat; 168 } 169 170 /** 171 * Returns an element of the state estimate x-hat. 172 * 173 * @param row Row of x-hat. 174 * @return the state estimate x-hat at that row. 175 */ 176 public double getXhat(int row) { 177 return m_xHat.get(row, 0); 178 } 179 180 /** 181 * Project the model into the future with a new control input u. 182 * 183 * @param u New control input from controller. 184 * @param dtSeconds Timestep for prediction. 185 */ 186 public void predict(Matrix<Inputs, N1> u, double dtSeconds) { 187 this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); 188 } 189 190 /** 191 * Correct the state estimate x-hat using the measurements in y. 192 * 193 * @param u Same control input used in the last predict step. 194 * @param y Measurement vector. 195 */ 196 public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) { 197 final var C = m_plant.getC(); 198 final var D = m_plant.getD(); 199 200 // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) 201 m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u))))); 202 } 203}