[wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)

The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.

This is a breaking change but is worthwhile to avoid hard-to-debug errors.

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include <array>
+#include <wpi/array.h>
 
 #include "Eigen/Core"
 #include "frc/estimator/KalmanFilterLatencyCompensator.h"
@@ -74,9 +74,9 @@ class DifferentialDrivePoseEstimator {
    */
   DifferentialDrivePoseEstimator(
       const Rotation2d& gyroAngle, const Pose2d& initialPose,
-      const std::array<double, 5>& stateStdDevs,
-      const std::array<double, 3>& localMeasurementStdDevs,
-      const std::array<double, 3>& visionMeasurementStdDevs,
+      const wpi::array<double, 5>& stateStdDevs,
+      const wpi::array<double, 3>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
       units::second_t nominalDt = 0.02_s);
 
   /**
@@ -92,7 +92,7 @@ class DifferentialDrivePoseEstimator {
    *                                 radians.
    */
   void SetVisionMeasurementStdDevs(
-      const std::array<double, 3>& visionMeasurementStdDevs);
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
 
   /**
    * Resets the robot's position on the field.
@@ -183,7 +183,7 @@ class DifferentialDrivePoseEstimator {
   Rotation2d m_previousAngle;
 
   template <int Dim>
-  static std::array<double, Dim> StdDevMatrixToArray(
+  static wpi::array<double, Dim> StdDevMatrixToArray(
       const Eigen::Matrix<double, Dim, 1>& stdDevs);
 
   static Eigen::Matrix<double, 5, 1> F(const Eigen::Matrix<double, 5, 1>& x,

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -4,9 +4,10 @@
 
 #pragma once
 
-#include <array>
 #include <functional>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LDLT.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
@@ -40,8 +41,8 @@ class ExtendedKalmanFilter {
                            const Eigen::Matrix<double, States, 1>&,
                            const Eigen::Matrix<double, Inputs, 1>&)>
                            h,
-                       const std::array<double, States>& stateStdDevs,
-                       const std::array<double, Outputs>& measurementStdDevs,
+                       const wpi::array<double, States>& stateStdDevs,
+                       const wpi::array<double, Outputs>& measurementStdDevs,
                        units::second_t dt)
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -4,9 +4,10 @@
 
 #pragma once
 
-#include <array>
 #include <cmath>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LDLT.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
@@ -47,8 +48,8 @@ class KalmanFilterImpl {
    * @param dt                 Nominal discretization timestep.
    */
   KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
-                   const std::array<double, States>& stateStdDevs,
-                   const std::array<double, Outputs>& measurementStdDevs,
+                   const wpi::array<double, States>& stateStdDevs,
+                   const wpi::array<double, Outputs>& measurementStdDevs,
                    units::second_t dt) {
     m_plant = &plant;
 
@@ -193,8 +194,8 @@ class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
    * @param dt                 Nominal discretization timestep.
    */
   KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
-               const std::array<double, States>& stateStdDevs,
-               const std::array<double, Outputs>& measurementStdDevs,
+               const wpi::array<double, States>& stateStdDevs,
+               const wpi::array<double, Outputs>& measurementStdDevs,
                units::second_t dt)
       : detail::KalmanFilterImpl<States, Inputs, Outputs>{
             plant, stateStdDevs, measurementStdDevs, dt} {}
