Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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
CONTRIBUTING.md
View File

@@ -41,6 +41,8 @@ WPILib uses modified Google style guides for both C++ and Java, which can be fou
While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `<pre>` tags in Javadoc so they render with monospace font.
## Submitting Changes
### Pull Request Format

7
build.gradle
View File

@@ -100,6 +100,13 @@ subprojects {
}
}
// Enables UTF-8 support in Javadoc
tasks.withType(Javadoc) {
options.addStringOption("charset", "utf-8")
options.addStringOption("docencoding", "utf-8")
options.addStringOption("encoding", "utf-8")
}
// Sign outputs with Developer ID
if (project.hasProperty("developerID")) {
tasks.withType(AbstractLinkTask) { task ->

2
cameraserver/CMakeLists.txt
View File

@@ -9,7 +9,7 @@ find_package( OpenCV REQUIRED )
if (WITH_JAVA)
find_package(Java REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
#find java files, copy them locally

1
cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
View File

@@ -47,6 +47,7 @@ public final class CameraServer {
/**
* Get the CameraServer instance.
*
* @return The CameraServer instance.
* @deprecated Use the static methods
*/
@Deprecated

2
cscore/CMakeLists.txt
View File

@@ -79,7 +79,7 @@ if (WITH_JAVA)
find_package(Java REQUIRED)
find_package(JNI REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
#find java files, copy them locally

5
docs/build.gradle
View File

@@ -108,8 +108,8 @@ apply from: "${rootDir}/shared/opencv.gradle"
task generateJavaDocs(type: Javadoc) {
classpath += project(":wpimath").sourceSets.main.compileClasspath
options.links("https://docs.oracle.com/en/java/javase/11/docs/api/")
options.addStringOption "tag", "pre:a:Pre-Condition"
options.addBooleanOption "Xdoclint:html,missing,reference,syntax", true
options.addStringOption("tag", "pre:a:Pre-Condition")
options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
options.addBooleanOption('html5', true)
dependsOn project(':wpilibj').generateJavaVersion
dependsOn project(':hal').generateUsageReporting
@@ -126,7 +126,6 @@ task generateJavaDocs(type: Javadoc) {
source configurations.javaSource.collect { zipTree(it) }
include '**/*.java'
failOnError = true
options.encoding = 'UTF-8'
title = "WPILib API ${wpilibVersioning.version.get()}"
ext.entryPoint = "$destinationDir/index.html"

2
hal/CMakeLists.txt
View File

@@ -74,7 +74,7 @@ if (WITH_JAVA)
find_package(Java REQUIRED)
find_package(JNI REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
configure_file(src/generate/FRCNetComm.java.in FRCNetComm.java)

2
ntcore/CMakeLists.txt
View File

@@ -35,7 +35,7 @@ if (WITH_JAVA)
find_package(Java REQUIRED)
find_package(JNI REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
file(GLOB
ntcore_jni_src src/main/native/cpp/jni/NetworkTablesJNI.cpp)

7
shared/java/javacommon.gradle
View File

@@ -96,7 +96,12 @@ sourceSets {
}
tasks.withType(JavaCompile).configureEach {
options.compilerArgs = ['--release', '11']
options.compilerArgs = [
'--release',
'11',
'-encoding',
'UTF8'
]
}
dependencies {

2
wpilibNewCommands/CMakeLists.txt
View File

@@ -7,7 +7,7 @@ include(AddTest)
if (WITH_JAVA)
find_package(Java REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands)

2
wpilibOldCommands/CMakeLists.txt
View File

@@ -7,7 +7,7 @@ include(AddTest)
if (WITH_JAVA)
find_package(Java REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
add_jar(wpilibOldCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibOldCommands)

8
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -35,7 +35,7 @@ class DifferentialDrivetrainSim {
* meters.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. Can be omitted if no noise is desired. Gyro standard
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.
@@ -61,7 +61,7 @@ class DifferentialDrivetrainSim {
* right wheels.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. Can be omitted if no noise is desired. Gyro standard
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.
@@ -242,7 +242,7 @@ class DifferentialDrivetrainSim {
* @param wheelSize The wheel size.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. Can be omitted if no noise is desired. Gyro standard
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.
@@ -271,7 +271,7 @@ class DifferentialDrivetrainSim {
* calculated using frc-characterization.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]^T. Can be omitted if no noise is desired. Gyro standard
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.

2
wpilibj/CMakeLists.txt
View File

@@ -6,7 +6,7 @@ find_package( OpenCV REQUIRED )
if (WITH_JAVA)
find_package(Java REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -29,10 +29,10 @@ import edu.wpi.first.wpilibj.RobotController;
*
* <p>Our state-space system is:
*
* <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]^T in the field coordinate system (dist_* are
* <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are
* wheel distances.)
*
* <p>u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep from a
* <p>u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a
* LTVDiffDriveController.
*
* <p>y = x
@@ -67,7 +67,7 @@ public class DifferentialDrivetrainSim {
* @param wheelRadiusMeters The radius of the wheels on the drivetrain.
* @param trackWidthMeters The robot's track width, or distance between left and right wheels.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]^T. Can be null if no noise is
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
* m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
* point.
@@ -112,7 +112,7 @@ public class DifferentialDrivetrainSim {
* frc-characterization.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]^T. Can be null if no noise is
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
* m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
* point.
@@ -447,7 +447,7 @@ public class DifferentialDrivetrainSim {
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]^T. Can be null if no noise is
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
* m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
* point.
@@ -476,7 +476,7 @@ public class DifferentialDrivetrainSim {
* @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
* frc-characterization.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]^T. Can be null if no noise is
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
* m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
* point.

2
wpimath/CMakeLists.txt
View File

@@ -11,7 +11,7 @@ if (WITH_JAVA)
find_package(Java REQUIRED)
find_package(JNI REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
set(BASE_URL "https://search.maven.org/remotecontent?filepath=")

2
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -290,7 +290,7 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Calculates the transpose, M^T of this matrix.
* Calculates the transpose, Mᵀ of this matrix.
*
* @return The transpose matrix.
*/

4
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -88,8 +88,8 @@ public final class StateSpaceUtil {
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(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)
* %3C n where n is number of states.
* absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
* where n is number of states.
*
* @param <States> Num representing the size of A.
* @param <Inputs> Num representing the columns of B.

4
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -103,8 +103,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
@@ -137,8 +137,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));

24
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -36,15 +36,15 @@ import java.util.function.BiConsumer;
*
* <p>Our state-space system is:
*
* <p><strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field coordinate system
* (dist_* are wheel distances.)
* <p><strong> x = [[x, y, theta, dist_l, dist_r]]ᵀ </strong> in the field coordinate system (dist_*
* are wheel distances.)
*
* <p><strong> u = [[vx, vy, omega]]^T </strong> (robot-relative velocities) -- NB: using velocities
* <p><strong> u = [[vx, vy, omega]]ᵀ </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.
*
* <p><strong>y = [[x, y, theta]]^T </strong> from vision, or <strong>y = [[dist_l, dist_r, theta]]
* <p><strong>y = [[x, y, theta]]ᵀ </strong> from vision, or <strong>y = [[dist_l, dist_r, theta]]
* </strong> from encoders and gyro.
*/
public class DifferentialDrivePoseEstimator {
@@ -66,14 +66,14 @@ public class DifferentialDrivePoseEstimator {
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @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, dist_l, dist_r]^T,
* model's state estimates less. This matrix is in the form [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 meters and radians.
* is in the form [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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle,
@@ -96,14 +96,14 @@ public class DifferentialDrivePoseEstimator {
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @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, dist_l, dist_r]^T,
* model's state estimates less. This matrix is in the form [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 meters and radians.
* is in the form [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 radians.
* theta]ᵀ, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -160,7 +160,7 @@ public class DifferentialDrivePoseEstimator {
*
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -279,7 +279,7 @@ public class DifferentialDrivePoseEstimator {
* source in this case.
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,

24
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -141,7 +141,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discR = Discretization.discretizeR(m_contR, dtSeconds);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
// IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
if (isObservable && outputs.getNum() <= states.getNum()) {
m_initP =
@@ -267,7 +267,10 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discQ = discPair.getSecond();
m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
m_dtSeconds = dtSeconds;
}
@@ -338,23 +341,24 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var S = C.times(m_P).times(C.transpose()).plus(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
//
// Now we have the Kalman gain
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
// Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
}
}

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

@@ -76,7 +76,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
var C = plant.getC();
// isStabilizable(A^T, C^T) will tell us if the system is observable.
// isStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
if (!isObservable) {
MathSharedStore.reportError(
@@ -90,20 +90,21 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
new Matrix<>(
Drake.discreteAlgebraicRiccatiEquation(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^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 =
new Matrix<>(
S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
@@ -194,6 +195,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
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)))));
}
}

20
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -34,12 +34,12 @@ import java.util.function.BiConsumer;
*
* <p>Our state-space system is:
*
* <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
* <p><strong> x = [[x, y, theta]]ᵀ </strong> in the field-coordinate system.
*
* <p><strong> u = [[vx, vy, theta]]^T </strong> in the field-coordinate system.
* <p><strong> u = [[vx, vy, theta]]ᵀ </strong> in the field-coordinate system.
*
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
* [[theta]]^T </strong> from the gyro.
* <p><strong> y = [[x, y, theta]]ᵀ </strong> in field coords from vision, or <strong> y =
* [[theta]]ᵀ </strong> from the gyro.
*/
public class MecanumDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
@@ -62,14 +62,14 @@ public class MecanumDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @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 in
* model's state estimates less. This matrix 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 trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in 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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
@@ -95,14 +95,14 @@ public class MecanumDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @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 in
* model's state estimates less. This matrix 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 trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in 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 radians.
* theta]ᵀ, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -161,7 +161,7 @@ public class MecanumDrivePoseEstimator {
*
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -242,7 +242,7 @@ public class MecanumDrivePoseEstimator {
* Timer.getFPGATimestamp as your time source in this case.
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,

20
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -34,12 +34,12 @@ import java.util.function.BiConsumer;
*
* <p>Our state-space system is:
*
* <p><strong> x = [[x, y, theta]]^T </strong> in the field-coordinate system.
* <p><strong> x = [[x, y, theta]]ᵀ </strong> in the field-coordinate system.
*
* <p><strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
* <p><strong> u = [[vx, vy, omega]]ᵀ </strong> in the field-coordinate system.
*
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
* [[theta]]^T </strong> from the gyro.
* <p><strong> y = [[x, y, theta]]ᵀ </strong> in field coords from vision, or <strong> y =
* [[theta]]ᵀ </strong> from the gyro.
*/
public class SwerveDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
@@ -62,14 +62,14 @@ public class SwerveDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @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 in
* model's state estimates less. This matrix 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 trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in 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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public SwerveDrivePoseEstimator(
Rotation2d gyroAngle,
@@ -95,14 +95,14 @@ public class SwerveDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @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 in
* model's state estimates less. This matrix 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 trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in 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 radians.
* theta]ᵀ, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -161,7 +161,7 @@ public class SwerveDrivePoseEstimator {
*
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -242,7 +242,7 @@ public class SwerveDrivePoseEstimator {
* Timer.getFPGATimestamp as your time source in this case.
* @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 radians.
* theta]ᵀ, with units in meters and radians.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,

20
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -171,7 +171,9 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
}
// 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
Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
@@ -403,22 +405,24 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
// Compute cross covariance of the state and the measurements
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
// Pxy += (sigmas_f[:, i] - xHat) * (sigmas_h[:, i] - yHat)^T * W_c[i]
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
}
// 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}ᵀ)ᵀ
Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
// xHat + K * (y - yHat)
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
// Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
m_P = m_P.minus(K.times(Py).times(K.transpose()));
}
}

15
wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
View File

@@ -13,8 +13,8 @@ import java.util.Arrays;
* This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
* Static factory methods are provided to create commonly used types of filters.
*
* <p>Filters are of the form: 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])
* <p>Filters are of the form: 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])
*
* <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
* the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
@@ -71,8 +71,8 @@ public class LinearFilter {
}
/**
* Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
* gain = e^(-dt / T), T is the time constant in seconds.
* Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain) x[n] + gain y[n-1] where
* gain = e<sup>-dt / T</sup>, T is the time constant in seconds.
*
* <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency above which the
* input starts to attenuate.
@@ -92,8 +92,8 @@ public class LinearFilter {
}
/**
* Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
* gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
* Creates a first-order high-pass filter of the form: y[n] = gain x[n] + (-gain) x[n-1] + gain
* y[n-1] where gain = e<sup>-dt / T</sup>, T is the time constant in seconds.
*
* <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency below which the
* input starts to attenuate.
@@ -113,8 +113,7 @@ public class LinearFilter {
}
/**
* Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
* x[0]).
* Creates a K-tap FIR moving average filter of the form: y[n] = 1/k (x[k] + x[k-1] + ... + x[0]).
*
* <p>This filter is always stable.
*

5
wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View File

@@ -64,6 +64,7 @@ public final class Discretization {
/**
* Discretizes the given continuous A and Q matrices.
*
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param dtSeconds Discretization timestep.
@@ -128,11 +129,11 @@ public final class Discretization {
Matrix<States, States> lastTerm = Q.copy();
double lastCoeff = dtSeconds;
// A^T^n
// Aᵀⁿ
Matrix<States, States> Atn = contA.transpose();
Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
// i = 6 i.e. 6th order should be enough precision
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
lastCoeff *= dtSeconds / ((double) i);

14
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -18,7 +18,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of an elevator system. The states of the system are [position,
* velocity]^T, inputs are [voltage], and outputs are [position].
* velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
* @param motor The motor (or gearbox) attached to the arm.
* @param massKg The mass of the elevator carriage, in kilograms.
@@ -72,7 +72,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
* [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are [v_left, v_right]^T.
* [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.
*
* @param motor the gearbox representing the motors driving the drivetrain.
* @param massKg the mass of the robot.
@@ -157,7 +157,7 @@ public final class LinearSystemId {
/**
* Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
* constants cam be found using frc-characterization. The states of the system are [position,
* velocity]^T, inputs are [voltage], and outputs are [position].
* velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.
@@ -181,8 +181,8 @@ public final class LinearSystemId {
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
* volts/(meter/sec^2)) cases. This can be found using frc-characterization. The states of the
* system are [left velocity, right velocity]^T, inputs are [left voltage, right voltage]^T, and
* outputs are [left velocity, right velocity]^T.
* system are [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and
* outputs are [left velocity, right velocity]ᵀ.
*
* @param kVLinear The linear velocity gain, volts per (meter per second).
* @param kALinear The linear acceleration gain, volts per (meter per second squared).
@@ -211,8 +211,8 @@ public final class LinearSystemId {
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
* volts/(radian/sec^2)) cases. This can be found using frc-characterization. The states of the
* system are [left velocity, right velocity]^T, inputs are [left voltage, right voltage]^T, and
* outputs are [left velocity, right velocity]^T.
* system are [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and
* outputs are [left velocity, right velocity]ᵀ.
*
* @param kVLinear The linear velocity gain, volts per (meter per second).
* @param kALinear The linear acceleration gain, volts per (meter per second squared).

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;

4
wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
View File

@@ -45,8 +45,8 @@ public class KalmanFilterTest {
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
}
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T,
// Y is [x, y, theta]^T and u is [ax, ay, alpha}^T
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,
// Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
LinearSystem<N6, N3, N3> m_swerveObserverSystem =
new LinearSystem<>(
Matrix.mat(Nat.N6(), Nat.N6())

10
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View File

@@ -58,8 +58,9 @@ public class DiscretizationTest {
assertEquals(x1Truth, x1Discrete);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
// dt
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
@Test
public void testDiscretizeSlowModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
@@ -86,8 +87,9 @@ public class DiscretizationTest {
+ discQIntegrated);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
// dt
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
@Test
public void testDiscretizeFastModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);

10
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -60,8 +60,9 @@ TEST(DiscretizationTest, DiscretizeAB) {
EXPECT_EQ(x1Truth, x1Discrete);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
// dt
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
@@ -92,8 +93,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
<< discQIntegrated;
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
// dt
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, -1406.29;

2
wpiutil/CMakeLists.txt
View File

@@ -12,7 +12,7 @@ if (WITH_JAVA)
find_package(Java REQUIRED)
find_package(JNI REQUIRED)
include(UseJava)
set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar")
set(BASE_URL "https://search.maven.org/remotecontent?filepath=")