diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index b27723829d..f9ba581bbd 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -127,16 +127,6 @@ Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x, Vectord<5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)}; } -template -wpi::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( - const Vectord& stdDevs) { - wpi::array array; - for (size_t i = 0; i < Dim; ++i) { - array[i] = stdDevs(i); - } - return array; -} - Vectord<5> DifferentialDrivePoseEstimator::FillStateVector( const Pose2d& pose, units::meter_t leftDistance, units::meter_t rightDistance) { diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index afe6bed98f..3654c3edc3 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -233,10 +233,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - template - static wpi::array StdDevMatrixToArray( - const Vectord& stdDevs); - static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u); static Vectord<5> FillStateVector(const Pose2d& pose, units::meter_t leftDistance, diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index eb8219cfd1..24a79cfa1a 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -222,16 +222,6 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - - template - static wpi::array StdDevMatrixToArray( - const Vectord& vector) { - wpi::array array; - for (size_t i = 0; i < Dim; ++i) { - array[i] = vector(i); - } - return array; - } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index ce40e3208c..531ad94de9 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -304,16 +304,6 @@ class SwerveDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - - template - static wpi::array StdDevMatrixToArray( - const Vectord& vector) { - wpi::array array; - for (size_t i = 0; i < Dim; ++i) { - array[i] = vector(i); - } - return array; - } }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)