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.system; 006 007import edu.wpi.first.math.Matrix; 008import edu.wpi.first.math.Num; 009import edu.wpi.first.math.Pair; 010import org.ejml.simple.SimpleMatrix; 011 012public final class Discretization { 013 private Discretization() { 014 // Utility class 015 } 016 017 /** 018 * Discretizes the given continuous A matrix. 019 * 020 * @param <States> Num representing the number of states. 021 * @param contA Continuous system matrix. 022 * @param dtSeconds Discretization timestep. 023 * @return the discrete matrix system. 024 */ 025 public static <States extends Num> Matrix<States, States> discretizeA( 026 Matrix<States, States> contA, double dtSeconds) { 027 // A_d = eᴬᵀ 028 return contA.times(dtSeconds).exp(); 029 } 030 031 /** 032 * Discretizes the given continuous A and B matrices. 033 * 034 * @param <States> Nat representing the states of the system. 035 * @param <Inputs> Nat representing the inputs to the system. 036 * @param contA Continuous system matrix. 037 * @param contB Continuous input matrix. 038 * @param dtSeconds Discretization timestep. 039 * @return a Pair representing discA and diskB. 040 */ 041 public static <States extends Num, Inputs extends Num> 042 Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB( 043 Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) { 044 int states = contA.getNumRows(); 045 int inputs = contB.getNumCols(); 046 047 // M = [A B] 048 // [0 0] 049 var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs)); 050 M.assignBlock(0, 0, contA); 051 M.assignBlock(0, contA.getNumCols(), contB); 052 053 // ϕ = eᴹᵀ = [A_d B_d] 054 // [ 0 I ] 055 var phi = M.times(dtSeconds).exp(); 056 057 var discA = new Matrix<States, States>(new SimpleMatrix(states, states)); 058 discA.extractFrom(0, 0, phi); 059 060 var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs)); 061 discB.extractFrom(0, contB.getNumRows(), phi); 062 063 return new Pair<>(discA, discB); 064 } 065 066 /** 067 * Discretizes the given continuous A and Q matrices. 068 * 069 * @param <States> Nat representing the number of states. 070 * @param contA Continuous system matrix. 071 * @param contQ Continuous process noise covariance matrix. 072 * @param dtSeconds Discretization timestep. 073 * @return a pair representing the discrete system matrix and process noise covariance matrix. 074 */ 075 public static <States extends Num> 076 Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ( 077 Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) { 078 int states = contA.getNumRows(); 079 080 // Make continuous Q symmetric if it isn't already 081 Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0); 082 083 // M = [−A Q ] 084 // [ 0 Aᵀ] 085 final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states)); 086 M.assignBlock(0, 0, contA.times(-1.0)); 087 M.assignBlock(0, states, Q); 088 M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states))); 089 M.assignBlock(states, states, contA.transpose()); 090 091 // ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d] 092 // [ 0 A_dᵀ ] 093 final var phi = M.times(dtSeconds).exp(); 094 095 // ϕ₁₂ = A_d⁻¹Q_d 096 final Matrix<States, States> phi12 = phi.block(states, states, 0, states); 097 098 // ϕ₂₂ = A_dᵀ 099 final Matrix<States, States> phi22 = phi.block(states, states, states, states); 100 101 final var discA = phi22.transpose(); 102 103 Q = discA.times(phi12); 104 105 // Make discrete Q symmetric if it isn't already 106 final var discQ = Q.plus(Q.transpose()).div(2.0); 107 108 return new Pair<>(discA, discQ); 109 } 110 111 /** 112 * Returns a discretized version of the provided continuous measurement noise covariance matrix. 113 * Note that dt=0.0 divides R by zero. 114 * 115 * @param <O> Nat representing the number of outputs. 116 * @param contR Continuous measurement noise covariance matrix. 117 * @param dtSeconds Discretization timestep. 118 * @return Discretized version of the provided continuous measurement noise covariance matrix. 119 */ 120 public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) { 121 // R_d = 1/T R 122 return contR.div(dtSeconds); 123 } 124}