Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use unicode characters in docs equations (#3487)

Browse Source 
javac and javadoc needed the encoding set to UTF-8.
This commit is contained in:
Tyler Veness
2021-07-29 22:42:43 -07:00
committed by GitHub
parent 85748f2e6f
commit 3838cc4ec4
42 changed files with 216 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -252,7 +252,7 @@ Eigen::Matrix<double, 4, 1> PoseTo4dVector(const Pose2d& pose);
*
* (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank(lambda * I - A, B) < n where n is number of states.
* uncontrollable if rank(λI - A, B) < n where n is number of states.
*
* @param A System matrix.
* @param B Input matrix.

5
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -86,6 +86,8 @@ class LinearQuadraticRegulatorImpl {
Eigen::Matrix<double, States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA);
@@ -115,6 +117,8 @@ class LinearQuadraticRegulatorImpl {
Eigen::Matrix<double, States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (B.transpose() * S * B + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
@@ -225,6 +229,7 @@ class LinearQuadraticRegulatorImpl {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
}

16
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -32,16 +32,16 @@ namespace frc {
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field
* <strong> x = [[x, y, theta, dist_l, dist_r]]ᵀ </strong> in the field
* coordinate system.
*
* <strong> u = [[d_l, d_r, dtheta]]^T </strong> (robot-relative velocities) --
* <strong> u = [[d_l, d_r, dtheta]]ᵀ </strong> (robot-relative velocities) --
* NB: using velocities make things considerably easier, because it means that
* teams don't have to worry about getting an accurate model. Basically, we
* suspect that it's easier for teams to get good encoder data than it is for
* them to perform system identification well enough to get a good model
*
* <strong>y = [[x, y, theta]]^T </strong> from vision,
* <strong>y = [[x, y, theta]]ᵀ </strong> from vision,
* or <strong>y = [[dist_l, dist_r, theta]] </strong> from encoders and gyro.
*/
class DifferentialDrivePoseEstimator {
@@ -55,20 +55,20 @@ class DifferentialDrivePoseEstimator {
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form
* [x, y, theta, dist_l, dist_r]^T,
* [x, y, theta, dist_l, dist_r]ᵀ,
* with units in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
* trust sensor readings from
* encoders and gyros less.
* This matrix is in the form
* [dist_l, dist_r, theta]^T, with units in
* [dist_l, dist_r, theta]ᵀ, with units in
* meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from
* vision less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
* @param nominalDt The period of the loop calling Update().
*/
@@ -88,7 +88,7 @@ class DifferentialDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void SetVisionMeasurementStdDevs(
@@ -163,7 +163,7 @@ class DifferentialDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void AddVisionMeasurement(

27
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -71,7 +71,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
@@ -137,7 +137,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
@@ -211,8 +211,6 @@ class ExtendedKalmanFilter {
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
m_dt = dt;
// Find continuous A
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -223,7 +221,11 @@ class ExtendedKalmanFilter {
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
/**
@@ -292,22 +294,25 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PC^T S^-1
// KS = PC^T
// (KS)^T = (PC^T)^T
// S^T K^T = CP^T
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// K^T = S^T.solve(CP^T)
// K = (S^T.solve(CP^T))^T
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Eigen::Matrix<double, States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
// Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
}

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

@@ -64,7 +64,7 @@ class KalmanFilterImpl {
const auto& C = plant.C();
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (!isObservable) {
@@ -78,20 +78,21 @@ class KalmanFilterImpl {
drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PC^T S^-1
// KS = PC^T
// (KS)^T = (PC^T)^T
// S^T K^T = CP^T
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// K^T = S^T.solve(CP^T)
// K = (S^T.solve(CP^T))^T
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Reset();
@@ -163,6 +164,7 @@ class KalmanFilterImpl {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
}

16
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -33,13 +33,13 @@ namespace frc {
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta]]^T </strong> in the
* <strong> x = [[x, y, theta]]ᵀ </strong> in the
* field-coordinate system.
*
* <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
* <strong> u = [[vx, vy, omega]]ᵀ </strong> in the field-coordinate system.
*
* <strong> y = [[x, y, theta]]^T </strong> in field
* coords from vision, or <strong> y = [[theta]]^T
* <strong> y = [[x, y, theta]]ᵀ </strong> in field
* coords from vision, or <strong> y = [[theta]]ᵀ
* </strong> from the gyro.
*/
class MecanumDrivePoseEstimator {
@@ -54,7 +54,7 @@ class MecanumDrivePoseEstimator {
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
@@ -65,7 +65,7 @@ class MecanumDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
* @param nominalDt The time in seconds between each robot
* loop.
@@ -87,7 +87,7 @@ class MecanumDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void SetVisionMeasurementStdDevs(
@@ -163,7 +163,7 @@ class MecanumDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void AddVisionMeasurement(

16
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -36,13 +36,13 @@ namespace frc {
*
* Our state-space system is:
*
* <strong> x = [[x, y, theta]]^T </strong> in the
* <strong> x = [[x, y, theta]]ᵀ </strong> in the
* field-coordinate system.
*
* <strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
* <strong> u = [[vx, vy, omega]]ᵀ </strong> in the field-coordinate system.
*
* <strong> y = [[x, y, theta]]^T </strong> in field coords from vision,
* or <strong> y = [[theta]]^T </strong> from the gyro.
* <strong> y = [[x, y, theta]]ᵀ </strong> in field coords from vision,
* or <strong> y = [[theta]]ᵀ </strong> from the gyro.
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator {
@@ -57,7 +57,7 @@ class SwerveDrivePoseEstimator {
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
@@ -68,7 +68,7 @@ class SwerveDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
* @param nominalDt The time in seconds between each robot
* loop.
@@ -155,7 +155,7 @@ class SwerveDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void SetVisionMeasurementStdDevs(
@@ -217,7 +217,7 @@ class SwerveDrivePoseEstimator {
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* [x, y, theta]ᵀ, with units in meters and
* radians.
*/
void AddVisionMeasurement(

14
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -343,6 +343,7 @@ class UnscentedKalmanFilter {
Eigen::Matrix<double, States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -350,15 +351,18 @@ class UnscentedKalmanFilter {
.transpose();
}
// K = P_{xy} Py^-1
// K^T = P_y^T^-1 P_{xy}^T
// P_y^T K^T = P_{xy}^T
// K^T = P_y^T.solve(P_{xy}^T)
// K = (P_y^T.solve(P_{xy}^T)^T
// K = P_{xy} P_y⁻¹
// Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
Eigen::Matrix<double, States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
// Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
m_P -= K * Py * K.transpose();
}

6
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -41,14 +41,16 @@ UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Matrix<double, CovDim, 1>&)>
residualFunc) {
// New mean is usually just the sum of the sigmas * weight:
// dot = \Sigma^n_1 (W[k]*Xi[k])
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
Eigen::Matrix<double, CovDim, 1> x = meanFunc(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Eigen::Matrix<double, CovDim, 2 * States + 1> y;
for (int i = 0; i < 2 * States + 1; ++i) {
// y[:, i] = sigmas[:, i] - x;
// y[:, i] = sigmas[:, i] - x
y.template block<CovDim, 1>(0, i) =
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
}

10
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -24,8 +24,8 @@ namespace frc {
* used types of filters.
*
* Filters are of the form:<br>
* y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
* (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
* y[n] = (b0 x[n] + b1 x[n-1] + … + bP x[n-P]) -
* (a0 y[n-1] + a2 y[n-2] + … + aQ y[n-Q])
*
* Where:<br>
* y[n] is the output at time "n"<br>
@@ -109,7 +109,7 @@ class LinearFilter {
// Static methods to create commonly used filters
/**
* Creates a one-pole IIR low-pass filter of the form:<br>
* y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
* y[n] = (1 - gain) x[n] + gain y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
@@ -129,7 +129,7 @@ class LinearFilter {
/**
* Creates a first-order high-pass filter of the form:<br>
* y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
* y[n] = gain x[n] + (-gain) x[n-1] + gain y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
@@ -148,7 +148,7 @@ class LinearFilter {
/**
* Creates a K-tap FIR moving average filter of the form:<br>
* y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
* y[n] = 1/k (x[k] + x[k-1] + … + x[0])
*
* This filter is always stable.
*

2
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -125,7 +125,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
Eigen::Matrix<double, States, States> lastTerm = Q;
double lastCoeff = dt.to<double>();
// A^T^n
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;