diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index d9c8a056dd..61f121d6a1 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -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 `
` tags in Javadoc so they render with monospace font.
+
 ## Submitting Changes
 
 ### Pull Request Format
diff --git a/build.gradle b/build.gradle
index 4446a5d7d3..d3b663fbec 100644
--- a/build.gradle
+++ b/build.gradle
@@ -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 ->
diff --git a/cameraserver/CMakeLists.txt b/cameraserver/CMakeLists.txt
index aa8e65513a..4916be3b66 100644
--- a/cameraserver/CMakeLists.txt
+++ b/cameraserver/CMakeLists.txt
@@ -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
 
diff --git a/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java b/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
index 204b414f03..11de96957e 100644
--- a/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
+++ b/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
@@ -47,6 +47,7 @@ public final class CameraServer {
   /**
    * Get the CameraServer instance.
    *
+   * @return The CameraServer instance.
    * @deprecated Use the static methods
    */
   @Deprecated
diff --git a/cscore/CMakeLists.txt b/cscore/CMakeLists.txt
index 3a9cbf2536..9ab9ca709c 100644
--- a/cscore/CMakeLists.txt
+++ b/cscore/CMakeLists.txt
@@ -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
 
diff --git a/docs/build.gradle b/docs/build.gradle
index ff871b9d33..8a80b0fb57 100644
--- a/docs/build.gradle
+++ b/docs/build.gradle
@@ -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"
diff --git a/hal/CMakeLists.txt b/hal/CMakeLists.txt
index 8bd764ffa2..fae74aa5f2 100644
--- a/hal/CMakeLists.txt
+++ b/hal/CMakeLists.txt
@@ -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)
 
diff --git a/ntcore/CMakeLists.txt b/ntcore/CMakeLists.txt
index 160e09af75..0833494dc7 100644
--- a/ntcore/CMakeLists.txt
+++ b/ntcore/CMakeLists.txt
@@ -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)
diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle
index d83ead2c6e..677c64dfab 100644
--- a/shared/java/javacommon.gradle
+++ b/shared/java/javacommon.gradle
@@ -96,7 +96,12 @@ sourceSets {
 }
 
 tasks.withType(JavaCompile).configureEach {
-    options.compilerArgs = ['--release', '11']
+    options.compilerArgs = [
+        '--release',
+        '11',
+        '-encoding',
+        'UTF8'
+    ]
 }
 
 dependencies {
diff --git a/wpilibNewCommands/CMakeLists.txt b/wpilibNewCommands/CMakeLists.txt
index 53652669e4..d44939f874 100644
--- a/wpilibNewCommands/CMakeLists.txt
+++ b/wpilibNewCommands/CMakeLists.txt
@@ -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)
diff --git a/wpilibOldCommands/CMakeLists.txt b/wpilibOldCommands/CMakeLists.txt
index 2c615b6fac..a9b7044f28 100644
--- a/wpilibOldCommands/CMakeLists.txt
+++ b/wpilibOldCommands/CMakeLists.txt
@@ -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)
diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index e510e18b81..bcb23e32e1 100644
--- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -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.
diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt
index 9a91fbe2d2..5d4e4edee3 100644
--- a/wpilibj/CMakeLists.txt
+++ b/wpilibj/CMakeLists.txt
@@ -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/)
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
index 0a9e59dff6..3401a75e36 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
@@ -29,10 +29,10 @@ import edu.wpi.first.wpilibj.RobotController;
  *
  * 

Our state-space system is: * - *

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]^T in the field coordinate system (dist_* are + *

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are * wheel distances.) * - *

u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep from a + *

u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a * LTVDiffDriveController. * *

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. diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 78bcf645ba..a141201662 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -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=") diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java index 99b3efa410..113758baa6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java @@ -290,7 +290,7 @@ public class Matrix { } /** - * Calculates the transpose, M^T of this matrix. + * Calculates the transpose, Mᵀ of this matrix. * * @return The transpose matrix. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 4f7d7267a9..fbc9cd3f66 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -88,8 +88,8 @@ public final class StateSpaceUtil { * Returns true if (A, B) is a stabilizable pair. * *

(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 Num representing the size of A. * @param Num representing the columns of B. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java index 97fa5faa28..1c957b7069 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java @@ -103,8 +103,8 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumRows(), 1)); @@ -137,8 +137,8 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumRows(), 1)); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 6bfb3e2199..6e0a231ac5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -36,15 +36,15 @@ import java.util.function.BiConsumer; * *