@@ -209,8 +210,8 @@ template <>
 class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> {
  public:
   KalmanFilter(LinearSystem<1, 1, 1>& plant,
-               const std::array<double, 1>& stateStdDevs,
-               const std::array<double, 1>& measurementStdDevs,
+               const wpi::array<double, 1>& stateStdDevs,
+               const wpi::array<double, 1>& measurementStdDevs,
                units::second_t dt);
 
   KalmanFilter(KalmanFilter&&) = default;
@@ -223,8 +224,8 @@ template <>
 class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> {
  public:
   KalmanFilter(LinearSystem<2, 1, 1>& plant,
-               const std::array<double, 2>& stateStdDevs,
-               const std::array<double, 1>& measurementStdDevs,
+               const wpi::array<double, 2>& stateStdDevs,
+               const wpi::array<double, 1>& measurementStdDevs,
                units::second_t dt);
 
   KalmanFilter(KalmanFilter&&) = default;

wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h

@@ -6,6 +6,8 @@
 
 #include <functional>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "frc/estimator/KalmanFilterLatencyCompensator.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
@@ -71,9 +73,9 @@ class MecanumDrivePoseEstimator {
   MecanumDrivePoseEstimator(
       const Rotation2d& gyroAngle, const Pose2d& initialPose,
       MecanumDriveKinematics kinematics,
-      const std::array<double, 3>& stateStdDevs,
-      const std::array<double, 1>& localMeasurementStdDevs,
-      const std::array<double, 3>& visionMeasurementStdDevs,
+      const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 1>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
       units::second_t nominalDt = 0.02_s);
 
   /**
@@ -89,7 +91,7 @@ class MecanumDrivePoseEstimator {
    *                                 radians.
    */
   void SetVisionMeasurementStdDevs(
-      const std::array<double, 3>& visionMeasurementStdDevs);
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
 
   /**
    * Resets the robot's position on the field.
@@ -177,9 +179,9 @@ class MecanumDrivePoseEstimator {
   Rotation2d m_previousAngle;
 
   template <int Dim>
-  static std::array<double, Dim> StdDevMatrixToArray(
+  static wpi::array<double, Dim> StdDevMatrixToArray(
       const Eigen::Matrix<double, Dim, 1>& vector) {
-    std::array<double, Dim> array;
+    wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);
     }

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -4,9 +4,9 @@
 
 #pragma once
 
-#include <array>
 #include <limits>
 
+#include <wpi/array.h>
 #include <wpi/timestamp.h>
 
 #include "Eigen/Core"
@@ -77,9 +77,9 @@ class SwerveDrivePoseEstimator {
   SwerveDrivePoseEstimator(
       const Rotation2d& gyroAngle, const Pose2d& initialPose,
       SwerveDriveKinematics<NumModules>& kinematics,
-      const std::array<double, 3>& stateStdDevs,
-      const std::array<double, 1>& localMeasurementStdDevs,
-      const std::array<double, 3>& visionMeasurementStdDevs,
+      const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 1>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
       units::second_t nominalDt = 0.02_s)
       : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
                       const Eigen::Matrix<double, 3, 1>& u) { return u; },
@@ -158,7 +158,7 @@ class SwerveDrivePoseEstimator {
    *                                 radians.
    */
   void SetVisionMeasurementStdDevs(
-      const std::array<double, 3>& visionMeasurementStdDevs) {
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
     // Create R (covariances) for vision measurements.
     Eigen::Matrix<double, 3, 3> visionContR =
         frc::MakeCovMatrix(visionMeasurementStdDevs);
@@ -266,9 +266,9 @@ class SwerveDrivePoseEstimator {
   Rotation2d m_previousAngle;
 
   template <int Dim>
-  static std::array<double, Dim> StdDevMatrixToArray(
+  static wpi::array<double, Dim> StdDevMatrixToArray(
       const Eigen::Matrix<double, Dim, 1>& vector) {
-    std::array<double, Dim> array;
+    wpi::array<double, Dim> array;
     for (size_t i = 0; i < Dim; ++i) {
       array[i] = vector(i);
     }

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -4,9 +4,10 @@
 
 #pragma once
 
-#include <array>
 #include <functional>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LDLT.h"
 #include "frc/StateSpaceUtil.h"
@@ -41,8 +42,8 @@ class UnscentedKalmanFilter {
                             const Eigen::Matrix<double, States, 1>&,
                             const Eigen::Matrix<double, Inputs, 1>&)>
                             h,
-                        const std::array<double, States>& stateStdDevs,
-                        const std::array<double, Outputs>& measurementStdDevs,
+                        const wpi::array<double, States>& stateStdDevs,
+                        const wpi::array<double, Outputs>& measurementStdDevs,
                         units::second_t dt)
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
@@ -101,8 +102,8 @@ class UnscentedKalmanFilter {
           const Eigen::Matrix<double, States, 1>&,
           const Eigen::Matrix<double, Inputs, 1>&)>
           h,
-      const std::array<double, States>& stateStdDevs,
-      const std::array<double, Outputs>& measurementStdDevs,
+      const wpi::array<double, States>& stateStdDevs,
+      const wpi::array<double, Outputs>& measurementStdDevs,
       std::function<Eigen::Matrix<double, States, 1>(
           const Eigen::Matrix<double, States, 2 * States + 1>&,
           const Eigen::Matrix<double, 2 * States + 1, 1>&)>