Our state-space system is: * - *

x = [[x, y, theta, dist_l, dist_r]]^T in the field coordinate system - * (dist_* are wheel distances.) + *

x = [[x, y, theta, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* + * are wheel distances.) * - *

u = [[vx, vy, omega]]^T (robot-relative velocities) -- NB: using velocities + *

u = [[vx, vy, omega]]ᵀ (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. * - *

y = [[x, y, theta]]^T from vision, or y = [[dist_l, dist_r, theta]] + *

y = [[x, y, theta]]ᵀ from vision, or y = [[dist_l, dist_r, theta]] * 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 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, diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index 16ed9503fa..fba97c3d94 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -141,7 +141,7 @@ public class ExtendedKalmanFilter 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); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java index 9643fc8379..d5e90271f7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java @@ -76,7 +76,7 @@ public class KalmanFilter( 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 u, Matrix 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))))); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index c460bd3a52..9053fb4e12 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -34,12 +34,12 @@ import java.util.function.BiConsumer; * *

Our state-space system is: * - *

x = [[x, y, theta]]^T in the field-coordinate system. + *

x = [[x, y, theta]]ᵀ in the field-coordinate system. * - *

u = [[vx, vy, theta]]^T in the field-coordinate system. + *

u = [[vx, vy, theta]]ᵀ in the field-coordinate system. * - *

y = [[x, y, theta]]^T in field coords from vision, or y = - * [[theta]]^T from the gyro. + *

y = [[x, y, theta]]ᵀ in field coords from vision, or y = + * [[theta]]ᵀ from the gyro. */ public class MecanumDrivePoseEstimator { private final UnscentedKalmanFilter 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 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, diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index d1b457958d..3bb3edde5b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -34,12 +34,12 @@ import java.util.function.BiConsumer; * *

Our state-space system is: * - *

x = [[x, y, theta]]^T in the field-coordinate system. + *

x = [[x, y, theta]]ᵀ in the field-coordinate system. * - *

u = [[vx, vy, omega]]^T in the field-coordinate system. + *

u = [[vx, vy, omega]]ᵀ in the field-coordinate system. * - *

y = [[x, y, theta]]^T in field coords from vision, or y = - * [[theta]]^T from the gyro. + *

y = [[x, y, theta]]ᵀ in field coords from vision, or y = + * [[theta]]ᵀ from the gyro. */ public class SwerveDrivePoseEstimator { private final UnscentedKalmanFilter 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 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, diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index 4eb0b816e7..f22adf13cf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -171,7 +171,9 @@ public class UnscentedKalmanFilter 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 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 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())); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java index 134de796d7..d85d663891 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java @@ -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. * - *

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]) + *

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]) * *

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-dt / T, T is the time constant in seconds. * *

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-dt / T, T is the time constant in seconds. * *

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]). * *

This filter is always stable. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java index 0a8eb5d23a..ffbd99e218 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java @@ -64,6 +64,7 @@ public final class Discretization { /** * Discretizes the given continuous A and Q matrices. * + * @param 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 lastTerm = Q.copy(); double lastCoeff = dtSeconds; - // A^T^n + // Aᵀⁿ Matrix Atn = contA.transpose(); Matrix 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); diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index c49bb6f2ff..41fd8ea621 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -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]. * *

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). diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 98613400a4..ae462f1170 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -252,7 +252,7 @@ Eigen::Matrix 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. diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index 8b6be757bc..30ba8b7c56 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -86,6 +86,8 @@ class LinearQuadraticRegulatorImpl { Eigen::Matrix 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 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 discA; Eigen::Matrix discB; DiscretizeAB(plant.A(), plant.B(), dt, &discA, &discB); + m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt); } diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 3a244c300e..95f2217f28 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -32,16 +32,16 @@ namespace frc { * * Our state-space system is: * - * x = [[x, y, theta, dist_l, dist_r]]^T in the field + * x = [[x, y, theta, dist_l, dist_r]]ᵀ in the field * coordinate system. * - * u = [[d_l, d_r, dtheta]]^T (robot-relative velocities) -- + * u = [[d_l, d_r, dtheta]]ᵀ (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 * - * y = [[x, y, theta]]^T from vision, + * y = [[x, y, theta]]ᵀ from vision, * or y = [[dist_l, dist_r, theta]] 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( diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 460cbfe81d..7e9e74a5fd 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -71,7 +71,7 @@ class ExtendedKalmanFilter { Eigen::Matrix discR = DiscretizeR(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(discA.transpose(), C.transpose()); if (isObservable && Outputs <= States) { @@ -137,7 +137,7 @@ class ExtendedKalmanFilter { Eigen::Matrix discR = DiscretizeR(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(discA.transpose(), C.transpose()); if (isObservable && Outputs <= States) { @@ -211,8 +211,6 @@ class ExtendedKalmanFilter { * @param dt Timestep for prediction. */ void Predict(const Eigen::Matrix& u, units::second_t dt) { - m_dt = dt; - // Find continuous A Eigen::Matrix contA = NumericalJacobianX(m_f, m_xHat, u); @@ -223,7 +221,11 @@ class ExtendedKalmanFilter { DiscretizeAQTaylor(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 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 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::Identity() - K * C) * m_P; } diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h index 2e8a7afe54..adf6fecf79 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -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(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 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& u, const Eigen::Matrix& y) { + // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁)) m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u)); } diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 0db20c916e..61f748c0cf 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -33,13 +33,13 @@ namespace frc { * * Our state-space system is: * - * x = [[x, y, theta]]^T in the + * x = [[x, y, theta]]ᵀ in the * field-coordinate system. * - * u = [[vx, vy, omega]]^T in the field-coordinate system. + * u = [[vx, vy, omega]]ᵀ in the field-coordinate system. * - * y = [[x, y, theta]]^T in field - * coords from vision, or y = [[theta]]^T + * y = [[x, y, theta]]ᵀ in field + * coords from vision, or y = [[theta]]ᵀ * 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( diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 6914086faa..d4a1c3121f 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -36,13 +36,13 @@ namespace frc { * * Our state-space system is: * - * x = [[x, y, theta]]^T in the + * x = [[x, y, theta]]ᵀ in the * field-coordinate system. * - * u = [[vx, vy, omega]]^T in the field-coordinate system. + * u = [[vx, vy, omega]]ᵀ in the field-coordinate system. * - * y = [[x, y, theta]]^T in field coords from vision, - * or y = [[theta]]^T from the gyro. + * y = [[x, y, theta]]ᵀ in field coords from vision, + * or y = [[theta]]ᵀ from the gyro. */ template 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( diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 7ad9c24a2a..8ce39ad446 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -343,6 +343,7 @@ class UnscentedKalmanFilter { Eigen::Matrix 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(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 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(); } diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h index bf056593df..3e4592d2a8 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -41,14 +41,16 @@ UnscentedTransform(const Eigen::Matrix& sigmas, const Eigen::Matrix&)> 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 x = meanFunc(sigmas, Wm); // New covariance is the sum of the outer product of the residuals times the // weights Eigen::Matrix y; for (int i = 0; i < 2 * States + 1; ++i) { - // y[:, i] = sigmas[:, i] - x; + // y[:, i] = sigmas[:, i] - x y.template block(0, i) = residualFunc(sigmas.template block(0, i), x); } diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 1c22ba1330..925c67b8aa 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -24,8 +24,8 @@ namespace frc { * used types of filters. * * 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]) + * 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:
* y[n] is the output at time "n"
@@ -109,7 +109,7 @@ class LinearFilter { // Static methods to create commonly used filters /** * Creates a one-pole IIR low-pass filter of the form:
- * y[n] = (1 - gain) * x[n] + gain * y[n-1]
+ * y[n] = (1 - gain) x[n] + gain y[n-1]
* where gain = e-dt / T, 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:
- * y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]
+ * 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 * * 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:
- * 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. * diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h index b9e8c6e80a..4216d5b9b3 100644 --- a/wpimath/src/main/native/include/frc/system/Discretization.h +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -125,7 +125,7 @@ void DiscretizeAQTaylor(const Eigen::Matrix& contA, Eigen::Matrix lastTerm = Q; double lastCoeff = dt.to(); - // A^T^n + // Aᵀⁿ Eigen::Matrix Atn = contA.transpose(); Eigen::Matrix phi12 = lastTerm * lastCoeff; diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 8cc18b2a34..252e6958dc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -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 m_swerveObserverSystem = new LinearSystem<>( Matrix.mat(Nat.N6(), Nat.N6()) diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java index 54f9cd6347..add1afb31f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java @@ -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); diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index 70e4e4b1e3..55862d476a 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -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 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 contA; contA << 0, 1, 0, -1406.29; diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index adddc58b2e..a152018b3b 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -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=")