From 8bd8022794c1dbe74113f8b09207311335f5852c Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 14:53:06 -0400 Subject: [PATCH 01/23] Start kinda doing stuff --- .../DifferentialDrivePoseEstimator.cpp | 94 +++++++++++++------ .../DifferentialDriveStateEstimator.cpp | 38 +++++--- .../estimator/MecanumDrivePoseEstimator.cpp | 12 +-- .../DifferentialDrivePoseEstimator.h | 34 ++++--- .../DifferentialDriveStateEstimator.h | 43 +++++---- .../KalmanFilterLatencyCompensator.h | 3 +- .../frc/estimator/MecanumDrivePoseEstimator.h | 10 +- .../frc/estimator/SwerveDrivePoseEstimator.h | 16 ++-- .../DifferentialDriveStateEstimatorTest.cpp | 13 ++- .../frc/estimator/UnscentedKalmanFilter.h | 19 +++- 10 files changed, 177 insertions(+), 105 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 2b80b75d887..246d5da4a29 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -14,15 +14,14 @@ using namespace frc; DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - const Vector<5>& stateStdDevs, const Vector<3>& localMeasurementStdDevs, - const Vector<3>& visionMeasurmentStdDevs, units::second_t nominalDt) - : m_observer( + const Eigen::Matrix& stateStdDevs, const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurmentStdDevs, units::second_t nominalDt) + : m_stateStdDevs(stateStdDevs), m_localMeasurementStdDevs(localMeasurementStdDevs), m_observer( &DifferentialDrivePoseEstimator::F, - [](const Vector<5>& x, const Vector<3>& u) { - return frc::MakeMatrix<3, 1>(x(3, 0), x(4, 0), x(2, 0)); - }, - StdDevMatrixToArray<5>(stateStdDevs), - StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt), + &DifferentialDrivePoseEstimator::LocalMeasurementModel, + MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)), + MakeRDiagonals(localMeasurementStdDevs, FillStateVector(initialPose, 0_m, 0_m)), + nominalDt), m_nominalDt(nominalDt) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = @@ -30,10 +29,10 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Vector<3>& u, const Vector<3>& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { m_observer.Correct<3>( u, y, - [](const Vector<5>& x, const Vector<3>&) { + [](const Eigen::Matrix& x, const Eigen::Matrix&) { return x.block<3, 1>(0, 0); }, m_visionDiscR); @@ -82,42 +81,61 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( auto angle = gyroAngle + m_gyroOffset; auto omega = (gyroAngle - m_previousAngle).Radians() / dt; - auto u = frc::MakeMatrix<3, 1>( + Eigen::Matrix u = frc::MakeMatrix<3, 1>( (wheelSpeeds.left + wheelSpeeds.right).to() / 2.0, 0.0, omega.to()); m_previousAngle = angle; - auto localY = frc::MakeMatrix<3, 1>(leftDistance.to(), + Eigen::Matrix localY = frc::MakeMatrix<4, 1>(leftDistance.to(), rightDistance.to(), - angle.Radians().to()); + angle.Cos(), angle.Sin()); m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); - m_observer.Predict(u, dt); - m_observer.Correct(u, localY); + m_observer.Predict(u, frc::MakeCovMatrix<6>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), dt); + m_observer.Correct<4>(u, localY, &LocalMeasurementModel, frc::MakeCovMatrix<4>(MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); return GetEstimatedPosition(); } -Vector<5> DifferentialDrivePoseEstimator::F(const Vector<5>& x, - const Vector<3>& u) { +Eigen::Matrix DifferentialDrivePoseEstimator::F(const Eigen::Matrix& x, + const Eigen::Matrix& u) { // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does // that for us. - auto& theta = x(2, 0); - Eigen::Matrix toFieldRotation = frc::MakeMatrix<5, 5>( - // clang-format off - std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0, - std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0); // clang-format on - return toFieldRotation * - frc::MakeMatrix<5, 1>(u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)); + double cosTheta = x(2); + double sinTheta = x(3); + + // We want vx and vy to be in the field frame, so we apply a rotation matrix. + // to u_Field = u = [[vx_field, vy_field, omega]]^T + Eigen::Matrix chassisVel_local = MakeMatrix<2, 1>((u(0, 0) + u(1, 0)) / 2.0, 0.0); + Eigen::Matrix toFieldRotation = MakeMatrix<2, 2>( + cosTheta, -sinTheta, + sinTheta, cosTheta + ); + Eigen::Matrix chassisVel_field = toFieldRotation * chassisVel_local; + + // dcos(theta)/dt = -sin(theta) * dtheta/dt = -sin(theta) * omega + double dcosTheta = -sinTheta * u(2, 0); + // dsin(theta)/dt = cos(theta) * omega + double dsinTheta = cosTheta * u(2, 0); + + // As x = [[x_field, y_field, cos(theta), sin(theta), dist_l, dist_r]]^T, + // we need to return x-dot = [[vx_field, vy_field, d/dt cos(theta), d/dt sin(theta), vel_left, vel_right]]^T + // Assuming no wheel slip, vx = (v_left + v_right) / 2, and vy = 0; + + return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), + dcosTheta, dsinTheta, + u(0), u(1)); +} + +Eigen::Matrix DifferentialDrivePoseEstimator::LocalMeasurementModel( + const Eigen::Matrix& x, const Eigen::Matrix& u) { + return frc::MakeMatrix<4, 1>(x(4), x(5), x(2), x(3)); } template std::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( - const Vector& stdDevs) { + const Eigen::Matrix& stdDevs) { std::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = stdDevs(i); @@ -125,11 +143,25 @@ std::array DifferentialDrivePoseEstimator::StdDevMatrixToArray( return array; } -Vector<5> DifferentialDrivePoseEstimator::FillStateVector( +Eigen::Matrix DifferentialDrivePoseEstimator::FillStateVector( const Pose2d& pose, units::meter_t leftDistance, units::meter_t rightDistance) { - return frc::MakeMatrix<5, 1>( + return frc::MakeMatrix<6, 1>( pose.Translation().X().to(), pose.Translation().Y().to(), - pose.Rotation().Radians().to(), leftDistance.to(), + pose.Rotation().Cos(), pose.Rotation().Sin(), leftDistance.to(), rightDistance.to()); } + +static Eigen::Matrix MakeQDiagonals( + const Eigen::Matrix& stdDevs, + const Eigen::Matrix& x) { + return frc::MakeMatrix<6, 1>(stdDevs(0), stdDevs(1), stdDevs(2) * x(2), + stdDevs(2) * x(3), stdDevs(3), stdDevs(4)); +} + +static Eigen::Matrix MakeRDiagonals( + const Eigen::Matrix& stdDevs, + const Eigen::Matrix& x) { + return frc::MakeMatrix<4, 1>(stdDevs(0), stdDevs(1), stdDevs(2) * x(2), + stdDevs(2) * x(3)); +} diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp index 00c993b76ed..1d55aa1b694 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp @@ -13,8 +13,10 @@ using namespace frc; DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( - const LinearSystem<2, 2, 2>& plant, const Eigen::Matrix& initialState, - const Eigen::Matrix& stateStdDevs, const Eigen::Matrix& localMeasurementStdDevs, + const LinearSystem<2, 2, 2>& plant, + const Eigen::Matrix& initialState, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, const Eigen::Matrix& globalMeasurementStdDevs, const DifferentialDriveKinematics& kinematics, units::second_t nominalDt) : m_plant(plant), @@ -35,7 +37,8 @@ DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( frc::DiscretizeR<3>(globalContR, m_nominalDt); // Create correction mechanism for global measurements. - m_globalCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { + m_globalCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { m_observer.Correct<3>( u, y, &DifferentialDriveStateEstimator::GlobalMeasurementModel, globalDiscR); @@ -58,21 +61,22 @@ Vector<10> DifferentialDriveStateEstimator::GetEstimatedState() const { frc::Pose2d DifferentialDriveStateEstimator::GetEstimatedPosition() { Vector<10> xHat = GetEstimatedState(); return frc::Pose2d( - units::meter_t(xHat(State::kX, 0)), - units::meter_t(xHat(State::kY, 0)), - frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0)))); + units::meter_t(xHat(State::kX, 0)), units::meter_t(xHat(State::kY, 0)), + frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0)))); } Vector<10> DifferentialDriveStateEstimator::Update( units::radian_t heading, units::meter_t leftPosition, - units::meter_t rightPosition, const Eigen::Matrix& controlInput) { + units::meter_t rightPosition, + const Eigen::Matrix& controlInput) { return UpdateWithTime(heading, leftPosition, rightPosition, controlInput, frc2::Timer::GetFPGATimestamp()); } Vector<10> DifferentialDriveStateEstimator::UpdateWithTime( units::radian_t heading, units::meter_t leftPosition, - units::meter_t rightPosition, const Eigen::Matrix& controlInput, + units::meter_t rightPosition, + const Eigen::Matrix& controlInput, units::second_t currentTime) { auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt; m_prevTime = currentTime; @@ -90,7 +94,8 @@ Vector<10> DifferentialDriveStateEstimator::UpdateWithTime( return GetEstimatedState(); } -void DifferentialDriveStateEstimator::Reset(const Eigen::Matrix& initialState) { +void DifferentialDriveStateEstimator::Reset( + const Eigen::Matrix& initialState) { m_observer.Reset(); m_observer.SetXhat(initialState); @@ -98,8 +103,9 @@ void DifferentialDriveStateEstimator::Reset(const Eigen::Matrix& void DifferentialDriveStateEstimator::Reset() { m_observer.Reset(); } -Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix& x, - const Eigen::Matrix& u) { +Vector<10> DifferentialDriveStateEstimator::Dynamics( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { Eigen::Matrix B; B.block<2, 2>(0, 0) = m_plant.B(); B.block<2, 2>(2, 0).setZero(); @@ -124,8 +130,9 @@ Vector<10> DifferentialDriveStateEstimator::Dynamics(const Eigen::Matrix DifferentialDriveStateEstimator::LocalMeasurementModel( - const Eigen::Matrix& x, const Eigen::Matrix& u) { +Eigen::Matrix DifferentialDriveStateEstimator::LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { static_cast(u); Eigen::Matrix y; @@ -134,8 +141,9 @@ Vector<3> DifferentialDriveStateEstimator::LocalMeasurementModel( return y; } -Vector<3> DifferentialDriveStateEstimator::GlobalMeasurementModel( - const Eigen::Matrix& x, const Eigen::Matrix& u) { +Eigen::Matrix DifferentialDriveStateEstimator::GlobalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { static_cast(u); Eigen::Matrix y; diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 4bb97fc84f6..201867599aa 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -15,12 +15,12 @@ using namespace frc; frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - MecanumDriveKinematics kinematics, const Vector<3>& stateStdDevs, + MecanumDriveKinematics kinematics, const Eigen::Matrix& stateStdDevs, const Vector<1>& localMeasurementStdDevs, - const Vector<3>& visionMeasurementStdDevs, units::second_t nominalDt) + const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt) : m_observer( &MecanumDrivePoseEstimator::F, - [](const Vector<4>& x, const Vector<3>& u) { + [](const Vector<4>& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -43,9 +43,9 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create vision correction mechanism. - m_visionCorrect = [&](const Vector<3>& u, const Vector<4>& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, const Vector<4>& y) { m_observer.Correct<4>( - u, y, [](const Vector<4>& x, const Vector<3>& u) { return x; }, + u, y, [](const Vector<4>& x, const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -117,6 +117,6 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( } Vector<4> frc::MecanumDrivePoseEstimator::F(const Vector<4>& x, - const Vector<3>& u) { + const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 1eb23ed2787..3e4d4aa9a26 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -12,7 +12,7 @@ #include #include -#include "frc/estimator/ExtendedKalmanFilter.h" +#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" @@ -20,9 +20,6 @@ namespace frc { -template -using Vector = Eigen::Matrix; - /** * This class wraps an Extended Kalman Filter to fuse latency-compensated * vision measurements with differential drive encoder measurements. It will @@ -70,9 +67,9 @@ class DifferentialDrivePoseEstimator { */ DifferentialDrivePoseEstimator(const Rotation2d& gyroAngle, const Pose2d& initialPose, - const Vector<5>& stateStdDevs, - const Vector<3>& localMeasurementStdDevs, - const Vector<3>& visionMeasurementStdDevs, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** @@ -148,10 +145,10 @@ class DifferentialDrivePoseEstimator { units::meter_t rightDistance); private: - ExtendedKalmanFilter<5, 3, 3> m_observer; - KalmanFilterLatencyCompensator<5, 3, 3, ExtendedKalmanFilter<5, 3, 3>> + UnscentedKalmanFilter<6, 3, 4> m_observer; + KalmanFilterLatencyCompensator<6, 3, 4, UnscentedKalmanFilter<6, 3, 4>> m_latencyCompensator; - std::function& u, const Vector<3>& y)> m_visionCorrect; + std::function& u, const Eigen::Matrix& y)> m_visionCorrect; Eigen::Matrix m_visionDiscR; @@ -161,14 +158,25 @@ class DifferentialDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; + Eigen::Matrix m_stateStdDevs; + Eigen::Matrix m_localMeasurementStdDevs; + template static std::array StdDevMatrixToArray( - const Vector& stdDevs); + const Eigen::Matrix& stdDevs); - static Vector<5> F(const Vector<5>& x, const Vector<3>& u); - static Vector<5> FillStateVector(const Pose2d& pose, + static Eigen::Matrix LocalMeasurementModel(const Eigen::Matrix& x, const Eigen::Matrix& u); + static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u); + static Eigen::Matrix FillStateVector(const Pose2d& pose, units::meter_t leftDistance, units::meter_t rightDistance); + + static std::array MakeQDiagonals( + const Eigen::Matrix& stdDevs, + const Eigen::Matrix& x); + static std::array MakeRDiagonals( + const Eigen::Matrix& stdDevs, + const Eigen::Matrix& x); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h index 05afe0e0661..b1d7853faf4 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h @@ -40,10 +40,10 @@ using Vector = Eigen::Matrix; * DifferentialDriveStateEstimator::Update should be called every * robot loop (if your robot loops are faster than the default then you should * use DifferentialDriveStateEstimator::DifferentialDriveStateEstimator(const - * LinearSystem<2, 2, 2>&, const Eigen::Matrix&, const Eigen::Matrix&, const - * Vector<3>&, const Eigen::Matrix&, const DifferentialDriveKinematics&, - * units::second_t) - * to change the nominal delta time.) + * LinearSystem<2, 2, 2>&, const Eigen::Matrix&, const + * Eigen::Matrix&, const Eigen::Matrix&, const Eigen::Matrix&, const DifferentialDriveKinematics&, units::second_t) to change the + * nominal delta time.) * * DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement can be * called as infrequently as you want. @@ -80,13 +80,14 @@ class DifferentialDriveStateEstimator { * @param nominalDtSeconds The time in seconds between each robot * loop. */ - DifferentialDriveStateEstimator(const LinearSystem<2, 2, 2>& plant, - const Eigen::Matrix& initialState, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& globalMeasurementStdDevs, - const DifferentialDriveKinematics& kinematics, - units::second_t nominalDt = 0.02_s); + DifferentialDriveStateEstimator( + const LinearSystem<2, 2, 2>& plant, + const Eigen::Matrix& initialState, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& globalMeasurementStdDevs, + const DifferentialDriveKinematics& kinematics, + units::second_t nominalDt = 0.02_s); /** * Applies a global measurement with a given timestamp. @@ -111,7 +112,8 @@ class DifferentialDriveStateEstimator { Vector<10> GetEstimatedState() const; /** - * Gets the state of the robot at the current time as estimated by the Extended Kalman Filter. + * Gets the state of the robot at the current time as estimated by the + * Extended Kalman Filter. * * @return The robot state estimate. */ @@ -168,12 +170,13 @@ class DifferentialDriveStateEstimator { */ void Reset(); - Vector<10> Dynamics(const Eigen::Matrix& x, const Eigen::Matrix& u); + Vector<10> Dynamics(const Eigen::Matrix& x, + const Eigen::Matrix& u); - static Vector<3> LocalMeasurementModel(const Eigen::Matrix& x, + static Eigen::Matrix LocalMeasurementModel(const Eigen::Matrix& x, const Eigen::Matrix& u); - static Vector<3> GlobalMeasurementModel(const Eigen::Matrix& x, + static Eigen::Matrix GlobalMeasurementModel(const Eigen::Matrix& x, const Eigen::Matrix& u); private: @@ -187,16 +190,18 @@ class DifferentialDriveStateEstimator { KalmanFilterLatencyCompensator<10, 2, 3, ExtendedKalmanFilter<10, 2, 3>> m_latencyCompensator; - std::function& u, const Eigen::Matrix& y)> m_globalCorrect; + std::function& u, + const Eigen::Matrix& y)> + m_globalCorrect; units::second_t m_prevTime = -1_s; - Vector<3> m_localY; - Vector<3> m_globalY; + Eigen::Matrix m_localY; + Eigen::Matrix m_globalY; template static std::array StdDevMatrixToArray( - const Vector& stdDevs); + const Eigen::Matrix& stdDevs); class State { public: diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index 1e79a8855ec..322188bb557 100644 --- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -53,7 +53,8 @@ class KalmanFilterLatencyCompensator { void ApplyPastMeasurement( KalmanFilterType* observer, units::second_t nominalDt, Eigen::Matrix y, - std::function& u, const Eigen::Matrix& y)> + std::function& u, + const Eigen::Matrix& y)> globalMeasurementCorrect, units::second_t timestamp) { if (m_pastObserverSnapshots.size() == 0) { diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 130f2ab4cc9..3fb81e4fcf6 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -73,9 +73,9 @@ class MecanumDrivePoseEstimator { MecanumDrivePoseEstimator(const Rotation2d& gyroAngle, const Pose2d& initialPose, MecanumDriveKinematics kinematics, - const Vector<3>& stateStdDevs, + const Eigen::Matrix& stateStdDevs, const Vector<1>& localMeasurementStdDevs, - const Vector<3>& visionMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** @@ -151,7 +151,7 @@ class MecanumDrivePoseEstimator { MecanumDriveKinematics m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Vector<4>& y)> m_visionCorrect; + std::function& u, const Vector<4>& y)> m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -161,11 +161,11 @@ class MecanumDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Vector<4> F(const Vector<4>& x, const Vector<3>& u); + static Vector<4> F(const Vector<4>& x, const Eigen::Matrix& u); template static std::array StdDevMatrixToArray( - const Vector& vector) { + const Eigen::Matrix& vector) { std::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 97a110e90b0..938922a1c90 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -77,13 +77,13 @@ class SwerveDrivePoseEstimator { SwerveDrivePoseEstimator(const Rotation2d& gyroAngle, const Pose2d& initialPose, SwerveDriveKinematics& kinematics, - const Vector<3>& stateStdDevs, + const Eigen::Matrix& stateStdDevs, const Vector<1>& localMeasurementStdDevs, - const Vector<3>& visionMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s) : m_observer( &SwerveDrivePoseEstimator::F, - [](const Vector<4>& x, const Vector<3>& u) { + [](const Vector<4>& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -106,9 +106,9 @@ class SwerveDrivePoseEstimator { m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Vector<3>& u, const Vector<4>& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, const Vector<4>& y) { m_observer.Correct<4>( - u, y, [](const Vector<4>& x, const Vector<3>& u) { return x; }, + u, y, [](const Vector<4>& x, const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -241,7 +241,7 @@ class SwerveDrivePoseEstimator { SwerveDriveKinematics& m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Vector<4>& y)> m_visionCorrect; + std::function& u, const Vector<4>& y)> m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -251,13 +251,13 @@ class SwerveDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Vector<4> F(const Vector<4>& x, const Vector<3>& u) { + static Vector<4> F(const Vector<4>& x, const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } template static std::array StdDevMatrixToArray( - const Vector& vector) { + const Eigen::Matrix& vector) { std::array array; for (size_t i = 0; i < Dim; ++i) { array[i] = vector(i); diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp index a7925c9af9d..7fe91f081f5 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp @@ -145,10 +145,10 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { } errorSum += error; - std::cout << groundTruthState.pose.Translation().X().to() - << "," << groundTruthState.pose.Translation().Y().to() - << "," << estimatedTranslation.X().to() << "," - << estimatedTranslation.Y().to() << "," << error << "\n"; + std::cout << groundTruthState.pose.Translation().X().to() << "," + << groundTruthState.pose.Translation().Y().to() << "," + << estimatedTranslation.X().to() << "," + << estimatedTranslation.Y().to() << "," << error << "\n"; t += dt; } @@ -157,5 +157,8 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { 0.2); EXPECT_LT(maxError, 0.4); - std::cout << "Sum over time: " << errorSum / (trajectory.TotalTime().to() / dt.to()) << " max error " << maxError << "\n"; + std::cout << "Sum over time: " + << errorSum / + (trajectory.TotalTime().to() / dt.to()) + << " max error " << maxError << "\n"; } diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 8c2c31f0e62..dabdbc0e2f6 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -111,13 +111,28 @@ class UnscentedKalmanFilter { m_sigmasF.setZero(); } + /** + * Project the model into the future with a new control input u. + * + *

This is useful for when the process noise covariance matrix is time variant. + * The the Q matrix provided in the constructor is used if it is not provided + * (the two-argument version of this function). + * + * @param u New control input from controller. + * @param q The continuous process noise covariance matrix. + * @param dtSeconds Timestep for prediction. + */ + void Predict(const Eigen::Matrix& u, units::second_t dt) { + Predict(u, m_contQ, dt); + } + /** * Project the model into the future with a new control input u. * * @param u New control input from controller. * @param dt Timestep for prediction. */ - void Predict(const Eigen::Matrix& u, units::second_t dt) { + void Predict(const Eigen::Matrix& u, const Eigen::Matrix q, units::second_t dt) { m_dt = dt; // Discretize Q before projecting mean and covariance forward @@ -125,7 +140,7 @@ class UnscentedKalmanFilter { NumericalJacobianX(m_f, m_xHat, u); Eigen::Matrix discA; Eigen::Matrix discQ; - DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); + DiscretizeAQTaylor(contA, q, dt, &discA, &discQ); Eigen::Matrix sigmas = m_pts.SigmaPoints(m_xHat, m_P); From 3cb52f3d4ef61143e725818da2d5c29b7e6d0c4a Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 15:35:33 -0400 Subject: [PATCH 02/23] aaah --- .../DifferentialDrivePoseEstimator.cpp | 24 ++++++++++--------- .../DifferentialDrivePoseEstimator.h | 2 -- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 246d5da4a29..1146730740e 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -15,7 +15,7 @@ using namespace frc; DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, const Eigen::Matrix& stateStdDevs, const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurmentStdDevs, units::second_t nominalDt) + const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt) : m_stateStdDevs(stateStdDevs), m_localMeasurementStdDevs(localMeasurementStdDevs), m_observer( &DifferentialDrivePoseEstimator::F, &DifferentialDrivePoseEstimator::LocalMeasurementModel, @@ -25,17 +25,19 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( m_nominalDt(nominalDt) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = - frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurmentStdDevs)); - m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurementStdDevs)); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { - m_observer.Correct<3>( + + auto& h = [](const Eigen::Matrix& x_, const Eigen::Matrix& u_) { + return x_.block<4, 1>(0, 0); + }; + + m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { + m_observer.Correct<4>( u, y, - [](const Eigen::Matrix& x, const Eigen::Matrix&) { - return x.block<3, 1>(0, 0); - }, - m_visionDiscR); + h, + DiscretizeR<4>(MakeCovMatrix<4>(MakeRDiagonals(visionMeasurementStdDevs, m_observer.Xhat())), m_nominalDt)); }; m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -58,8 +60,8 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { void DifferentialDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { - m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt, - PoseTo3dVector(visionRobotPose), + m_latencyCompensator.ApplyPastMeasurement<4>(&m_observer, m_nominalDt, + PoseTo4dVector(visionRobotPose), m_visionCorrect, timestamp); } diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 3e4d4aa9a26..b2b7a62e57c 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -150,8 +150,6 @@ class DifferentialDrivePoseEstimator { m_latencyCompensator; std::function& u, const Eigen::Matrix& y)> m_visionCorrect; - Eigen::Matrix m_visionDiscR; - units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; From 6ff202264fc9eb858f454c1f3c120775a8834b5e Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 15:55:27 -0400 Subject: [PATCH 03/23] Finally --- .../DifferentialDrivePoseEstimator.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 1146730740e..2ec77802847 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -28,16 +28,13 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurementStdDevs)); // Create correction mechanism for vision measurements. - - auto& h = [](const Eigen::Matrix& x_, const Eigen::Matrix& u_) { - return x_.block<4, 1>(0, 0); - }; - m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { m_observer.Correct<4>( u, y, - h, - DiscretizeR<4>(MakeCovMatrix<4>(MakeRDiagonals(visionMeasurementStdDevs, m_observer.Xhat())), m_nominalDt)); + [](const Eigen::Matrix& x_, const Eigen::Matrix& u_) { + return x_.block<4, 1>(0, 0); + }, + DiscretizeR<4>(MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals(visionMeasurementStdDevs, m_observer.Xhat())), m_nominalDt)); }; m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -154,16 +151,16 @@ Eigen::Matrix DifferentialDrivePoseEstimator::FillStateVector( rightDistance.to()); } -static Eigen::Matrix MakeQDiagonals( +std::array DifferentialDrivePoseEstimator::MakeQDiagonals( const Eigen::Matrix& stdDevs, const Eigen::Matrix& x) { - return frc::MakeMatrix<6, 1>(stdDevs(0), stdDevs(1), stdDevs(2) * x(2), - stdDevs(2) * x(3), stdDevs(3), stdDevs(4)); + return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), + stdDevs(2) * x(3), stdDevs(3), stdDevs(4)}; } -static Eigen::Matrix MakeRDiagonals( +std::array DifferentialDrivePoseEstimator::MakeRDiagonals( const Eigen::Matrix& stdDevs, const Eigen::Matrix& x) { - return frc::MakeMatrix<4, 1>(stdDevs(0), stdDevs(1), stdDevs(2) * x(2), - stdDevs(2) * x(3)); + return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), + stdDevs(2) * x(3)}; } From 037d5c49c38c25661aebb47bae62c5bc8231e2bf Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 15:57:55 -0400 Subject: [PATCH 04/23] Remove uses of Vector --- .../estimator/DifferentialDriveStateEstimator.cpp | 10 +++++----- .../cpp/estimator/MecanumDrivePoseEstimator.cpp | 10 +++++----- .../estimator/DifferentialDriveStateEstimator.h | 11 ++++------- .../frc/estimator/MecanumDrivePoseEstimator.h | 9 +++------ .../frc/estimator/SwerveDrivePoseEstimator.h | 15 ++++++--------- 5 files changed, 23 insertions(+), 32 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp index 1d55aa1b694..c4898fd622f 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp @@ -54,18 +54,18 @@ void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement( m_globalCorrect, timestamp); } -Vector<10> DifferentialDriveStateEstimator::GetEstimatedState() const { +Eigen::Matrix DifferentialDriveStateEstimator::GetEstimatedState() const { return m_observer.Xhat(); } frc::Pose2d DifferentialDriveStateEstimator::GetEstimatedPosition() { - Vector<10> xHat = GetEstimatedState(); + Eigen::Matrix xHat = GetEstimatedState(); return frc::Pose2d( units::meter_t(xHat(State::kX, 0)), units::meter_t(xHat(State::kY, 0)), frc::Rotation2d(units::radian_t(xHat(State::kHeading, 0)))); } -Vector<10> DifferentialDriveStateEstimator::Update( +Eigen::Matrix DifferentialDriveStateEstimator::Update( units::radian_t heading, units::meter_t leftPosition, units::meter_t rightPosition, const Eigen::Matrix& controlInput) { @@ -73,7 +73,7 @@ Vector<10> DifferentialDriveStateEstimator::Update( frc2::Timer::GetFPGATimestamp()); } -Vector<10> DifferentialDriveStateEstimator::UpdateWithTime( +Eigen::Matrix DifferentialDriveStateEstimator::UpdateWithTime( units::radian_t heading, units::meter_t leftPosition, units::meter_t rightPosition, const Eigen::Matrix& controlInput, @@ -103,7 +103,7 @@ void DifferentialDriveStateEstimator::Reset( void DifferentialDriveStateEstimator::Reset() { m_observer.Reset(); } -Vector<10> DifferentialDriveStateEstimator::Dynamics( +Eigen::Matrix DifferentialDriveStateEstimator::Dynamics( const Eigen::Matrix& x, const Eigen::Matrix& u) { Eigen::Matrix B; diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 201867599aa..f823cbb4bad 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -16,11 +16,11 @@ using namespace frc; frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, MecanumDriveKinematics kinematics, const Eigen::Matrix& stateStdDevs, - const Vector<1>& localMeasurementStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt) : m_observer( &MecanumDrivePoseEstimator::F, - [](const Vector<4>& x, const Eigen::Matrix& u) { + [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -43,9 +43,9 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create vision correction mechanism. - m_visionCorrect = [&](const Eigen::Matrix& u, const Vector<4>& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { m_observer.Correct<4>( - u, y, [](const Vector<4>& x, const Eigen::Matrix& u) { return x; }, + u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -116,7 +116,7 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( return GetEstimatedPosition(); } -Vector<4> frc::MecanumDrivePoseEstimator::F(const Vector<4>& x, +Eigen::Matrix frc::MecanumDrivePoseEstimator::F(const Eigen::Matrix& x, const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h index b1d7853faf4..bf2ec01e360 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h @@ -21,9 +21,6 @@ namespace frc { -template -using Vector = Eigen::Matrix; - /** * This class wraps an ExtendedKalmanFilter to fuse latency-compensated * global measurements(ex. vision) with differential drive encoder measurements. @@ -109,7 +106,7 @@ class DifferentialDriveStateEstimator { * * @return The robot state estimate. */ - Vector<10> GetEstimatedState() const; + Eigen::Matrix GetEstimatedState() const; /** * Gets the state of the robot at the current time as estimated by the @@ -133,7 +130,7 @@ class DifferentialDriveStateEstimator { * @param controlInput The control input. * @return The robot state estimate. */ - Vector<10> Update(units::radian_t heading, units::meter_t leftPosition, + Eigen::Matrix Update(units::radian_t heading, units::meter_t leftPosition, units::meter_t rightPosition, const Eigen::Matrix& controlInput); @@ -152,7 +149,7 @@ class DifferentialDriveStateEstimator { * @param currentTimeSeconds Time at which this method was called, in seconds. * @return The robot state estimate. */ - Vector<10> UpdateWithTime(units::radian_t heading, + Eigen::Matrix UpdateWithTime(units::radian_t heading, units::meter_t leftPosition, units::meter_t rightPosition, const Eigen::Matrix& controlInput, @@ -170,7 +167,7 @@ class DifferentialDriveStateEstimator { */ void Reset(); - Vector<10> Dynamics(const Eigen::Matrix& x, + Eigen::Matrix Dynamics(const Eigen::Matrix& x, const Eigen::Matrix& u); static Eigen::Matrix LocalMeasurementModel(const Eigen::Matrix& x, diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 3fb81e4fcf6..0b3d2747ac2 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -21,9 +21,6 @@ namespace frc { -template -using Vector = Eigen::Matrix; - /** * This class wraps an ExtendedKalmanFilter to fuse latency-compensated vision * measurements with mecanum drive encoder velocity measurements. It will @@ -74,7 +71,7 @@ class MecanumDrivePoseEstimator { const Pose2d& initialPose, MecanumDriveKinematics kinematics, const Eigen::Matrix& stateStdDevs, - const Vector<1>& localMeasurementStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); @@ -151,7 +148,7 @@ class MecanumDrivePoseEstimator { MecanumDriveKinematics m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Vector<4>& y)> m_visionCorrect; + std::function& u, const Eigen::Matrix& y)> m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -161,7 +158,7 @@ class MecanumDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Vector<4> F(const Vector<4>& x, const Eigen::Matrix& u); + static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u); template static std::array StdDevMatrixToArray( diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 938922a1c90..b72bf648dd7 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -24,9 +24,6 @@ namespace frc { -template -using Vector = Eigen::Matrix; - /** * This class wraps an ExtendedKalmanFilter to fuse latency-compensated vision * measurements with swerve drive encoder velocity measurements. It will correct @@ -78,12 +75,12 @@ class SwerveDrivePoseEstimator { const Pose2d& initialPose, SwerveDriveKinematics& kinematics, const Eigen::Matrix& stateStdDevs, - const Vector<1>& localMeasurementStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s) : m_observer( &SwerveDrivePoseEstimator::F, - [](const Vector<4>& x, const Eigen::Matrix& u) { + [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -106,9 +103,9 @@ class SwerveDrivePoseEstimator { m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Matrix& u, const Vector<4>& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { m_observer.Correct<4>( - u, y, [](const Vector<4>& x, const Eigen::Matrix& u) { return x; }, + u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -241,7 +238,7 @@ class SwerveDrivePoseEstimator { SwerveDriveKinematics& m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Vector<4>& y)> m_visionCorrect; + std::function& u, const Eigen::Matrix& y)> m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -251,7 +248,7 @@ class SwerveDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Vector<4> F(const Vector<4>& x, const Eigen::Matrix& u) { + static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } From 1fe89f954e82ea9ec2e7612af1a0543aa213aeb6 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 16:03:57 -0400 Subject: [PATCH 05/23] Update DifferentialDrivePoseEstimatorTest.cpp --- .../DifferentialDrivePoseEstimatorTest.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index a37eb0f3cf6..f2e3a006067 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -26,13 +26,17 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::DifferentialDrivePoseEstimator estimator{ frc::Rotation2d(), frc::Pose2d(), frc::MakeMatrix<5, 1>(0.01, 0.01, 0.01, 0.01, 0.01), - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1), + frc::MakeMatrix<3, 1>(0.5, 0.5, 0.5), frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d(), frc::Pose2d(20_m, 20_m, frc::Rotation2d()), - frc::Pose2d(54_m, 54_m, frc::Rotation2d())}, - frc::TrajectoryConfig(10_mps, 5.0_mps_sq)); + frc::Pose2d(10_m, 10_m, 180_deg), + frc::Pose2d(30_m, 30_m, 0_deg), + frc::Pose2d(20_m, 20_m, 180_deg), + frc::Pose2d(10_m, 10_m, 0_deg), + }, + frc::TrajectoryConfig(0.5_mps, 2.0_mps_sq)); frc::DifferentialDriveKinematics kinematics{1.0_m}; frc::DifferentialDriveOdometry odometry{frc::Rotation2d()}; @@ -68,8 +72,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { lastVisionPose = groundTruthState.pose + frc::Transform2d( - frc::Translation2d(distribution(generator) * 0.1 * 1_m, - distribution(generator) * 0.1 * 1_m), + frc::Translation2d(distribution(generator) * 0.5 * 1_m, + distribution(generator) * 0.5 * 1_m), frc::Rotation2d(distribution(generator) * 0.1 * 1_rad)); lastVisionUpdateRealTimestamp = frc2::Timer::GetFPGATimestamp(); @@ -103,8 +107,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { << std::endl; std::cout << "max error " << maxError << std::endl; - // EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().to() / - // dt.to()), - // 0.2); - // EXPECT_NEAR(0.0, maxError, 0.4); + EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().to() / + dt.to()), + 0.2); + EXPECT_NEAR(0.0, maxError, 0.4); } From 2f8e84982a6beeab808af744b5de75c2a4af4c4d Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 16:17:06 -0400 Subject: [PATCH 06/23] Fix infine loop bug in latency comp --- .../include/frc/estimator/KalmanFilterLatencyCompensator.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index 322188bb557..d4716581263 100644 --- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -69,8 +70,9 @@ class KalmanFilterLatencyCompensator { // This index starts at one because we use the previous state later on, and // we always want to have a "previous state". + int maxIdx = m_pastObserverSnapshots.size() - 1; int low = 1; - int high = m_pastObserverSnapshots.size() - 1; + int high = std::max(maxIdx, 1); while (low != high) { int mid = (low + high) / 2.0; From b13945f6241287102b91810832d775d14fe5fc28 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 16:18:10 -0400 Subject: [PATCH 07/23] Run wpiformat --- .../DifferentialDrivePoseEstimator.cpp | 87 +++++++++++-------- .../DifferentialDriveStateEstimator.cpp | 9 +- .../estimator/MecanumDrivePoseEstimator.cpp | 21 +++-- .../DifferentialDrivePoseEstimator.h | 31 ++++--- .../DifferentialDriveStateEstimator.h | 35 ++++---- .../KalmanFilterLatencyCompensator.h | 2 +- .../frc/estimator/MecanumDrivePoseEstimator.h | 21 +++-- .../frc/estimator/SwerveDrivePoseEstimator.h | 31 ++++--- .../DifferentialDrivePoseEstimatorTest.cpp | 22 ++--- 9 files changed, 153 insertions(+), 106 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 2ec77802847..dacf5f550c7 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -14,13 +14,18 @@ using namespace frc; DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - const Eigen::Matrix& stateStdDevs, const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt) - : m_stateStdDevs(stateStdDevs), m_localMeasurementStdDevs(localMeasurementStdDevs), m_observer( + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, + units::second_t nominalDt) + : m_stateStdDevs(stateStdDevs), + m_localMeasurementStdDevs(localMeasurementStdDevs), + m_observer( &DifferentialDrivePoseEstimator::F, &DifferentialDrivePoseEstimator::LocalMeasurementModel, MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)), - MakeRDiagonals(localMeasurementStdDevs, FillStateVector(initialPose, 0_m, 0_m)), + MakeRDiagonals(localMeasurementStdDevs, + FillStateVector(initialPose, 0_m, 0_m)), nominalDt), m_nominalDt(nominalDt) { // Create R (covariances) for vision measurements. @@ -28,13 +33,18 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurementStdDevs)); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { m_observer.Correct<4>( u, y, - [](const Eigen::Matrix& x_, const Eigen::Matrix& u_) { + [](const Eigen::Matrix& x_, + const Eigen::Matrix& u_) { return x_.block<4, 1>(0, 0); }, - DiscretizeR<4>(MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals(visionMeasurementStdDevs, m_observer.Xhat())), m_nominalDt)); + DiscretizeR<4>( + MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals( + visionMeasurementStdDevs, m_observer.Xhat())), + m_nominalDt)); }; m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -86,19 +96,25 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( m_previousAngle = angle; - Eigen::Matrix localY = frc::MakeMatrix<4, 1>(leftDistance.to(), - rightDistance.to(), - angle.Cos(), angle.Sin()); + Eigen::Matrix localY = frc::MakeMatrix<4, 1>( + leftDistance.to(), rightDistance.to(), angle.Cos(), + angle.Sin()); m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); - m_observer.Predict(u, frc::MakeCovMatrix<6>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), dt); - m_observer.Correct<4>(u, localY, &LocalMeasurementModel, frc::MakeCovMatrix<4>(MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); + m_observer.Predict( + u, + frc::MakeCovMatrix<6>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), + dt); + m_observer.Correct<4>(u, localY, &LocalMeasurementModel, + frc::MakeCovMatrix<4>(MakeRDiagonals( + m_localMeasurementStdDevs, m_observer.Xhat()))); return GetEstimatedPosition(); } -Eigen::Matrix DifferentialDrivePoseEstimator::F(const Eigen::Matrix& x, - const Eigen::Matrix& u) { +Eigen::Matrix DifferentialDrivePoseEstimator::F( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does // that for us. double cosTheta = x(2); @@ -106,29 +122,31 @@ Eigen::Matrix DifferentialDrivePoseEstimator::F(const Eigen::Matri // We want vx and vy to be in the field frame, so we apply a rotation matrix. // to u_Field = u = [[vx_field, vy_field, omega]]^T - Eigen::Matrix chassisVel_local = MakeMatrix<2, 1>((u(0, 0) + u(1, 0)) / 2.0, 0.0); - Eigen::Matrix toFieldRotation = MakeMatrix<2, 2>( - cosTheta, -sinTheta, - sinTheta, cosTheta - ); - Eigen::Matrix chassisVel_field = toFieldRotation * chassisVel_local; - - // dcos(theta)/dt = -sin(theta) * dtheta/dt = -sin(theta) * omega + Eigen::Matrix chassisVel_local = + MakeMatrix<2, 1>((u(0, 0) + u(1, 0)) / 2.0, 0.0); + Eigen::Matrix toFieldRotation = + MakeMatrix<2, 2>(cosTheta, -sinTheta, sinTheta, cosTheta); + Eigen::Matrix chassisVel_field = + toFieldRotation * chassisVel_local; + + // dcos(theta)/dt = -std::sin(theta) * dtheta/dt = -std::sin(theta) * omega double dcosTheta = -sinTheta * u(2, 0); - // dsin(theta)/dt = cos(theta) * omega + // dsin(theta)/dt = std::cos(theta) * omega double dsinTheta = cosTheta * u(2, 0); - // As x = [[x_field, y_field, cos(theta), sin(theta), dist_l, dist_r]]^T, - // we need to return x-dot = [[vx_field, vy_field, d/dt cos(theta), d/dt sin(theta), vel_left, vel_right]]^T - // Assuming no wheel slip, vx = (v_left + v_right) / 2, and vy = 0; + // As x = [[x_field, y_field, std::cos(theta), std::sin(theta), dist_l, + // dist_r]]^T, we need to return x-dot = [[vx_field, vy_field, d/dt + // cos(theta), d/dt sin(theta), vel_left, vel_right]]^T Assuming no wheel + // slip, vx = (v_left + v_right) / 2, and vy = 0; - return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), - dcosTheta, dsinTheta, - u(0), u(1)); + return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), dcosTheta, + dsinTheta, u(0), u(1)); } -Eigen::Matrix DifferentialDrivePoseEstimator::LocalMeasurementModel( - const Eigen::Matrix& x, const Eigen::Matrix& u) { +Eigen::Matrix +DifferentialDrivePoseEstimator::LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(x(4), x(5), x(2), x(3)); } @@ -154,13 +172,12 @@ Eigen::Matrix DifferentialDrivePoseEstimator::FillStateVector( std::array DifferentialDrivePoseEstimator::MakeQDiagonals( const Eigen::Matrix& stdDevs, const Eigen::Matrix& x) { - return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), - stdDevs(2) * x(3), stdDevs(3), stdDevs(4)}; + return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), + stdDevs(2) * x(3), stdDevs(3), stdDevs(4)}; } std::array DifferentialDrivePoseEstimator::MakeRDiagonals( const Eigen::Matrix& stdDevs, const Eigen::Matrix& x) { - return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), - stdDevs(2) * x(3)}; + return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), stdDevs(2) * x(3)}; } diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp index c4898fd622f..012a94c7674 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp @@ -54,7 +54,8 @@ void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement( m_globalCorrect, timestamp); } -Eigen::Matrix DifferentialDriveStateEstimator::GetEstimatedState() const { +Eigen::Matrix +DifferentialDriveStateEstimator::GetEstimatedState() const { return m_observer.Xhat(); } @@ -130,7 +131,8 @@ Eigen::Matrix DifferentialDriveStateEstimator::Dynamics( return result; } -Eigen::Matrix DifferentialDriveStateEstimator::LocalMeasurementModel( +Eigen::Matrix +DifferentialDriveStateEstimator::LocalMeasurementModel( const Eigen::Matrix& x, const Eigen::Matrix& u) { static_cast(u); @@ -141,7 +143,8 @@ Eigen::Matrix DifferentialDriveStateEstimator::LocalMeasurementMod return y; } -Eigen::Matrix DifferentialDriveStateEstimator::GlobalMeasurementModel( +Eigen::Matrix +DifferentialDriveStateEstimator::GlobalMeasurementModel( const Eigen::Matrix& x, const Eigen::Matrix& u) { static_cast(u); diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index f823cbb4bad..b9f1ccd6696 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -15,12 +15,15 @@ using namespace frc; frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - MecanumDriveKinematics kinematics, const Eigen::Matrix& stateStdDevs, + MecanumDriveKinematics kinematics, + const Eigen::Matrix& stateStdDevs, const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, units::second_t nominalDt) + const Eigen::Matrix& visionMeasurementStdDevs, + units::second_t nominalDt) : m_observer( &MecanumDrivePoseEstimator::F, - [](const Eigen::Matrix& x, const Eigen::Matrix& u) { + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -43,9 +46,12 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create vision correction mechanism. - m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { m_observer.Correct<4>( - u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, + u, y, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -116,7 +122,8 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( return GetEstimatedPosition(); } -Eigen::Matrix frc::MecanumDrivePoseEstimator::F(const Eigen::Matrix& x, - const Eigen::Matrix& u) { +Eigen::Matrix frc::MecanumDrivePoseEstimator::F( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index b2b7a62e57c..83d1ba791cc 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -12,8 +12,8 @@ #include #include -#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/kinematics/DifferentialDriveWheelSpeeds.h" @@ -65,12 +65,12 @@ class DifferentialDrivePoseEstimator { * vision less. * @param nominalDt The period of the loop calling Update(). */ - DifferentialDrivePoseEstimator(const Rotation2d& gyroAngle, - const Pose2d& initialPose, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, - units::second_t nominalDt = 0.02_s); + DifferentialDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s); /** * Resets the robot's position on the field. @@ -148,7 +148,9 @@ class DifferentialDrivePoseEstimator { UnscentedKalmanFilter<6, 3, 4> m_observer; KalmanFilterLatencyCompensator<6, 3, 4, UnscentedKalmanFilter<6, 3, 4>> m_latencyCompensator; - std::function& u, const Eigen::Matrix& y)> m_visionCorrect; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; @@ -163,11 +165,14 @@ class DifferentialDrivePoseEstimator { static std::array StdDevMatrixToArray( const Eigen::Matrix& stdDevs); - static Eigen::Matrix LocalMeasurementModel(const Eigen::Matrix& x, const Eigen::Matrix& u); - static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u); - static Eigen::Matrix FillStateVector(const Pose2d& pose, - units::meter_t leftDistance, - units::meter_t rightDistance); + static Eigen::Matrix LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u); + static Eigen::Matrix F(const Eigen::Matrix& x, + const Eigen::Matrix& u); + static Eigen::Matrix FillStateVector( + const Pose2d& pose, units::meter_t leftDistance, + units::meter_t rightDistance); static std::array MakeQDiagonals( const Eigen::Matrix& stdDevs, diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h index bf2ec01e360..2b6e28e1e84 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h @@ -38,9 +38,9 @@ namespace frc { * robot loop (if your robot loops are faster than the default then you should * use DifferentialDriveStateEstimator::DifferentialDriveStateEstimator(const * LinearSystem<2, 2, 2>&, const Eigen::Matrix&, const - * Eigen::Matrix&, const Eigen::Matrix&, const Eigen::Matrix&, const DifferentialDriveKinematics&, units::second_t) to change the - * nominal delta time.) + * Eigen::Matrix&, const Eigen::Matrix&, const + * Eigen::Matrix&, const DifferentialDriveKinematics&, + * units::second_t) to change the nominal delta time.) * * DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement can be * called as infrequently as you want. @@ -130,9 +130,10 @@ class DifferentialDriveStateEstimator { * @param controlInput The control input. * @return The robot state estimate. */ - Eigen::Matrix Update(units::radian_t heading, units::meter_t leftPosition, - units::meter_t rightPosition, - const Eigen::Matrix& controlInput); + Eigen::Matrix Update( + units::radian_t heading, units::meter_t leftPosition, + units::meter_t rightPosition, + const Eigen::Matrix& controlInput); /** * Updates the the Extended Kalman Filter using wheel encoder information, @@ -149,11 +150,11 @@ class DifferentialDriveStateEstimator { * @param currentTimeSeconds Time at which this method was called, in seconds. * @return The robot state estimate. */ - Eigen::Matrix UpdateWithTime(units::radian_t heading, - units::meter_t leftPosition, - units::meter_t rightPosition, - const Eigen::Matrix& controlInput, - units::second_t currentTime); + Eigen::Matrix UpdateWithTime( + units::radian_t heading, units::meter_t leftPosition, + units::meter_t rightPosition, + const Eigen::Matrix& controlInput, + units::second_t currentTime); /** * Resets any internal state with a given initial state. @@ -168,13 +169,15 @@ class DifferentialDriveStateEstimator { void Reset(); Eigen::Matrix Dynamics(const Eigen::Matrix& x, - const Eigen::Matrix& u); + const Eigen::Matrix& u); - static Eigen::Matrix LocalMeasurementModel(const Eigen::Matrix& x, - const Eigen::Matrix& u); + static Eigen::Matrix LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u); - static Eigen::Matrix GlobalMeasurementModel(const Eigen::Matrix& x, - const Eigen::Matrix& u); + static Eigen::Matrix GlobalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u); private: LinearSystem<2, 2, 2> m_plant; diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index d4716581263..e5e12fb7507 100644 --- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -7,11 +7,11 @@ #pragma once +#include #include #include #include #include -#include #include #include diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 0b3d2747ac2..82d3b2b2a4e 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -67,13 +67,13 @@ class MecanumDrivePoseEstimator { * @param nominalDt The time in seconds between each robot * loop. */ - MecanumDrivePoseEstimator(const Rotation2d& gyroAngle, - const Pose2d& initialPose, - MecanumDriveKinematics kinematics, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, - units::second_t nominalDt = 0.02_s); + MecanumDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + MecanumDriveKinematics kinematics, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s); /** * Resets the robot's position on the field. @@ -148,7 +148,9 @@ class MecanumDrivePoseEstimator { MecanumDriveKinematics m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Eigen::Matrix& y)> m_visionCorrect; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -158,7 +160,8 @@ class MecanumDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u); + static Eigen::Matrix F(const Eigen::Matrix& x, + const Eigen::Matrix& u); template static std::array StdDevMatrixToArray( diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index b72bf648dd7..11b2014a692 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -71,16 +71,17 @@ class SwerveDrivePoseEstimator { * @param nominalDt The time in seconds between each robot * loop. */ - SwerveDrivePoseEstimator(const Rotation2d& gyroAngle, - const Pose2d& initialPose, - SwerveDriveKinematics& kinematics, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, - units::second_t nominalDt = 0.02_s) + SwerveDrivePoseEstimator( + const Rotation2d& gyroAngle, const Pose2d& initialPose, + SwerveDriveKinematics& kinematics, + const Eigen::Matrix& stateStdDevs, + const Eigen::Matrix& localMeasurementStdDevs, + const Eigen::Matrix& visionMeasurementStdDevs, + units::second_t nominalDt = 0.02_s) : m_observer( &SwerveDrivePoseEstimator::F, - [](const Eigen::Matrix& x, const Eigen::Matrix& u) { + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( @@ -103,9 +104,12 @@ class SwerveDrivePoseEstimator { m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create correction mechanism for vision measurements. - m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { + m_visionCorrect = [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { m_observer.Correct<4>( - u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, + u, y, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x; }, m_visionDiscR); }; @@ -238,7 +242,9 @@ class SwerveDrivePoseEstimator { SwerveDriveKinematics& m_kinematics; KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> m_latencyCompensator; - std::function& u, const Eigen::Matrix& y)> m_visionCorrect; + std::function& u, + const Eigen::Matrix& y)> + m_visionCorrect; Eigen::Matrix4d m_visionDiscR; @@ -248,7 +254,8 @@ class SwerveDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u) { + static Eigen::Matrix F(const Eigen::Matrix& x, + const Eigen::Matrix& u) { return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index f2e3a006067..e038334b526 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -30,12 +30,14 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( - std::vector{frc::Pose2d(), frc::Pose2d(20_m, 20_m, frc::Rotation2d()), - frc::Pose2d(10_m, 10_m, 180_deg), - frc::Pose2d(30_m, 30_m, 0_deg), - frc::Pose2d(20_m, 20_m, 180_deg), - frc::Pose2d(10_m, 10_m, 0_deg), - }, + std::vector{ + frc::Pose2d(), + frc::Pose2d(20_m, 20_m, frc::Rotation2d()), + frc::Pose2d(10_m, 10_m, 180_deg), + frc::Pose2d(30_m, 30_m, 0_deg), + frc::Pose2d(20_m, 20_m, 180_deg), + frc::Pose2d(10_m, 10_m, 0_deg), + }, frc::TrajectoryConfig(0.5_mps, 2.0_mps_sq)); frc::DifferentialDriveKinematics kinematics{1.0_m}; @@ -107,8 +109,8 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { << std::endl; std::cout << "max error " << maxError << std::endl; - EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().to() / - dt.to()), - 0.2); - EXPECT_NEAR(0.0, maxError, 0.4); + EXPECT_NEAR( + 0.0, errorSum / (trajectory.TotalTime().to() / dt.to()), + 0.2); + EXPECT_NEAR(0.0, maxError, 0.4); } From 4cd86f8da1ef8399b9553bb5bfae7bc1e255a4b6 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 22:36:44 -0400 Subject: [PATCH 08/23] Add KF latency test --- .../KalmanFilterLatencyCompensator.h | 1 + .../KalmanFilterLatencyCompensatorTest.cpp | 22 +++++++++++++++++++ 2 files changed, 23 insertions(+) create mode 100644 wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index e5e12fb7507..446a19a523d 100644 --- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -15,6 +15,7 @@ #include #include +#include namespace frc { diff --git a/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp new file mode 100644 index 00000000000..79c9ab8d1ea --- /dev/null +++ b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp @@ -0,0 +1,22 @@ +#include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/system/plant/LinearSystemId.h" +#include "frc/StateSpaceUtil.h" + +#include "gtest/gtest.h" + +TEST(KalmanFilterLatencyCompensatorTest, TestAccuracy) { + frc::KalmanFilterLatencyCompensator<1, 1, 1, frc::UnscentedKalmanFilter<1, 1, 1>> comp{}; + auto system = frc::LinearSystemId::IdentifyVelocitySystem(1.0, 1.0); + frc::UnscentedKalmanFilter<1, 1, 1> kf{ + [&](const Eigen::Matrix& x, const Eigen::Matrix& u) + {return system.A() * x + system.B() * u;}, + [&](const Eigen::Matrix& x, const Eigen::Matrix& u) + {return x;}, + {1}, {1}, 1_s}; + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(0.0), 0.0_s); + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(1.0), 1_s); + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(3.0), 2_s); + comp.ApplyPastMeasurement<1>(&kf, 1_s, frc::MakeMatrix<1, 1>(2.0), + [&](const Eigen::Matrix& u, const Eigen::Matrix& y){kf.Correct(u, y);}, 1.5_s); +} From 60dfa6a4a54b29e3ab9656448663a54901985150 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 18 Oct 2020 22:45:08 -0400 Subject: [PATCH 09/23] Cleanup cos/sin --- .../main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp | 3 +-- .../native/include/frc/estimator/SwerveDrivePoseEstimator.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index b9f1ccd6696..c0349bbe904 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -110,8 +110,7 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( omega.to()); auto localY = - frc::MakeMatrix<2, 1>(std::cos(angle.Radians().template to()), - std::sin(angle.Radians().template to())); + frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 11b2014a692..8ea9e271c6d 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -225,8 +225,7 @@ class SwerveDrivePoseEstimator { omega.template to()); auto localY = - frc::MakeMatrix<2, 1>(std::cos(angle.Radians().template to()), - std::sin(angle.Radians().template to())); + frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); From 3602d0c0e89d819543c937f869dcc9c40381c362 Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 19 Oct 2020 23:49:42 -0400 Subject: [PATCH 10/23] address reviewaaaah --- .../DifferentialDrivePoseEstimator.cpp | 16 +++++++++------- .../DifferentialDriveStateEstimator.cpp | 18 +++++++++--------- .../estimator/DifferentialDrivePoseEstimator.h | 10 +++++++--- .../DifferentialDriveStateEstimator.h | 10 +++++----- .../DifferentialDriveStateEstimatorTest.cpp | 10 +++++----- wpimath/src/main/native/cpp/StateSpaceUtil.cpp | 9 +++++++++ .../main/native/include/frc/StateSpaceUtil.h | 10 ++++++++++ 7 files changed, 54 insertions(+), 29 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index dacf5f550c7..c477d9c3732 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -14,23 +14,25 @@ using namespace frc; DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, units::second_t nominalDt) : m_stateStdDevs(stateStdDevs), m_localMeasurementStdDevs(localMeasurementStdDevs), m_observer( &DifferentialDrivePoseEstimator::F, &DifferentialDrivePoseEstimator::LocalMeasurementModel, - MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)), - MakeRDiagonals(localMeasurementStdDevs, + MakeQDiagonals(ArrayToVector<5>(stateStdDevs), FillStateVector(initialPose, 0_m, 0_m)), + MakeRDiagonals(ArrayToVector<3>(localMeasurementStdDevs), FillStateVector(initialPose, 0_m, 0_m)), nominalDt), m_nominalDt(nominalDt) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = - frc::MakeCovMatrix(StdDevMatrixToArray<3>(visionMeasurementStdDevs)); + frc::MakeCovMatrix(visionMeasurementStdDevs); + + const auto& stuff = ArrayToVector<3>({0.0, 0.0, 0.0}); // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, @@ -43,7 +45,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( }, DiscretizeR<4>( MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals( - visionMeasurementStdDevs, m_observer.Xhat())), + ArrayToVector<3>(visionMeasurementStdDevs), m_observer.Xhat())), m_nominalDt)); }; diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp index 012a94c7674..922f5c483c2 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp @@ -14,24 +14,24 @@ using namespace frc; DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( const LinearSystem<2, 2, 2>& plant, - const Eigen::Matrix& initialState, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& globalMeasurementStdDevs, + const std::array& initialState, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& globalMeasurementStdDevs, const DifferentialDriveKinematics& kinematics, units::second_t nominalDt) : m_plant(plant), m_rb(kinematics.trackWidth / 2.0), m_observer([this](auto& x, auto& u) { return Dynamics(x, u); }, &DifferentialDriveStateEstimator::LocalMeasurementModel, - StdDevMatrixToArray<10>(stateStdDevs), - StdDevMatrixToArray<3>(localMeasurementStdDevs), nominalDt), + stateStdDevs, + localMeasurementStdDevs, nominalDt), m_nominalDt(nominalDt) { m_localY.setZero(); m_globalY.setZero(); // Create R (covariances) for global measurements. Eigen::Matrix globalContR = - frc::MakeCovMatrix(StdDevMatrixToArray<3>(globalMeasurementStdDevs)); + frc::MakeCovMatrix(globalMeasurementStdDevs); Eigen::Matrix globalDiscR = frc::DiscretizeR<3>(globalContR, m_nominalDt); @@ -43,8 +43,8 @@ DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( u, y, &DifferentialDriveStateEstimator::GlobalMeasurementModel, globalDiscR); }; - - Reset(initialState); + + Reset(ArrayToVector<10>(initialState)); } void DifferentialDriveStateEstimator::ApplyPastGlobalMeasurement( diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 83d1ba791cc..2a01de14bdc 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -67,9 +67,9 @@ class DifferentialDrivePoseEstimator { */ DifferentialDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** @@ -164,6 +164,10 @@ class DifferentialDrivePoseEstimator { template static std::array StdDevMatrixToArray( const Eigen::Matrix& stdDevs); + + template + static Eigen::Matrix ArrayToVector( + const std::array& array); static Eigen::Matrix LocalMeasurementModel( const Eigen::Matrix& x, diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h index 2b6e28e1e84..351a535535e 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h @@ -47,7 +47,7 @@ namespace frc { * * Our state-space system is: * - * x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, + * x = [[x, y, cos(theta), sin(theta), vel_l, vel_r, dist_l, dist_r, voltError_l, * voltError_r, angularVelError]]^T in the field coordinate system * (dist_* are wheel distances.) * @@ -79,10 +79,10 @@ class DifferentialDriveStateEstimator { */ DifferentialDriveStateEstimator( const LinearSystem<2, 2, 2>& plant, - const Eigen::Matrix& initialState, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& globalMeasurementStdDevs, + const std::array& initialState, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& globalMeasurementStdDevs, const DifferentialDriveKinematics& kinematics, units::second_t nominalDt = 0.02_s); diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp index 7fe91f081f5..3cd28d64b5a 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp @@ -47,11 +47,11 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { frc::DifferentialDriveStateEstimator estimator{ plant, - Eigen::Matrix::Zero(), - frc::MakeMatrix<10, 1>(0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, - 10.0, 2.0), - frc::MakeMatrix<3, 1>(0.0001, 0.005, 0.005), - frc::MakeMatrix<3, 1>(0.5, 0.5, 0.5), + {0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, + 10.0, 2.0}, + {0.0001, 0.005, 0.005}, + {0.5, 0.5, 0.5}, kinematics, dt}; diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index c1037512dfd..28b639cfc2e 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -21,6 +21,15 @@ Eigen::Matrix PoseTo4dVector(const Pose2d& pose) { pose.Rotation().Cos(), pose.Rotation().Sin()); } +template +Eigen::Matrix ArrayToVector(const std::array& array_) { + Eigen::Matrix result; + for(size_t i = 0; i < Dim; ++i) { + result(i) = array_[i]; + } + return result; +} + template <> bool IsStabilizable<1, 1>(const Eigen::Matrix& A, const Eigen::Matrix& B) { diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 1f333eaba59..8d586b8edcb 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -287,6 +287,16 @@ bool IsStabilizable<2, 1>(const Eigen::Matrix& A, */ Eigen::Matrix PoseToVector(const Pose2d& pose); +/** + * Converts a vector into a Eigen::Matrix of size (Dim, 1). + * + * @param array The array to fill the vector with. + * + * @return The vector. + */ +template +Eigen::Matrix ArrayToVector(const std::array& array_); + /** * Clamps input vector between system's minimum and maximum allowable input. * From 41d3083128cb69fc9a17dc0ccd7b69cfb5f6071d Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 20 Oct 2020 00:53:02 -0400 Subject: [PATCH 11/23] whoop --- .../DifferentialDrivePoseEstimator.cpp | 14 ++++++------- .../estimator/MecanumDrivePoseEstimator.cpp | 20 +++++++++---------- .../DifferentialDrivePoseEstimator.h | 4 ---- .../frc/estimator/MecanumDrivePoseEstimator.h | 6 +++--- .../DifferentialDrivePoseEstimatorTest.cpp | 13 ++++++------ .../DifferentialDriveStateEstimatorTest.cpp | 8 ++++---- .../MecanumDrivePoseEstimatorTest.cpp | 6 +++--- .../src/main/native/cpp/StateSpaceUtil.cpp | 9 --------- .../main/native/include/frc/StateSpaceUtil.h | 8 +++++++- 9 files changed, 40 insertions(+), 48 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index c477d9c3732..dbb2a093828 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -7,7 +7,7 @@ #include "frc/estimator/DifferentialDrivePoseEstimator.h" -#include "frc/StateSpaceUtil.h" +#include #include "frc2/Timer.h" using namespace frc; @@ -18,21 +18,19 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const std::array& localMeasurementStdDevs, const std::array& visionMeasurementStdDevs, units::second_t nominalDt) - : m_stateStdDevs(stateStdDevs), - m_localMeasurementStdDevs(localMeasurementStdDevs), + : m_stateStdDevs(frc::ArrayToVector<5>(stateStdDevs)), + m_localMeasurementStdDevs(frc::ArrayToVector<3>(localMeasurementStdDevs)), m_observer( &DifferentialDrivePoseEstimator::F, &DifferentialDrivePoseEstimator::LocalMeasurementModel, - MakeQDiagonals(ArrayToVector<5>(stateStdDevs), FillStateVector(initialPose, 0_m, 0_m)), - MakeRDiagonals(ArrayToVector<3>(localMeasurementStdDevs), + MakeQDiagonals(frc::ArrayToVector<5>(stateStdDevs), FillStateVector(initialPose, 0_m, 0_m)), + MakeRDiagonals(frc::ArrayToVector<3>(localMeasurementStdDevs), FillStateVector(initialPose, 0_m, 0_m)), nominalDt), m_nominalDt(nominalDt) { // Create R (covariances) for vision measurements. Eigen::Matrix visionContR = - frc::MakeCovMatrix(visionMeasurementStdDevs); - - const auto& stuff = ArrayToVector<3>({0.0, 0.0, 0.0}); + frc::MakeCovMatrix<3>(visionMeasurementStdDevs); // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index c0349bbe904..44adfa38c58 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -16,9 +16,9 @@ using namespace frc; frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, MecanumDriveKinematics kinematics, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, units::second_t nominalDt) : m_observer( &MecanumDrivePoseEstimator::F, @@ -27,20 +27,20 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( return x.block<2, 1>(2, 0); }, StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - stateStdDevs(0), stateStdDevs(1), std::cos(stateStdDevs(2)), - std::sin(stateStdDevs(2)))), + stateStdDevs[0], stateStdDevs[1], std::cos(stateStdDevs[2]), + std::sin(stateStdDevs[2]))), StdDevMatrixToArray<2>( - frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs(0)), - std::sin(localMeasurementStdDevs(0)))), + frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs[0]), + std::sin(localMeasurementStdDevs[0]))), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { // Construct R (covariances) matrix for vision measurements. Eigen::Matrix4d visionContR = frc::MakeCovMatrix<4>(StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - visionMeasurementStdDevs(0), visionMeasurementStdDevs(1), - std::cos(visionMeasurementStdDevs(2)), - std::sin(visionMeasurementStdDevs(2))))); + visionMeasurementStdDevs[0], visionMeasurementStdDevs[1], + std::cos(visionMeasurementStdDevs[2]), + std::sin(visionMeasurementStdDevs[2])))); // Create and store discrete covariance matrix for vision measurements. m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 2a01de14bdc..01dd59b3949 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -165,10 +165,6 @@ class DifferentialDrivePoseEstimator { static std::array StdDevMatrixToArray( const Eigen::Matrix& stdDevs); - template - static Eigen::Matrix ArrayToVector( - const std::array& array); - static Eigen::Matrix LocalMeasurementModel( const Eigen::Matrix& x, const Eigen::Matrix& u); diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 82d3b2b2a4e..6f1443f5c63 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -70,9 +70,9 @@ class MecanumDrivePoseEstimator { MecanumDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, MecanumDriveKinematics kinematics, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s); /** diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index e038334b526..09661fe676b 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -12,6 +12,8 @@ #include #include +#include + #include "frc/StateSpaceUtil.h" #include "frc/estimator/DifferentialDrivePoseEstimator.h" #include "frc/geometry/Pose2d.h" @@ -25,9 +27,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::DifferentialDrivePoseEstimator estimator{ frc::Rotation2d(), frc::Pose2d(), - frc::MakeMatrix<5, 1>(0.01, 0.01, 0.01, 0.01, 0.01), - frc::MakeMatrix<3, 1>(0.5, 0.5, 0.5), - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)}; + {0.01, 0.01, 0.01, 0.01, 0.01}, + {0.5, 0.5, 0.5}, + {0.1, 0.1, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{ @@ -35,10 +37,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::Pose2d(20_m, 20_m, frc::Rotation2d()), frc::Pose2d(10_m, 10_m, 180_deg), frc::Pose2d(30_m, 30_m, 0_deg), - frc::Pose2d(20_m, 20_m, 180_deg), - frc::Pose2d(10_m, 10_m, 0_deg), + frc::Pose2d(20_m, 20_m, 180_deg) }, - frc::TrajectoryConfig(0.5_mps, 2.0_mps_sq)); + frc::TrajectoryConfig(2_mps, 3.0_mps_sq)); frc::DifferentialDriveKinematics kinematics{1.0_m}; frc::DifferentialDriveOdometry odometry{frc::Rotation2d()}; diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp index 3cd28d64b5a..ff94f491cd6 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp @@ -145,10 +145,10 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { } errorSum += error; - std::cout << groundTruthState.pose.Translation().X().to() << "," - << groundTruthState.pose.Translation().Y().to() << "," - << estimatedTranslation.X().to() << "," - << estimatedTranslation.Y().to() << "," << error << "\n"; + // std::cout << groundTruthState.pose.Translation().X().to() << "," + // << groundTruthState.pose.Translation().Y().to() << "," + // << estimatedTranslation.X().to() << "," + // << estimatedTranslation.Y().to() << "," << error << "\n"; t += dt; } diff --git a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 9ab32f501eb..5d736fd7b3d 100644 --- a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -24,9 +24,9 @@ TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) { frc::Rotation2d(), frc::Pose2d(), kinematics, - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1), - frc::MakeMatrix<1, 1>(0.05), - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)}; + {0.1, 0.1, 0.1}, + {0.05}, + {0.1, 0.1, 0.1}}; frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()}; diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 28b639cfc2e..c1037512dfd 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -21,15 +21,6 @@ Eigen::Matrix PoseTo4dVector(const Pose2d& pose) { pose.Rotation().Cos(), pose.Rotation().Sin()); } -template -Eigen::Matrix ArrayToVector(const std::array& array_) { - Eigen::Matrix result; - for(size_t i = 0; i < Dim; ++i) { - result(i) = array_[i]; - } - return result; -} - template <> bool IsStabilizable<1, 1>(const Eigen::Matrix& A, const Eigen::Matrix& B) { diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 8d586b8edcb..bbb9d44cd90 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -295,7 +295,13 @@ Eigen::Matrix PoseToVector(const Pose2d& pose); * @return The vector. */ template -Eigen::Matrix ArrayToVector(const std::array& array_); +Eigen::Matrix ArrayToVector(const std::array& array_) { + Eigen::Matrix result; + for(size_t i = 0; i < Dim; ++i) { + result(i) = array_[i]; + } + return result; +} /** * Clamps input vector between system's minimum and maximum allowable input. From 6de5ee655320351ddbd882826c6b878a88e16924 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 21 Oct 2020 17:12:16 -0400 Subject: [PATCH 12/23] Fix rotation2d bug in diff drive pose estimator --- .../native/cpp/estimator/DifferentialDrivePoseEstimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index dbb2a093828..b040d162535 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -62,7 +62,7 @@ void DifferentialDrivePoseEstimator::ResetPosition( Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { return Pose2d(units::meter_t(m_observer.Xhat(0)), units::meter_t(m_observer.Xhat(1)), - Rotation2d(units::radian_t(m_observer.Xhat(2)))); + Rotation2d(m_observer.Xhat(2), m_observer.Xhat(3))); } void DifferentialDrivePoseEstimator::AddVisionMeasurement( From a58bbae68a99ac2f4ba105daaea8a09f76d6cf43 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 21 Oct 2020 18:06:54 -0400 Subject: [PATCH 13/23] Update swerve to have time-varying Q and R --- .../frc/estimator/SwerveDrivePoseEstimator.h | 92 ++++++++++++------- 1 file changed, 58 insertions(+), 34 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 8ea9e271c6d..55f3876b9f8 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -15,7 +15,7 @@ #include #include "frc/StateSpaceUtil.h" -#include "frc/estimator/ExtendedKalmanFilter.h" +#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" @@ -25,7 +25,7 @@ namespace frc { /** - * This class wraps an ExtendedKalmanFilter to fuse latency-compensated vision + * This class wraps an UnscentedKalmanFilter to fuse latency-compensated vision * measurements with swerve drive encoder velocity measurements. It will correct * for noisy measurements and encoder drift. It is intended to be an easy but * more accurate drop-in for SwerveDriveOdometry. @@ -74,34 +74,24 @@ class SwerveDrivePoseEstimator { SwerveDrivePoseEstimator( const Rotation2d& gyroAngle, const Pose2d& initialPose, SwerveDriveKinematics& kinematics, - const Eigen::Matrix& stateStdDevs, - const Eigen::Matrix& localMeasurementStdDevs, - const Eigen::Matrix& visionMeasurementStdDevs, + const std::array& stateStdDevs, + const std::array& localMeasurementStdDevs, + const std::array& visionMeasurementStdDevs, units::second_t nominalDt = 0.02_s) - : m_observer( + : m_stateStdDevs(stateStdDevs), + m_localMeasurementStdDevs(localMeasurementStdDevs), + m_visionMeasurementStdDevs(visionMeasurementStdDevs), + m_observer( &SwerveDrivePoseEstimator::F, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, - StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - stateStdDevs(0), stateStdDevs(1), std::cos(stateStdDevs(2)), - std::sin(stateStdDevs(2)))), - StdDevMatrixToArray<2>( - frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs(0)), - std::sin(localMeasurementStdDevs(0)))), + MakeQDiagonals(stateStdDevs, frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), initialPose.Rotation().Sin())), + MakeRDiagonals(localMeasurementStdDevs, frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), initialPose.Rotation().Sin())), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { - // Construct R (covariances) matrix for vision measurements. - Eigen::Matrix4d visionContR = - frc::MakeCovMatrix<4>(StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - visionMeasurementStdDevs(0), visionMeasurementStdDevs(1), - std::cos(visionMeasurementStdDevs(2)), - std::sin(visionMeasurementStdDevs(2))))); - - // Create and store discrete covariance matrix for vision measurements. - m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, @@ -110,7 +100,7 @@ class SwerveDrivePoseEstimator { u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, - m_visionDiscR); + DiscretizeR<4>(MakeCovMatrix<4>(MakeVisionRDiagonals(visionMeasurementStdDevs, y)), nominalDt)); }; // Set initial state. @@ -142,7 +132,7 @@ class SwerveDrivePoseEstimator { } /** - * Gets the pose of the robot at the current time as estimated by the Extended + * Gets the pose of the robot at the current time as estimated by the Unscented * Kalman Filter. * * @return The estimated robot pose in meters. @@ -153,7 +143,7 @@ class SwerveDrivePoseEstimator { } /** - * Add a vision measurement to the Extended Kalman Filter. This will correct + * Add a vision measurement to the Unscented Kalman Filter. This will correct * the odometry pose estimate while still accounting for measurement noise. * * This method can be called as infrequently as you want, as long as you are @@ -178,7 +168,7 @@ class SwerveDrivePoseEstimator { } /** - * Updates the the Extended Kalman Filter using only wheel encoder + * Updates the the Unscented Kalman Filter using only wheel encoder * information. This should be called every loop, and the correct loop period * must be passed into the constructor of this class. * @@ -194,7 +184,7 @@ class SwerveDrivePoseEstimator { } /** - * Updates the the Extended Kalman Filter using only wheel encoder + * Updates the the Unscented Kalman Filter using only wheel encoder * information. This should be called every loop, and the correct loop period * must be passed into the constructor of this class. * @@ -224,37 +214,48 @@ class SwerveDrivePoseEstimator { fieldRelativeSpeeds.Y().template to(), omega.template to()); - auto localY = - frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); + auto localY = frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); - m_observer.Predict(u, dt); - m_observer.Correct(u, localY); + m_observer.Predict(u, frc::MakeCovMatrix<4>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), dt); + m_observer.Correct<2>(u, localY, [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return x.block<2, 1>(2, 0); + }, frc::MakeCovMatrix<2>(MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); return GetEstimatedPosition(); } private: - ExtendedKalmanFilter<4, 3, 2> m_observer; + UnscentedKalmanFilter<4, 3, 2> m_observer; SwerveDriveKinematics& m_kinematics; - KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> + KalmanFilterLatencyCompensator<4, 3, 2, UnscentedKalmanFilter<4, 3, 2>> m_latencyCompensator; std::function& u, const Eigen::Matrix& y)> m_visionCorrect; - Eigen::Matrix4d m_visionDiscR; - units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; Rotation2d m_gyroOffset; Rotation2d m_previousAngle; + /** + * Get x-dot given the current state and input. Recall that the state is [x, + * y, std::cos(theta), std::sin(theta)]^T In our case, x-dot will be [dx/dt, + * dy/dt, d/dt cos(theta), d/dt sin(theta)]. + * + * @param x The current state. + * @param u The current input. In our case, u = [vx, vy, d/dt theta]^T + */ static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u) { + // Need to return [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)] + // dx/dt and dy/dt are from u. + // d/dt cos(theta) = -std::sin(theta) * d/dt(theta) by the chain rule. return frc::MakeMatrix<4, 1>(u(0), u(1), -x(3) * u(2), x(2) * u(2)); } @@ -267,6 +268,29 @@ class SwerveDrivePoseEstimator { } return array; } + + std::array m_stateStdDevs; + std::array m_localMeasurementStdDevs; + std::array m_visionMeasurementStdDevs; + + static std::array MakeQDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& x) { + // Std dev in [x, y, std::cos(theta), std::sin(theta)] form. + return { stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3) }; + } + + static std::array MakeRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& x) { + return {stdDevs[0] * x(2), stdDevs[0] * x(3)}; + } + + static std::array MakeVisionRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& y) { + return {stdDevs[0], stdDevs[1], stdDevs[2] * y(0), stdDevs[2] * y(1)}; + } }; } // namespace frc From 157012f2037f6968c766e4eacefc063350ff40c9 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 21 Oct 2020 18:07:34 -0400 Subject: [PATCH 14/23] Run format --- .../DifferentialDrivePoseEstimator.cpp | 25 ++++----- .../DifferentialDriveStateEstimator.cpp | 5 +- .../estimator/MecanumDrivePoseEstimator.cpp | 3 +- .../DifferentialDrivePoseEstimator.h | 10 ++-- .../DifferentialDriveStateEstimator.h | 6 +-- .../KalmanFilterLatencyCompensator.h | 2 +- .../DifferentialDrivePoseEstimatorTest.cpp | 46 +++++++++++----- .../DifferentialDriveStateEstimatorTest.cpp | 6 +-- .../KalmanFilterLatencyCompensatorTest.cpp | 48 +++++++++++------ .../MecanumDrivePoseEstimatorTest.cpp | 8 +-- .../SwerveDrivePoseEstimatorTest.cpp | 8 +-- .../DifferentialDriveStateEstimator.java | 54 +++++++++---------- .../estimator/SwerveDrivePoseEstimator.java | 10 ++++ .../DifferentialDrivePoseEstimatorTest.java | 44 ++++++++++----- .../DifferentialDriveStateEstimatorTest.java | 8 +-- 15 files changed, 170 insertions(+), 113 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index b040d162535..75cea91f555 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -8,6 +8,7 @@ #include "frc/estimator/DifferentialDrivePoseEstimator.h" #include + #include "frc2/Timer.h" using namespace frc; @@ -18,13 +19,13 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( const std::array& localMeasurementStdDevs, const std::array& visionMeasurementStdDevs, units::second_t nominalDt) - : m_stateStdDevs(frc::ArrayToVector<5>(stateStdDevs)), - m_localMeasurementStdDevs(frc::ArrayToVector<3>(localMeasurementStdDevs)), + : m_stateStdDevs(stateStdDevs), + m_localMeasurementStdDevs(localMeasurementStdDevs), m_observer( &DifferentialDrivePoseEstimator::F, &DifferentialDrivePoseEstimator::LocalMeasurementModel, - MakeQDiagonals(frc::ArrayToVector<5>(stateStdDevs), FillStateVector(initialPose, 0_m, 0_m)), - MakeRDiagonals(frc::ArrayToVector<3>(localMeasurementStdDevs), + MakeQDiagonals(stateStdDevs, FillStateVector(initialPose, 0_m, 0_m)), + MakeRDiagonals(localMeasurementStdDevs, FillStateVector(initialPose, 0_m, 0_m)), nominalDt), m_nominalDt(nominalDt) { @@ -43,7 +44,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( }, DiscretizeR<4>( MakeCovMatrix<4>(DifferentialDrivePoseEstimator::MakeRDiagonals( - ArrayToVector<3>(visionMeasurementStdDevs), m_observer.Xhat())), + visionMeasurementStdDevs, m_observer.Xhat())), m_nominalDt)); }; @@ -136,8 +137,8 @@ Eigen::Matrix DifferentialDrivePoseEstimator::F( // As x = [[x_field, y_field, std::cos(theta), std::sin(theta), dist_l, // dist_r]]^T, we need to return x-dot = [[vx_field, vy_field, d/dt - // cos(theta), d/dt sin(theta), vel_left, vel_right]]^T Assuming no wheel - // slip, vx = (v_left + v_right) / 2, and vy = 0; + // std::cos(theta), d/dt sin(theta), vel_left, vel_right]]^T Assuming no + // wheel slip, vx = (v_left + v_right) / 2, and vy = 0; return MakeMatrix<6, 1>(chassisVel_field(0), chassisVel_field(1), dcosTheta, dsinTheta, u(0), u(1)); @@ -170,14 +171,14 @@ Eigen::Matrix DifferentialDrivePoseEstimator::FillStateVector( } std::array DifferentialDrivePoseEstimator::MakeQDiagonals( - const Eigen::Matrix& stdDevs, + const std::array& stdDevs, const Eigen::Matrix& x) { - return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), - stdDevs(2) * x(3), stdDevs(3), stdDevs(4)}; + return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), + stdDevs[2] * x(3), stdDevs[3], stdDevs[4]}; } std::array DifferentialDrivePoseEstimator::MakeRDiagonals( - const Eigen::Matrix& stdDevs, + const std::array& stdDevs, const Eigen::Matrix& x) { - return {stdDevs(0), stdDevs(1), stdDevs(2) * x(2), stdDevs(2) * x(3)}; + return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)}; } diff --git a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp index 922f5c483c2..3ebf22f826a 100644 --- a/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/DifferentialDriveStateEstimator.cpp @@ -23,8 +23,7 @@ DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( m_rb(kinematics.trackWidth / 2.0), m_observer([this](auto& x, auto& u) { return Dynamics(x, u); }, &DifferentialDriveStateEstimator::LocalMeasurementModel, - stateStdDevs, - localMeasurementStdDevs, nominalDt), + stateStdDevs, localMeasurementStdDevs, nominalDt), m_nominalDt(nominalDt) { m_localY.setZero(); m_globalY.setZero(); @@ -43,7 +42,7 @@ DifferentialDriveStateEstimator::DifferentialDriveStateEstimator( u, y, &DifferentialDriveStateEstimator::GlobalMeasurementModel, globalDiscR); }; - + Reset(ArrayToVector<10>(initialState)); } diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 44adfa38c58..5bbececd141 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -109,8 +109,7 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( fieldRelativeVelocities.Y().to(), omega.to()); - auto localY = - frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); + auto localY = frc::MakeMatrix<2, 1>(angle.Cos(), angle.Sin()); m_previousAngle = angle; m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 01dd59b3949..45ee838a4b1 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -158,13 +158,13 @@ class DifferentialDrivePoseEstimator { Rotation2d m_gyroOffset; Rotation2d m_previousAngle; - Eigen::Matrix m_stateStdDevs; - Eigen::Matrix m_localMeasurementStdDevs; + std::array m_stateStdDevs; + std::array m_localMeasurementStdDevs; template static std::array StdDevMatrixToArray( const Eigen::Matrix& stdDevs); - + static Eigen::Matrix LocalMeasurementModel( const Eigen::Matrix& x, const Eigen::Matrix& u); @@ -175,10 +175,10 @@ class DifferentialDrivePoseEstimator { units::meter_t rightDistance); static std::array MakeQDiagonals( - const Eigen::Matrix& stdDevs, + const std::array& stdDevs, const Eigen::Matrix& x); static std::array MakeRDiagonals( - const Eigen::Matrix& stdDevs, + const std::array& stdDevs, const Eigen::Matrix& x); }; diff --git a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h index 351a535535e..2253cc840b1 100644 --- a/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/DifferentialDriveStateEstimator.h @@ -47,9 +47,9 @@ namespace frc { * * Our state-space system is: * - * x = [[x, y, cos(theta), sin(theta), vel_l, vel_r, dist_l, dist_r, voltError_l, - * voltError_r, angularVelError]]^T in the field coordinate system - * (dist_* are wheel distances.) + * x = [[x, y, std::cos(theta), std::sin(theta), vel_l, vel_r, dist_l, + * dist_r, voltError_l, voltError_r, angularVelError]]^T 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 LTVDiffDriveController. diff --git a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h index 446a19a523d..3b5adbacf51 100644 --- a/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h +++ b/wpilibc/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h @@ -14,8 +14,8 @@ #include #include -#include #include +#include namespace frc { diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 09661fe676b..6e73243c5bf 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -5,6 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include #include #include @@ -12,8 +13,6 @@ #include #include -#include - #include "frc/StateSpaceUtil.h" #include "frc/estimator/DifferentialDrivePoseEstimator.h" #include "frc/geometry/Pose2d.h" @@ -24,20 +23,36 @@ #include "frc2/Timer.h" #include "gtest/gtest.h" +TEST(DifferentialDrivePoseEstimatorTest, TestStraightLine) { + frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(), + frc::Pose2d(), + {0.01, 0.01, 0.01, 0.01, 0.01}, + {0.5, 0.5, 0.5}, + {0.1, 0.1, 0.1}}; + + for (int i = 0; i < 100; i++) { + frc::Pose2d pose = estimator.UpdateWithTime( + i * 0.02_s, frc::Rotation2d(0_deg), {1_m / 1_s, 1_m / 1_s}, + 1_m * i * 0.02, 1_m * i * 0.02); + std::cout << pose.Translation().X().to() << ", " + << pose.Translation().Y().to() << ", " + << pose.Rotation().Degrees().to() << std::endl; + } +} + TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { - frc::DifferentialDrivePoseEstimator estimator{ - frc::Rotation2d(), frc::Pose2d(), - {0.01, 0.01, 0.01, 0.01, 0.01}, - {0.5, 0.5, 0.5}, - {0.1, 0.1, 0.1}}; + frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(), + frc::Pose2d(), + {0.01, 0.01, 0.01, 0.01, 0.01}, + {0.5, 0.5, 0.5}, + {0.1, 0.1, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{ - frc::Pose2d(), - frc::Pose2d(20_m, 20_m, frc::Rotation2d()), + frc::Pose2d(), frc::Pose2d(20_m, 20_m, frc::Rotation2d()), frc::Pose2d(10_m, 10_m, 180_deg), - frc::Pose2d(30_m, 30_m, 0_deg), - frc::Pose2d(20_m, 20_m, 180_deg) + // frc::Pose2d(30_m, 30_m, 0_deg), + // frc::Pose2d(20_m, 20_m, 180_deg) }, frc::TrajectoryConfig(2_mps, 3.0_mps_sq)); @@ -70,7 +85,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { if (lastVisionUpdateTime + kVisionUpdateRate < t) { if (lastVisionPose != frc::Pose2d()) { - estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + // estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } lastVisionPose = groundTruthState.pose + @@ -92,6 +107,13 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::Rotation2d(units::radian_t(distribution(generator) * 0.1)), input, leftDistance, rightDistance); + std::cout << groundTruthState.pose.Translation().X().to() << ", " + << groundTruthState.pose.Translation().Y().to() << ", " + << groundTruthState.pose.Rotation().Degrees().to() << ", " + << xhat.Translation().X().to() << ", " + << xhat.Translation().Y().to() << ", " + << xhat.Rotation().Degrees().to() << ", " << std::endl; + double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) .to(); diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp index ff94f491cd6..5e78c382519 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDriveStateEstimatorTest.cpp @@ -48,8 +48,7 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { frc::DifferentialDriveStateEstimator estimator{ plant, {0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, - 10.0, 2.0}, + {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, 10.0, 2.0}, {0.0001, 0.005, 0.005}, {0.5, 0.5, 0.5}, kinematics, @@ -148,7 +147,8 @@ TEST(DifferentialDriveStateEstimatorTest, TestAccuracy) { // std::cout << groundTruthState.pose.Translation().X().to() << "," // << groundTruthState.pose.Translation().Y().to() << "," // << estimatedTranslation.X().to() << "," - // << estimatedTranslation.Y().to() << "," << error << "\n"; + // << estimatedTranslation.Y().to() << "," << error << + // "\n"; t += dt; } diff --git a/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp index 79c9ab8d1ea..2c961491196 100644 --- a/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/KalmanFilterLatencyCompensatorTest.cpp @@ -1,22 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/StateSpaceUtil.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" #include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/system/plant/LinearSystemId.h" -#include "frc/StateSpaceUtil.h" - #include "gtest/gtest.h" TEST(KalmanFilterLatencyCompensatorTest, TestAccuracy) { - frc::KalmanFilterLatencyCompensator<1, 1, 1, frc::UnscentedKalmanFilter<1, 1, 1>> comp{}; - auto system = frc::LinearSystemId::IdentifyVelocitySystem(1.0, 1.0); - frc::UnscentedKalmanFilter<1, 1, 1> kf{ - [&](const Eigen::Matrix& x, const Eigen::Matrix& u) - {return system.A() * x + system.B() * u;}, - [&](const Eigen::Matrix& x, const Eigen::Matrix& u) - {return x;}, - {1}, {1}, 1_s}; - comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(0.0), 0.0_s); - comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(1.0), 1_s); - comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), frc::MakeMatrix<1, 1>(3.0), 2_s); - comp.ApplyPastMeasurement<1>(&kf, 1_s, frc::MakeMatrix<1, 1>(2.0), - [&](const Eigen::Matrix& u, const Eigen::Matrix& y){kf.Correct(u, y);}, 1.5_s); + frc::KalmanFilterLatencyCompensator<1, 1, 1, + frc::UnscentedKalmanFilter<1, 1, 1>> + comp{}; + auto system = frc::LinearSystemId::IdentifyVelocitySystem(1.0, 1.0); + frc::UnscentedKalmanFilter<1, 1, 1> kf{ + [&](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return system.A() * x + system.B() * u; + }, + [&](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x; }, + {1}, + {1}, + 1_s}; + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), + frc::MakeMatrix<1, 1>(0.0), 0.0_s); + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), + frc::MakeMatrix<1, 1>(1.0), 1_s); + comp.AddObserverState(kf, frc::MakeMatrix<1, 1>(1.0), + frc::MakeMatrix<1, 1>(3.0), 2_s); + comp.ApplyPastMeasurement<1>( + &kf, 1_s, frc::MakeMatrix<1, 1>(2.0), + [&](const Eigen::Matrix& u, + const Eigen::Matrix& y) { kf.Correct(u, y); }, + 1.5_s); } diff --git a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 5d736fd7b3d..670175875e0 100644 --- a/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -21,12 +21,8 @@ TEST(MecanumDrivePoseEstimatorTest, TestAccuracy) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::MecanumDrivePoseEstimator estimator{ - frc::Rotation2d(), - frc::Pose2d(), - kinematics, - {0.1, 0.1, 0.1}, - {0.05}, - {0.1, 0.1, 0.1}}; + frc::Rotation2d(), frc::Pose2d(), kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()}; diff --git a/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 5da90fe84d2..286a645d740 100644 --- a/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -21,12 +21,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestAccuracy) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::SwerveDrivePoseEstimator<4> estimator{ - frc::Rotation2d(), - frc::Pose2d(), - kinematics, - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1), - frc::MakeMatrix<1, 1>(0.05), - frc::MakeMatrix<3, 1>(0.1, 0.1, 0.1)}; + frc::Rotation2d(), frc::Pose2d(), kinematics, + {0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}}; frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java index f292862052f..fceda727e3b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java @@ -65,15 +65,15 @@ */ @SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "PMD.SingularField"}) public class DifferentialDriveStateEstimator { - private final UnscentedKalmanFilter m_observer; - private final KalmanFilterLatencyCompensator m_latencyCompensator; + private UnscentedKalmanFilter m_observer; + private KalmanFilterLatencyCompensator m_latencyCompensator; - private final BiConsumer, Matrix> m_globalCorrect; + private BiConsumer, Matrix> m_globalCorrect; - private final double m_rb; - private final LinearSystem m_plant; + private double m_rb; + private LinearSystem m_plant; - private final double m_nominalDt; // Seconds + private double m_nominalDt; // Seconds private double m_prevTimeSeconds = -1.0; @@ -96,7 +96,7 @@ public class DifferentialDriveStateEstimator { * differential drivetrain's kinematics. */ public DifferentialDriveStateEstimator(LinearSystem plant, - Matrix initialState, + Matrix initialState, Matrix stateStdDevs, Matrix localMeasurementStdDevs, Matrix globalMeasurementStdDevs, @@ -124,7 +124,7 @@ public DifferentialDriveStateEstimator(LinearSystem plant, */ public DifferentialDriveStateEstimator( LinearSystem plant, - Matrix initialState, + Matrix initialState, Matrix stateStdDevs, Matrix localMeasurementStdDevs, Matrix globalMeasurementStdDevs, @@ -142,21 +142,21 @@ public DifferentialDriveStateEstimator( var globalContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), globalMeasurementStdDevs); var globalDiscR = Discretization.discretizeR(globalContR, m_nominalDt); - m_observer = new UnscentedKalmanFilter<>( - Nat.N10(), Nat.N3(), - this::getDynamics, - this::getLocalMeasurementModel, - stateStdDevs, - localMeasurementStdDevs, - nominalDtSeconds - ); + // m_observer = new UnscentedKalmanFilter<>( + // Nat.N12(), Nat.N4(), + // this::getDynamics, + // this::getLocalMeasurementModel, + // stateStdDevs, + // localMeasurementStdDevs, + // nominalDtSeconds + // ); // Create correction mechanism for global measurements. - m_globalCorrect = (u, y) -> m_observer.correct( - Nat.N3(), u, y, - this::getGlobalMeasurementModel, - globalDiscR - ); + // m_globalCorrect = (u, y) -> m_observer.correct( + // Nat.N3(), u, y, + // this::getGlobalMeasurementModel, + // globalDiscR + // ); m_latencyCompensator = new KalmanFilterLatencyCompensator<>(); reset(initialState); @@ -239,7 +239,7 @@ public void applyPastGlobalMeasurement(Pose2d robotPoseMeters, * * @return The robot state estimate. */ - public Matrix getEstimatedState() { + public Matrix getEstimatedState() { return m_observer.getXhat(); } @@ -267,7 +267,7 @@ public Pose2d getEstimatedPosition() { * @param controlInput The control input from the last timestep. * @return The robot state estimate. */ - public Matrix update(double headingRadians, double leftPosition, + public Matrix update(double headingRadians, double leftPosition, double rightPosition, Matrix controlInput) { return updateWithTime(headingRadians, @@ -290,7 +290,7 @@ public Matrix update(double headingRadians, double leftPosition, * @return The robot state estimate. */ @SuppressWarnings("VariableDeclarationUsageDistance") - public Matrix updateWithTime(double headingRadians, double leftPosition, + public Matrix updateWithTime(double headingRadians, double leftPosition, double rightPosition, Matrix controlInput, double currentTimeSeconds) { @@ -301,10 +301,10 @@ public Matrix updateWithTime(double headingRadians, double leftPosition m_localY.set(LocalOutput.kLeftPosition.value, 0, leftPosition); m_localY.set(LocalOutput.kRightPosition.value, 0, rightPosition); - m_latencyCompensator.addObserverState(m_observer, controlInput, m_localY, currentTimeSeconds); + // m_latencyCompensator.addObserverState(m_observer, controlInput, m_localY, currentTimeSeconds); m_observer.predict(controlInput, dt); - m_observer.correct(controlInput, m_localY); + // m_observer.correct(controlInput, m_localY); return getEstimatedState(); } @@ -314,7 +314,7 @@ public Matrix updateWithTime(double headingRadians, double leftPosition * * @param initialState Initial state for state estimate. */ - public void reset(Matrix initialState) { + public void reset(Matrix initialState) { m_observer.reset(); m_observer.setXhat(initialState); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 55a77f67b05..2a305dfea4a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -141,8 +141,18 @@ public SwerveDrivePoseEstimator( m_observer.setXhat(StateSpaceUtil.poseTo4dVector(initialPoseMeters)); } + /** + * Get x-dot given the current state and input. Recall that the state is [x, y, cos(theta), sin(theta)]^T + * In our case, x-dot will be [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)]. + * + * @param x The current state. + * @param u The current input. In our case, u = [vx, vy, d/dt theta]^T + */ @SuppressWarnings({"ParameterName", "MethodName"}) private Matrix f(Matrix x, Matrix u) { + // Need to return [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)] + // dx/dt and dy/dt are from u. + // d/dt cos(theta) = -sin(theta) * d/dt(theta) by the chain rule. return VecBuilder.fill( u.get(0, 0), u.get(1, 0), -x.get(3, 0) * u.get(2, 0), x.get(2, 0) * u.get(2, 0) ); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java index 520f34a7fa8..549e72769af 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java @@ -38,12 +38,28 @@ import static org.junit.jupiter.api.Assertions.assertEquals; public class DifferentialDrivePoseEstimatorTest { + +@Test public void testStraight() { + DifferentialDrivePoseEstimator estimator = new DifferentialDrivePoseEstimator( + new Rotation2d(), new Pose2d(), + VecBuilder.fill(0.01, 0.01, 0.01, 0.01, 0.01), + VecBuilder.fill(0.5, 0.5, 0.1), + VecBuilder.fill(0.1, 0.1, 0.1)); + + for(double i = 0; i < 100; i++) { + var pose = estimator.updateWithTime(i * 0.02, new Rotation2d(0), new DifferentialDriveWheelSpeeds(1, + 1), 0.02 * i * 1, 0.02 * i * 1); + System.out.printf("%s, %s, %s%n", pose.getTranslation().getX(), pose.getTranslation().getY(), pose.getRotation().getDegrees()); + } +} + + @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.AvoidInstantiatingObjectsInLoops"}) @Test public void testAccuracy() { var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(), - new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), + new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.01, 0.01, 0.01, 0.01, 0.01), new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.5, 0.5, 0.1), new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01)); @@ -136,14 +152,14 @@ public void testAccuracy() { t += dt; } -// assertEquals( -// 0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.03, -// "Incorrect mean error" -// ); -// assertEquals( -// 0.0, maxError, 0.05, -// "Incorrect max error" -// ); + assertEquals( + 0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.4, + "Incorrect mean error" + ); + assertEquals( + 0.0, maxError, 1.0, + "Incorrect max error" + ); System.out.println("Mean error (meters): " + errorSum / (traj.getTotalTimeSeconds() / dt)); System.out.println("Max error (meters): " + maxError); @@ -156,11 +172,11 @@ public void testAccuracy() { chart.addSeries("Trajectory", trajXs, trajYs); chart.addSeries("xHat", observerXs, observerYs); - new SwingWrapper<>(chart).displayChart(); - try { - Thread.sleep(1000000000); - } catch (InterruptedException e) { - } +// new SwingWrapper<>(chart).displayChart(); +// try { +// Thread.sleep(1000000000); +// } catch (InterruptedException e) { +// } } @Test void test() { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java index ff7d69535b6..4c126841db6 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimatorTest.java @@ -59,14 +59,14 @@ public void testAccuracy() { var estimator = new DifferentialDriveStateEstimator( plant, - MatrixUtils.zeros(Nat.N10()), + MatrixUtils.zeros(Nat.N12()), new MatBuilder<>(Nat.N10(), Nat.N1()).fill( 0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, 10.0, 2.0), new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0001, 0.005, 0.005), new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.5, 0.5, 0.5), kinematics); - var feedforward = new ControlAffinePlantInversionFeedforward<>(Nat.N10(), Nat.N2(), + var feedforward = new ControlAffinePlantInversionFeedforward<>(Nat.N12(), Nat.N2(), estimator::getDynamics, dt); var config = new TrajectoryConfig(12 / 2.5, (12 / 0.642) - 17.5); @@ -124,10 +124,10 @@ public void testAccuracy() { ); var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - var x = new MatBuilder<>(Nat.N10(), Nat.N1()).fill( + var x = new MatBuilder<>(Nat.N12(), Nat.N1()).fill( groundTruthState.poseMeters.getTranslation().getX(), groundTruthState.poseMeters.getTranslation().getY(), - groundTruthState.poseMeters.getRotation().getRadians(), + groundTruthState.poseMeters.getRotation().getRadians(), // TODO fix wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond, 0.0, 0.0, 0.0, 0.0, 0.0); From 02ad673fa8235dfeb611df0ed2163c87497082eb Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 22 Oct 2020 17:10:35 -0400 Subject: [PATCH 15/23] Make mecanum pose q and r time-varying --- .../estimator/MecanumDrivePoseEstimator.cpp | 51 +++++++++++-------- .../frc/estimator/MecanumDrivePoseEstimator.h | 35 +++++++++---- .../frc/estimator/SwerveDrivePoseEstimator.h | 44 +++++++++++----- 3 files changed, 83 insertions(+), 47 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 5bbececd141..e482ae8ddd2 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -20,43 +20,42 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const std::array& localMeasurementStdDevs, const std::array& visionMeasurementStdDevs, units::second_t nominalDt) - : m_observer( + : m_stateStdDevs(stateStdDevs), + m_localMeasurementStdDevs(localMeasurementStdDevs), + m_visionMeasurementStdDevs(visionMeasurementStdDevs), + m_observer( &MecanumDrivePoseEstimator::F, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, - StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - stateStdDevs[0], stateStdDevs[1], std::cos(stateStdDevs[2]), - std::sin(stateStdDevs[2]))), - StdDevMatrixToArray<2>( - frc::MakeMatrix<2, 1>(std::cos(localMeasurementStdDevs[0]), - std::sin(localMeasurementStdDevs[0]))), + // X and Y here are zero because we only use the cosine and sin + // states of X in the diagonal. + MakeQDiagonals( + stateStdDevs, + frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), + initialPose.Rotation().Sin())), + MakeRDiagonals( + localMeasurementStdDevs, + frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), + initialPose.Rotation().Sin())), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { - // Construct R (covariances) matrix for vision measurements. - Eigen::Matrix4d visionContR = - frc::MakeCovMatrix<4>(StdDevMatrixToArray<4>(frc::MakeMatrix<4, 1>( - visionMeasurementStdDevs[0], visionMeasurementStdDevs[1], - std::cos(visionMeasurementStdDevs[2]), - std::sin(visionMeasurementStdDevs[2])))); - - // Create and store discrete covariance matrix for vision measurements. - m_visionDiscR = frc::DiscretizeR<4>(visionContR, m_nominalDt); - - // Create vision correction mechanism. + // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { m_observer.Correct<4>( u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, - m_visionDiscR); + DiscretizeR<4>( + MakeCovMatrix<4>(MakeVisionRDiagonals(visionMeasurementStdDevs, y)), + nominalDt)); }; // Set initial state. - m_observer.SetXhat(PoseTo4dVector(initialPose)); + ResetPosition(initialPose, gyroAngle); // Calculate offsets. m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -114,8 +113,16 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); - m_observer.Predict(u, dt); - m_observer.Correct(u, localY); + m_observer.Predict( + u, + frc::MakeCovMatrix<4>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), + dt); + m_observer.Correct<2>( + u, localY, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, + frc::MakeCovMatrix<2>( + MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); return GetEstimatedPosition(); } diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 6f1443f5c63..e99d73f80f5 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -12,8 +12,8 @@ #include #include -#include "frc/estimator/ExtendedKalmanFilter.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" @@ -144,9 +144,9 @@ class MecanumDrivePoseEstimator { const MecanumDriveWheelSpeeds& wheelSpeeds); private: - ExtendedKalmanFilter<4, 3, 2> m_observer; + UnscentedKalmanFilter<4, 3, 2> m_observer; MecanumDriveKinematics m_kinematics; - KalmanFilterLatencyCompensator<4, 3, 2, ExtendedKalmanFilter<4, 3, 2>> + KalmanFilterLatencyCompensator<4, 3, 2, UnscentedKalmanFilter<4, 3, 2>> m_latencyCompensator; std::function& u, const Eigen::Matrix& y)> @@ -163,14 +163,27 @@ class MecanumDrivePoseEstimator { static Eigen::Matrix F(const Eigen::Matrix& x, const Eigen::Matrix& u); - template - static std::array StdDevMatrixToArray( - const Eigen::Matrix& vector) { - std::array array; - for (size_t i = 0; i < Dim; ++i) { - array[i] = vector(i); - } - return array; + std::array m_stateStdDevs; + std::array m_localMeasurementStdDevs; + std::array m_visionMeasurementStdDevs; + + static std::array MakeQDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& x) { + // Std dev in [x, y, std::cos(theta), std::sin(theta)] form. + return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)}; + } + + static std::array MakeRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& x) { + return {stdDevs[0] * x(2), stdDevs[0] * x(3)}; + } + + static std::array MakeVisionRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& y) { + return {stdDevs[0], stdDevs[1], stdDevs[2] * y(0), stdDevs[2] * y(1)}; } }; diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 55f3876b9f8..b7d91619ffa 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -15,8 +15,8 @@ #include #include "frc/StateSpaceUtil.h" -#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/estimator/KalmanFilterLatencyCompensator.h" +#include "frc/estimator/UnscentedKalmanFilter.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/kinematics/SwerveDriveKinematics.h" @@ -87,12 +87,19 @@ class SwerveDrivePoseEstimator { const Eigen::Matrix& u) { return x.block<2, 1>(2, 0); }, - MakeQDiagonals(stateStdDevs, frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), initialPose.Rotation().Sin())), - MakeRDiagonals(localMeasurementStdDevs, frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), initialPose.Rotation().Sin())), + // X and Y here are zero because we only use the cosine and sin + // states of X in the diagonal. + MakeQDiagonals( + stateStdDevs, + frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), + initialPose.Rotation().Sin())), + MakeRDiagonals( + localMeasurementStdDevs, + frc::MakeMatrix<4, 1>(0.0, 0.0, initialPose.Rotation().Cos(), + initialPose.Rotation().Sin())), nominalDt), m_kinematics(kinematics), m_nominalDt(nominalDt) { - // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, const Eigen::Matrix& y) { @@ -100,11 +107,13 @@ class SwerveDrivePoseEstimator { u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, - DiscretizeR<4>(MakeCovMatrix<4>(MakeVisionRDiagonals(visionMeasurementStdDevs, y)), nominalDt)); + DiscretizeR<4>(MakeCovMatrix<4>( + MakeVisionRDiagonals(visionMeasurementStdDevs, y)), + nominalDt)); }; // Set initial state. - m_observer.SetXhat(PoseTo4dVector(initialPose)); + ResetPosition(initialPose, gyroAngle); // Calculate offsets. m_gyroOffset = initialPose.Rotation() - gyroAngle; @@ -132,8 +141,8 @@ class SwerveDrivePoseEstimator { } /** - * Gets the pose of the robot at the current time as estimated by the Unscented - * Kalman Filter. + * Gets the pose of the robot at the current time as estimated by the + * Unscented Kalman Filter. * * @return The estimated robot pose in meters. */ @@ -219,11 +228,18 @@ class SwerveDrivePoseEstimator { m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime); - m_observer.Predict(u, frc::MakeCovMatrix<4>(MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), dt); - m_observer.Correct<2>(u, localY, [](const Eigen::Matrix& x, - const Eigen::Matrix& u) { - return x.block<2, 1>(2, 0); - }, frc::MakeCovMatrix<2>(MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); + m_observer.Predict(u, + frc::MakeCovMatrix<4>( + MakeQDiagonals(m_stateStdDevs, m_observer.Xhat())), + dt); + m_observer.Correct<2>( + u, localY, + [](const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return x.block<2, 1>(2, 0); + }, + frc::MakeCovMatrix<2>( + MakeRDiagonals(m_localMeasurementStdDevs, m_observer.Xhat()))); return GetEstimatedPosition(); } @@ -277,7 +293,7 @@ class SwerveDrivePoseEstimator { const std::array& stdDevs, const Eigen::Matrix& x) { // Std dev in [x, y, std::cos(theta), std::sin(theta)] form. - return { stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3) }; + return {stdDevs[0], stdDevs[1], stdDevs[2] * x(2), stdDevs[2] * x(3)}; } static std::array MakeRDiagonals( From 527bc5445b7e41058ba726ee7bc5158b561df819 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 22 Oct 2020 17:53:20 -0400 Subject: [PATCH 16/23] Switch java pose estiamtor to time varying q/r --- .../estimator/MecanumDrivePoseEstimator.cpp | 12 +++++ .../frc/estimator/MecanumDrivePoseEstimator.h | 8 +-- .../frc/estimator/SwerveDrivePoseEstimator.h | 2 +- .../DifferentialDrivePoseEstimatorTest.cpp | 18 +++---- .../estimator/SwerveDrivePoseEstimator.java | 51 ++++++++++++------- 5 files changed, 57 insertions(+), 34 deletions(-) diff --git a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index e482ae8ddd2..5b39beaaf95 100644 --- a/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpilibc/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -127,6 +127,18 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( return GetEstimatedPosition(); } +std::array MecanumDrivePoseEstimator::MakeRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& x) { + return {stdDevs[0] * x(2), stdDevs[0] * x(3)}; +} + +std::array MecanumDrivePoseEstimator::MakeVisionRDiagonals( + const std::array& stdDevs, + const Eigen::Matrix& y) { + return {stdDevs[0], stdDevs[1], stdDevs[2] * y(2), stdDevs[2] * y(3)}; +} + Eigen::Matrix frc::MecanumDrivePoseEstimator::F( const Eigen::Matrix& x, const Eigen::Matrix& u) { diff --git a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index e99d73f80f5..e52f8ceae29 100644 --- a/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -176,15 +176,11 @@ class MecanumDrivePoseEstimator { static std::array MakeRDiagonals( const std::array& stdDevs, - const Eigen::Matrix& x) { - return {stdDevs[0] * x(2), stdDevs[0] * x(3)}; - } + const Eigen::Matrix& x); static std::array MakeVisionRDiagonals( const std::array& stdDevs, - const Eigen::Matrix& y) { - return {stdDevs[0], stdDevs[1], stdDevs[2] * y(0), stdDevs[2] * y(1)}; - } + const Eigen::Matrix& y); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index b7d91619ffa..99bbd2a89e9 100644 --- a/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpilibc/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -305,7 +305,7 @@ class SwerveDrivePoseEstimator { static std::array MakeVisionRDiagonals( const std::array& stdDevs, const Eigen::Matrix& y) { - return {stdDevs[0], stdDevs[1], stdDevs[2] * y(0), stdDevs[2] * y(1)}; + return {stdDevs[0], stdDevs[1], stdDevs[2] * y(2), stdDevs[2] * y(3)}; } }; diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 6e73243c5bf..0d2bf642696 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -34,9 +34,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestStraightLine) { frc::Pose2d pose = estimator.UpdateWithTime( i * 0.02_s, frc::Rotation2d(0_deg), {1_m / 1_s, 1_m / 1_s}, 1_m * i * 0.02, 1_m * i * 0.02); - std::cout << pose.Translation().X().to() << ", " - << pose.Translation().Y().to() << ", " - << pose.Rotation().Degrees().to() << std::endl; + // std::cout << pose.Translation().X().to() << ", " + // << pose.Translation().Y().to() << ", " + // << pose.Rotation().Degrees().to() << std::endl; } } @@ -107,12 +107,12 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { frc::Rotation2d(units::radian_t(distribution(generator) * 0.1)), input, leftDistance, rightDistance); - std::cout << groundTruthState.pose.Translation().X().to() << ", " - << groundTruthState.pose.Translation().Y().to() << ", " - << groundTruthState.pose.Rotation().Degrees().to() << ", " - << xhat.Translation().X().to() << ", " - << xhat.Translation().Y().to() << ", " - << xhat.Rotation().Degrees().to() << ", " << std::endl; + // std::cout << groundTruthState.pose.Translation().X().to() << ", " + // << groundTruthState.pose.Translation().Y().to() << ", " + // << groundTruthState.pose.Rotation().Degrees().to() << ", " + // << xhat.Translation().X().to() << ", " + // << xhat.Translation().Y().to() << ", " + // << xhat.Rotation().Degrees().to() << ", " << std::endl; double error = groundTruthState.pose.Translation() .Distance(xhat.Translation()) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 2a305dfea4a..67973d39284 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -26,7 +26,7 @@ import edu.wpi.first.wpiutil.math.numbers.N4; /** - * This class wraps an {@link UnscentedKalmanFilter ExtendedKalmanFilter} to fuse + * This class wraps an {@link UnscentedKalmanFilter UnscentedKalmanFilter} to fuse * latency-compensated vision measurements with swerve drive encoder velocity measurements. * It will correct for noisy measurements and encoder drift. It is intended to be an easy * but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry}. @@ -62,6 +62,10 @@ public class SwerveDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; + Matrix m_stateStdDevs; + Matrix m_localMeasurementStdDevs; + Matrix m_visionMeasurementStdDevs; + /** * Constructs a SwerveDrivePoseEstimator. * @@ -113,27 +117,24 @@ public SwerveDrivePoseEstimator( Nat.N4(), Nat.N2(), this::f, (x, u) -> x.block(Nat.N2(), Nat.N1(), 2, 0), - VecBuilder.fill(stateStdDevs.get(0, 0), stateStdDevs.get(1, 0), - Math.cos(stateStdDevs.get(2, 0)), Math.sin(stateStdDevs.get(2, 0))), - VecBuilder.fill(Math.cos(localMeasurementStdDevs.get(0, 0)), - Math.sin(localMeasurementStdDevs.get(0, 0))), + makeQDiagonals(stateStdDevs, VecBuilder.fill(0.0, 0.0, + initialPoseMeters.getRotation().getCos(), initialPoseMeters.getRotation().getSin())), + makeRDiagonals(localMeasurementStdDevs, VecBuilder.fill(0.0, 0.0, + initialPoseMeters.getRotation().getCos(), initialPoseMeters.getRotation().getSin())), m_nominalDt ); m_kinematics = kinematics; m_latencyCompensator = new KalmanFilterLatencyCompensator<>(); + m_stateStdDevs = stateStdDevs; + m_localMeasurementStdDevs = localMeasurementStdDevs; + m_visionMeasurementStdDevs = visionMeasurementStdDevs; - var fullVisionMeasurementStdDev = VecBuilder.fill( - visionMeasurementStdDevs.get(0, 0), visionMeasurementStdDevs.get(1, 0), - Math.cos(visionMeasurementStdDevs.get(2, 0)), - Math.sin(visionMeasurementStdDevs.get(2, 0))); - - var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), fullVisionMeasurementStdDev); - var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt); - + // Create correction mechanism for vision measurements. m_visionCorrect = (u, y) -> m_observer.correct( Nat.N4(), u, y, (x, u_) -> x, - visionDiscR + Discretization.discretizeR(StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), + makeVisionRDiagonals(visionMeasurementStdDevs, y)), nominalDtSeconds) ); m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle); @@ -176,7 +177,7 @@ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { } /** - * Gets the pose of the robot at the current time as estimated by the Extended Kalman Filter. + * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter. * * @return The estimated robot pose in meters. */ @@ -189,7 +190,7 @@ public Pose2d getEstimatedPosition() { } /** - * Add a vision measurement to the Extended Kalman Filter. This will correct the + * Add a vision measurement to the Unscented Kalman Filter. This will correct the * odometry pose estimate while still accounting for measurement noise. * *

This method can be called as infrequently as you want, as long as you are @@ -218,7 +219,7 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS } /** - * Updates the the Extended Kalman Filter using only wheel encoder information. + * Updates the the Unscented Kalman Filter using only wheel encoder information. * This should be called every loop, and the correct loop period must be passed * into the constructor of this class. * @@ -234,7 +235,7 @@ public Pose2d update( } /** - * Updates the the Extended Kalman Filter using only wheel encoder information. + * Updates the the Unscented Kalman Filter using only wheel encoder information. * This should be called every loop, and the correct loop period must be passed * into the constructor of this class. * @@ -273,4 +274,18 @@ public Pose2d updateWithTime( return getEstimatedPosition(); } + + private static Matrix makeQDiagonals(Matrix stdDevs, Matrix x) { + return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0), + stdDevs.get(2,0) * x.get(2,0), stdDevs.get(2, 0) * x.get(3, 0)); + } + + private static Matrix makeRDiagonals(Matrix stdDevs, Matrix x) { + return VecBuilder.fill(stdDevs.get(0,0) * x.get(2,0), stdDevs.get(0, 0) * x.get(3, 0)); + } + + private static Matrix makeVisionRDiagonals(Matrix stdDevs, Matrix y) { + return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0), + stdDevs.get(2,0) * y.get(2,0), stdDevs.get(2, 0) * y.get(3, 0)); + } } From b690d4fec2749877a4f1cf0d48a83b50acef6151 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 22 Oct 2020 20:11:55 -0400 Subject: [PATCH 17/23] timicbwdgtstiol --- .../estimator/SwerveDrivePoseEstimator.java | 12 ++++++++-- .../SwerveDrivePoseEstimatorTest.java | 23 +++++++++++-------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 67973d39284..2ce6e401658 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -269,8 +269,16 @@ public Pose2d updateWithTime( var localY = VecBuilder.fill(angle.getCos(), angle.getSin()); m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); - m_observer.predict(u, dt); - m_observer.correct(u, localY); + m_observer.predict( + u, + StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())), + dt); + m_observer.correct(Nat.N2(), + u, localY, + (x, u_) -> x.block(Nat.N2(), Nat.N1(), 2, 0), + StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), + makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat()))); + return getEstimatedPosition(); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java index 959f9ce53bf..b76b074496c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java @@ -21,6 +21,9 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpiutil.math.VecBuilder; +import org.knowm.xchart.SwingWrapper; +import org.knowm.xchart.XYChart; +import org.knowm.xchart.XYChartBuilder; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -153,16 +156,16 @@ public void testAccuracy() { t += dt; } - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, - "Incorrect mean error" - ); - assertEquals( - 0.0, maxError, 0.42, - "Incorrect max error" - ); +// assertEquals( +// 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, +// "Incorrect mean error" +// ); +// assertEquals( +// 0.0, maxError, 0.42, +// "Incorrect max error" +// ); - /* +// /* List charts = new ArrayList(); var chartBuilder = new XYChartBuilder(); @@ -209,6 +212,6 @@ public void testAccuracy() { Thread.sleep(1000000000); } catch (InterruptedException e) { } - */ +// */ } } From 2a592f7f858839638d19db21dbaee0d0ac57b4cc Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 23 Oct 2020 17:54:02 -0400 Subject: [PATCH 18/23] Start adding time varying q/r support to latency comp --- .../DifferentialDrivePoseEstimator.java | 12 +++--- .../DifferentialDriveStateEstimator.java | 3 +- .../KalmanFilterLatencyCompensator.java | 34 ++++++++++------- .../estimator/MecanumDrivePoseEstimator.java | 6 +-- .../estimator/SwerveDrivePoseEstimator.java | 37 ++++++++++--------- .../DifferentialDrivePoseEstimatorTest.java | 12 +++--- .../estimator/ExtendedKalmanFilter.java | 19 ++++++++-- .../wpilibj/estimator/KalmanTypeFilter.java | 14 +++---- .../estimator/UnscentedKalmanFilter.java | 2 +- 9 files changed, 80 insertions(+), 59 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java index 4f86857cc4a..c97b67e7f66 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java @@ -247,8 +247,7 @@ public Pose2d getEstimatedPosition() { */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { m_latencyCompensator.applyPastGlobalMeasurement( - Nat.N4(), - m_observer, m_nominalDt, + m_observer, m_nominalDt, StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters), m_visionCorrect, timestampSeconds @@ -315,11 +314,12 @@ public Pose2d updateWithTime( m_previousAngle = angle; var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getCos(), angle.getSin()); -// m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); + var q = StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())); + var r = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat())); + m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, this::localMeasurementModel, currentTimeSeconds); - m_observer.predict(u, StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())), dt); - - m_observer.correct(Nat.N4(), u, localY, this::localMeasurementModel, StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat()))); + m_observer.predict(u, q, dt); + m_observer.correct(Nat.N4(), u, localY, this::localMeasurementModel, r); return getEstimatedPosition(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java index fceda727e3b..3e0c85667a1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java @@ -226,8 +226,7 @@ public void applyPastGlobalMeasurement(Pose2d robotPoseMeters, m_globalY.set(GlobalOutput.kHeading.value, 0, robotPoseMeters.getRotation().getRadians()); m_latencyCompensator.applyPastGlobalMeasurement( - Nat.N3(), - m_observer, m_nominalDt, + m_observer, m_nominalDt, m_globalY, m_globalCorrect, timestampSeconds diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java index 491c3c56c55..804ec67fb33 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java @@ -11,9 +11,9 @@ import java.util.List; import java.util.Map; import java.util.function.BiConsumer; +import java.util.function.BiFunction; import edu.wpi.first.wpiutil.math.Matrix; -import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; @@ -32,15 +32,18 @@ public class KalmanFilterLatencyCompensator observer, Matrix u, Matrix localY, + Matrix q, Matrix r, + BiFunction, Matrix, Matrix> localMeasurementModel, double timestampSeconds ) { m_pastObserverSnapshots.add(Map.entry( - timestampSeconds, new ObserverSnapshot(observer, u, localY) + timestampSeconds, new ObserverSnapshot(observer, u, localY, q, r, localMeasurementModel) )); if (m_pastObserverSnapshots.size() > k_maxPastObserverStates) { @@ -52,7 +55,6 @@ timestampSeconds, new ObserverSnapshot(observer, u, localY) * Add past global measurements (such as from vision)to the estimator. * * @param The rows in the global measurement vector. - * @param rows The rows in the global measurement vector. * @param observer The observer to apply the past global measurement. * @param nominalDtSeconds The nominal timestep. * @param globalMeasurement The measurement. @@ -61,12 +63,11 @@ timestampSeconds, new ObserverSnapshot(observer, u, localY) */ @SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"}) public void applyPastGlobalMeasurement( - Nat rows, - KalmanTypeFilter observer, - double nominalDtSeconds, - Matrix globalMeasurement, - BiConsumer, Matrix> globalMeasurementCorrect, - double globalMeasurementTimestampSeconds + KalmanTypeFilter observer, + double nominalDtSeconds, + Matrix globalMeasurement, + BiConsumer, Matrix> globalMeasurementCorrect, + double globalMeasurementTimestampSeconds ) { if (m_pastObserverSnapshots.isEmpty()) { // State map was empty, which means that we got a past measurement right at startup. The only @@ -120,7 +121,7 @@ public void applyPastGlobalMeasurement( observer.setXhat(snapshot.xHat); } - observer.predict(snapshot.inputs, key - lastTimestamp); + observer.predict(snapshot.inputs, snapshot.q, key - lastTimestamp); observer.correct(snapshot.inputs, snapshot.localMeasurements); if (i == indexOfClosestEntry) { @@ -132,8 +133,8 @@ public void applyPastGlobalMeasurement( } lastTimestamp = key; - m_pastObserverSnapshots.set(i, Map.entry(key, - new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements))); + m_pastObserverSnapshots.set(i, Map.entry(key, new ObserverSnapshot(observer, snapshot.inputs, + snapshot.localMeasurements, snapshot.q, snapshot.r, snapshot.localMeasurementModel))); } } @@ -146,13 +147,20 @@ public class ObserverSnapshot { public final Matrix errorCovariances; public final Matrix inputs; public final Matrix localMeasurements; + public final Matrix q; + private final Matrix r; + private final BiFunction, Matrix, Matrix> localMeasurementModel; @SuppressWarnings("ParameterName") private ObserverSnapshot( - KalmanTypeFilter observer, Matrix u, Matrix localY + KalmanTypeFilter observer, Matrix u, Matrix localY, + Matrix q, Matrix r, BiFunction, Matrix, Matrix> localMeasurementModel ) { this.xHat = observer.getXhat(); this.errorCovariances = observer.getP(); + this.q = q; + this.r = r; + this.localMeasurementModel = localMeasurementModel; inputs = u; localMeasurements = localY; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java index f8798fa4284..5b5026ca5e7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -197,8 +197,7 @@ public Pose2d getEstimatedPosition() { */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { m_latencyCompensator.applyPastGlobalMeasurement( - Nat.N4(), - m_observer, m_nominalDt, + m_observer, m_nominalDt, StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters), m_visionCorrect, timestampSeconds @@ -250,7 +249,8 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, m_previousAngle = angle; var localY = VecBuilder.fill(angle.getCos(), angle.getSin()); - m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); + Matrix q = null; // TODO + m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, currentTimeSeconds); m_observer.predict(u, dt); m_observer.correct(u, localY); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 2ce6e401658..66d2ddcb3bd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.estimator; import java.util.function.BiConsumer; +import java.util.function.BiFunction; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -143,7 +144,8 @@ public SwerveDrivePoseEstimator( } /** - * Get x-dot given the current state and input. Recall that the state is [x, y, cos(theta), sin(theta)]^T + * Get x-dot given the current state and input. Recall that the state is [x, y, cos(theta), + * sin(theta)]^T. * In our case, x-dot will be [dx/dt, dy/dt, d/dt cos(theta), d/dt sin(theta)]. * * @param x The current state. @@ -210,8 +212,7 @@ public Pose2d getEstimatedPosition() { */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { m_latencyCompensator.applyPastGlobalMeasurement( - Nat.N4(), - m_observer, m_nominalDt, + m_observer, m_nominalDt, StateSpaceUtil.poseTo4dVector(visionRobotPoseMeters), m_visionCorrect, timestampSeconds @@ -268,32 +269,32 @@ public Pose2d updateWithTime( m_previousAngle = angle; var localY = VecBuilder.fill(angle.getCos(), angle.getSin()); - m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds); - m_observer.predict( - u, - StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())), - dt); - m_observer.correct(Nat.N2(), - u, localY, - (x, u_) -> x.block(Nat.N2(), Nat.N1(), 2, 0), - StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), - makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat()))); - - + var q = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), + makeQDiagonals(m_stateStdDevs, m_observer.getXhat())); + var r = StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), + makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat())); + BiFunction, Matrix, Matrix> model = + (x, u_) -> x.block(Nat.N2(), Nat.N1(), 2, 0); + m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, model, currentTimeSeconds); + m_observer.predict(u, q, dt); + m_observer.correct(Nat.N2(), u, localY, model, r); return getEstimatedPosition(); } + @SuppressWarnings("ParameterName") private static Matrix makeQDiagonals(Matrix stdDevs, Matrix x) { return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0), - stdDevs.get(2,0) * x.get(2,0), stdDevs.get(2, 0) * x.get(3, 0)); + stdDevs.get(2, 0) * x.get(2, 0), stdDevs.get(2, 0) * x.get(3, 0)); } + @SuppressWarnings("ParameterName") private static Matrix makeRDiagonals(Matrix stdDevs, Matrix x) { - return VecBuilder.fill(stdDevs.get(0,0) * x.get(2,0), stdDevs.get(0, 0) * x.get(3, 0)); + return VecBuilder.fill(stdDevs.get(0, 0) * x.get(2, 0), stdDevs.get(0, 0) * x.get(3, 0)); } + @SuppressWarnings("ParameterName") private static Matrix makeVisionRDiagonals(Matrix stdDevs, Matrix y) { return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0), - stdDevs.get(2,0) * y.get(2,0), stdDevs.get(2, 0) * y.get(3, 0)); + stdDevs.get(2, 0) * y.get(2, 0), stdDevs.get(2, 0) * y.get(3, 0)); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java index 549e72769af..a8b1fbb8e54 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java @@ -107,7 +107,7 @@ public void testAccuracy() { if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) { if (lastVisionPose != null) { - //estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } var groundPose = groundtruthState.poseMeters; lastVisionPose = new Pose2d( @@ -172,11 +172,11 @@ public void testAccuracy() { chart.addSeries("Trajectory", trajXs, trajYs); chart.addSeries("xHat", observerXs, observerYs); -// new SwingWrapper<>(chart).displayChart(); -// try { -// Thread.sleep(1000000000); -// } catch (InterruptedException e) { -// } + new SwingWrapper<>(chart).displayChart(); + try { + Thread.sleep(1000000000); + } catch (InterruptedException e) { + } } @Test void test() { diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java index 7474b02f745..298360e8af6 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java @@ -196,9 +196,20 @@ public void reset() { * @param dtSeconds Timestep for prediction. */ @SuppressWarnings("ParameterName") - @Override public void predict(Matrix u, double dtSeconds) { - predict(u, m_f, dtSeconds); + predict(u, m_f, m_contQ, dtSeconds); + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + @SuppressWarnings("ParameterName") + @Override + public void predict(Matrix u, Matrix q, double dtSeconds) { + predict(u, m_f, q, dtSeconds); } /** @@ -206,19 +217,21 @@ public void predict(Matrix u, double dtSeconds) { * * @param u New control input from controller. * @param f The function used to linearlize the model. + * @param q The process noise covariance matrix * @param dtSeconds Timestep for prediction. */ @SuppressWarnings("ParameterName") public void predict( Matrix u, BiFunction, Matrix, Matrix> f, + Matrix q, double dtSeconds ) { // Find continuous A final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u); // Find discrete A and Q - final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds); + final var discPair = Discretization.discretizeAQTaylor(contA, q, dtSeconds); final var discA = discPair.getFirst(); final var discQ = discPair.getSecond(); diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java index aa93b29588b..ff8583be3dc 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java @@ -12,24 +12,24 @@ import edu.wpi.first.wpiutil.math.numbers.N1; @SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"}) -interface KalmanTypeFilter { - Matrix getP(); +interface KalmanTypeFilter { + Matrix getP(); double getP(int i, int j); - void setP(Matrix newP); + void setP(Matrix newP); - Matrix getXhat(); + Matrix getXhat(); double getXhat(int i); - void setXhat(Matrix xHat); + void setXhat(Matrix xHat); void setXhat(int i, double value); void reset(); - void predict(Matrix u, double dtSeconds); + void predict(Matrix u, Matrix q, double dtSeconds); - void correct(Matrix u, Matrix y); + void correct(Matrix u, Matrix y); } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java index db44da12c6d..5b175c3b7ae 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java @@ -217,7 +217,6 @@ public void reset() { * @param dtSeconds Timestep for prediction. */ @SuppressWarnings({"LocalVariableName", "ParameterName"}) - @Override public void predict(Matrix u, double dtSeconds) { predict(u, m_contQ, dtSeconds); } @@ -234,6 +233,7 @@ public void predict(Matrix u, double dtSeconds) { * @param dtSeconds Timestep for prediction. */ @SuppressWarnings({"LocalVariableName", "ParameterName"}) + @Override public void predict(Matrix u, Matrix q, double dtSeconds) { // Discretize Q before projecting mean and covariance forward Matrix contA = From c617d2ad7f4bf9089e8c446b138fa9c8842ee882 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 23 Oct 2020 18:08:37 -0400 Subject: [PATCH 19/23] Whoops --- .../native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp | 2 +- .../wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java | 2 +- .../wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java | 2 +- .../first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 0d2bf642696..5a2af3b6e16 100644 --- a/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpilibc/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -85,7 +85,7 @@ TEST(DifferentialDrivePoseEstimatorTest, TestAccuracy) { if (lastVisionUpdateTime + kVisionUpdateRate < t) { if (lastVisionPose != frc::Pose2d()) { - // estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } lastVisionPose = groundTruthState.pose + diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java index 5b5026ca5e7..326785312f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -250,7 +250,7 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, var localY = VecBuilder.fill(angle.getCos(), angle.getSin()); Matrix q = null; // TODO - m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, currentTimeSeconds); + m_latencyCompensator.addObserverState(m_observer, u, localY, q, null, null, currentTimeSeconds); m_observer.predict(u, dt); m_observer.correct(u, localY); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java index a8b1fbb8e54..47fade37439 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java @@ -107,7 +107,7 @@ public void testAccuracy() { if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) { if (lastVisionPose != null) { - estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); +// estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } var groundPose = groundtruthState.poseMeters; lastVisionPose = new Pose2d( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java index b76b074496c..44df6704f82 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java @@ -94,7 +94,7 @@ public void testAccuracy() { if (lastVisionUpdateTime + visionUpdateRate < t) { if (lastVisionPose != null) { - estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); +// estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } lastVisionPose = new Pose2d( From 3048f95cf0c95a468402d84f61352fbfc7a6bb74 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 23 Oct 2020 18:08:42 -0400 Subject: [PATCH 20/23] Update build.gradle --- wpilibj/build.gradle | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle index df16022c4f2..1a0a264254d 100644 --- a/wpilibj/build.gradle +++ b/wpilibj/build.gradle @@ -75,6 +75,8 @@ dependencies { devImplementation project(':cscore') devImplementation project(':cameraserver') devImplementation sourceSets.main.output + + testImplementation 'org.knowm.xchart:xchart:3.6.5' } apply plugin: 'cpp' From cd12928cdfc08ce341aff39d91d69b3bdb40ae4f Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 23 Oct 2020 19:19:26 -0400 Subject: [PATCH 21/23] Add JNI for llt --- .../DifferentialDrivePoseEstimator.java | 2 +- .../KalmanFilterLatencyCompensator.java | 21 ++++++++++------- .../estimator/MecanumDrivePoseEstimator.java | 2 +- .../estimator/SwerveDrivePoseEstimator.java | 2 +- .../DifferentialDrivePoseEstimatorTest.java | 2 +- .../java/edu/wpi/first/math/WPIMathJNI.java | 11 ++++++++- .../wpilibj/estimator/KalmanTypeFilter.java | 6 +++++ .../estimator/UnscentedKalmanFilter.java | 2 +- .../edu/wpi/first/wpiutil/math/Matrix.java | 9 ++++++++ .../src/main/native/cpp/jni/WPIMathJNI.cpp | 23 +++++++++++++++++++ 10 files changed, 66 insertions(+), 14 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java index c97b67e7f66..29817f724f4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java @@ -316,7 +316,7 @@ public Pose2d updateWithTime( var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getCos(), angle.getSin()); var q = StateSpaceUtil.makeCovarianceMatrix(Nat.N6(), makeQDiagonals(m_stateStdDevs, m_observer.getXhat())); var r = StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat())); - m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, this::localMeasurementModel, currentTimeSeconds); + m_latencyCompensator.addObserverState(Nat.N4(), m_observer, u, localY, q, r, this::localMeasurementModel, currentTimeSeconds); m_observer.predict(u, q, dt); m_observer.correct(Nat.N4(), u, localY, this::localMeasurementModel, r); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java index 804ec67fb33..2e8f6d9e682 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java @@ -14,6 +14,7 @@ import java.util.function.BiFunction; import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; @@ -36,14 +37,15 @@ public class KalmanFilterLatencyCompensator void addObserverState( + Nat rows, KalmanTypeFilter observer, Matrix u, Matrix localY, Matrix q, Matrix r, BiFunction, Matrix, Matrix> localMeasurementModel, double timestampSeconds ) { m_pastObserverSnapshots.add(Map.entry( - timestampSeconds, new ObserverSnapshot(observer, u, localY, q, r, localMeasurementModel) + timestampSeconds, new ObserverSnapshot(observer, u, localY, q, r, localMeasurementModel, rows) )); if (m_pastObserverSnapshots.size() > k_maxPastObserverStates) { @@ -114,7 +116,7 @@ public void applyPastGlobalMeasurement( // get the present estimated state. for (int i = indexOfClosestEntry; i < m_pastObserverSnapshots.size(); i++) { var key = m_pastObserverSnapshots.get(i).getKey(); - var snapshot = m_pastObserverSnapshots.get(i).getValue(); + ObserverSnapshot snapshot = m_pastObserverSnapshots.get(i).getValue(); if (i == indexOfClosestEntry) { observer.setP(snapshot.errorCovariances); @@ -122,7 +124,7 @@ public void applyPastGlobalMeasurement( } observer.predict(snapshot.inputs, snapshot.q, key - lastTimestamp); - observer.correct(snapshot.inputs, snapshot.localMeasurements); + observer.correct(snapshot.localModelNats, snapshot.inputs, snapshot.localMeasurements, snapshot.localMeasurementModel, snapshot.r); if (i == indexOfClosestEntry) { // Note that the measurement is at a timestep close but probably not exactly equal to the @@ -134,7 +136,7 @@ public void applyPastGlobalMeasurement( lastTimestamp = key; m_pastObserverSnapshots.set(i, Map.entry(key, new ObserverSnapshot(observer, snapshot.inputs, - snapshot.localMeasurements, snapshot.q, snapshot.r, snapshot.localMeasurementModel))); + snapshot.localMeasurements, snapshot.q, snapshot.r, snapshot.localMeasurementModel, snapshot.localModelNats))); } } @@ -142,25 +144,28 @@ public void applyPastGlobalMeasurement( * This class contains all the information about our observer at a given time. */ @SuppressWarnings("MemberName") - public class ObserverSnapshot { + public class ObserverSnapshot { public final Matrix xHat; public final Matrix errorCovariances; public final Matrix inputs; public final Matrix localMeasurements; public final Matrix q; private final Matrix r; - private final BiFunction, Matrix, Matrix> localMeasurementModel; + private final BiFunction, Matrix, Matrix> localMeasurementModel; + private final Nat localModelNats; @SuppressWarnings("ParameterName") private ObserverSnapshot( KalmanTypeFilter observer, Matrix u, Matrix localY, - Matrix q, Matrix r, BiFunction, Matrix, Matrix> localMeasurementModel + Matrix q, Matrix r, BiFunction, Matrix, Matrix> localMeasurementModel, + Nat rows ) { this.xHat = observer.getXhat(); this.errorCovariances = observer.getP(); this.q = q; this.r = r; this.localMeasurementModel = localMeasurementModel; + this.localModelNats = rows; inputs = u; localMeasurements = localY; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java index 326785312f0..059bf629ef9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -250,7 +250,7 @@ public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, var localY = VecBuilder.fill(angle.getCos(), angle.getSin()); Matrix q = null; // TODO - m_latencyCompensator.addObserverState(m_observer, u, localY, q, null, null, currentTimeSeconds); + m_latencyCompensator.addObserverState(null, m_observer, u, localY, q, null, null, currentTimeSeconds); m_observer.predict(u, dt); m_observer.correct(u, localY); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 66d2ddcb3bd..f632ef4ef29 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -275,7 +275,7 @@ public Pose2d updateWithTime( makeRDiagonals(m_localMeasurementStdDevs, m_observer.getXhat())); BiFunction, Matrix, Matrix> model = (x, u_) -> x.block(Nat.N2(), Nat.N1(), 2, 0); - m_latencyCompensator.addObserverState(m_observer, u, localY, q, r, model, currentTimeSeconds); + m_latencyCompensator.addObserverState(null, m_observer, u, localY, q, r, model, currentTimeSeconds); m_observer.predict(u, q, dt); m_observer.correct(Nat.N2(), u, localY, model, r); return getEstimatedPosition(); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java index 47fade37439..a8b1fbb8e54 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java @@ -107,7 +107,7 @@ public void testAccuracy() { if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) { if (lastVisionPose != null) { -// estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); + estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime); } var groundPose = groundtruthState.poseMeters; lastVisionPose = new Pose2d( diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 30984ac3b15..f12c0eddcc0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -69,11 +69,20 @@ public static native void discreteAlgebraicRiccatiEquation( * Computes the matrix exp. * * @param src Array of elements of the matrix to be exponentiated. - * @param rows how many rows there are. + * @param rows How many rows there are. * @param dst Array where the result will be stored. */ public static native void exp(double[] src, int rows, double[] dst); + /** + * Computes the lower-left triangular decomp + * + * @param src Array of elements of the matrix to be decomposed. + * @param rows How many rows there are. + * @param dst Array where the result will be stored. + */ + public static native void llt(double[] src, int rows, double[] dst); + /** * Returns true if (A, B) is a stabilizable pair. * diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java index ff8583be3dc..baf4b3a2213 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java @@ -8,9 +8,12 @@ package edu.wpi.first.wpilibj.estimator; import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; +import java.util.function.BiFunction; + @SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"}) interface KalmanTypeFilter { Matrix getP(); @@ -32,4 +35,7 @@ interface KalmanTypeFilter { void predict(Matrix u, Matrix q, double dtSeconds); void correct(Matrix u, Matrix y); + + void correct(Nat rows, Matrix u, Matrix y, BiFunction, + Matrix, Matrix> localMeasurementModel, Matrix r); } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java index 5b175c3b7ae..dec5d9ebabe 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java @@ -264,7 +264,6 @@ public void predict(Matrix u, Matrix q, double dtSec * @param y Measurement vector. */ @SuppressWarnings("ParameterName") - @Override public void correct(Matrix u, Matrix y) { correct(m_outputs, u, y, m_h, m_contR); } @@ -283,6 +282,7 @@ public void correct(Matrix u, Matrix y) { * @param R Measurement noise covariance matrix. */ @SuppressWarnings({"ParameterName", "LocalVariableName"}) + @Override public void correct( Nat rows, Matrix u, Matrix y, diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index 5a0c6920a78..266fa0fdd5c 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -527,6 +527,15 @@ public Matrix lltDecompose(boolean lowerTriangular) { CholeskyDecomposition_F64 chol = DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular); + + for(int i = 0; i < temp.numRows(); i++) { + for(int j = 0; j < temp.numCols(); j++) { + if(temp.get(i, j) < 0) { + temp.set(i, j, 0); + } + } + } + if (!chol.decompose(temp.getMatrix())) { // check that the input is not all zeros -- if they are, we special case and return all // zeros. diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 26d57e2ab00..c37a4fb9028 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -15,6 +15,7 @@ #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" #include "unsupported/Eigen/MatrixFunctions" +#include "Eigen/src/Cholesky/LLT.h" using namespace wpi::java; @@ -105,6 +106,28 @@ Java_edu_wpi_first_math_WPIMathJNI_exp env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data()); } +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: llt + * Signature: ([DI[D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_math_WPIMathJNI_llt + (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst) +{ + jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr); + + Eigen::Map< + Eigen::Matrix> + Amat{arrayBody, rows, rows}; + + Eigen::Matrix U = + Amat.llt().matrixL(); + + env->ReleaseDoubleArrayElements(src, arrayBody, 0); + env->SetDoubleArrayRegion(dst, 0, rows * rows, U.data()); +} + /* * Class: edu_wpi_first_math_WPIMathJNI * Method: isStabilizable From 8946867e3991fae901e6bfefa40fcac2b6eb26f3 Mon Sep 17 00:00:00 2001 From: cttdev Date: Fri, 23 Oct 2020 20:57:49 -0700 Subject: [PATCH 22/23] Adding JNI llt decomposition with Eigen. --- .../main/java/edu/wpi/first/math/WPIMathJNI.java | 5 +++-- .../wpilibj/estimator/MerweScaledSigmaPoints.java | 2 +- .../java/edu/wpi/first/wpiutil/math/Matrix.java | 14 +++++++++++++- wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 10 +++++----- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index f12c0eddcc0..b07905fda54 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -75,13 +75,14 @@ public static native void discreteAlgebraicRiccatiEquation( public static native void exp(double[] src, int rows, double[] dst); /** - * Computes the lower-left triangular decomp + * Computes the lower-left triangular matrix decomposition. * * @param src Array of elements of the matrix to be decomposed. * @param rows How many rows there are. + * @param columns How many columns there are. * @param dst Array where the result will be stored. */ - public static native void llt(double[] src, int rows, double[] dst); + public static native void llt(double[] src, int rows, int columns, double[] dst); /** * Returns true if (A, B) is a stabilizable pair. diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java index 56e9288c07f..bc8abdc534c 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java @@ -90,7 +90,7 @@ public int getNumSigmas() { double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum(); var intermediate = P.times(lambda + m_states.getNum()); - var U = intermediate.lltDecompose(true); // Lower triangular + var U = intermediate.llt(); // Lower triangular // 2 * states + 1 by states Matrix sigmas = new Matrix<>( diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index 266fa0fdd5c..30c9644acf7 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -355,7 +355,7 @@ public final Matrix solve(Matrix b) { * This method only works for square matrices, and will * otherwise throw an {@link MatrixDimensionException}. * - * @return The exponential of A. + * @return The exponential of this matrix. */ public final Matrix exp() { if (this.getNumRows() != this.getNumCols()) { @@ -368,6 +368,18 @@ public final Matrix exp() { return toReturn; } + /** + * Computes the lower-left triangular matrix decomposition using Eigen's solver. + * + * @return The decomposition of this matrix. + */ + public final Matrix llt() { + Matrix toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols())); + WPIMathJNI.llt(this.m_storage.getDDRM().getData(), this.getNumRows(), this.getNumCols(), + toReturn.m_storage.getDDRM().getData()); + return toReturn; + } + /** * Returns the determinant of this matrix. * diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index c37a4fb9028..98f4a1320b6 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -109,23 +109,23 @@ Java_edu_wpi_first_math_WPIMathJNI_exp /* * Class: edu_wpi_first_math_WPIMathJNI * Method: llt - * Signature: ([DI[D)V + * Signature: ([DII[D)V */ JNIEXPORT void JNICALL Java_edu_wpi_first_math_WPIMathJNI_llt - (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst) + (JNIEnv* env, jclass, jdoubleArray src, jint rows, jint columns, jdoubleArray dst) { jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr); Eigen::Map< Eigen::Matrix> - Amat{arrayBody, rows, rows}; + P{arrayBody, rows, columns}; Eigen::Matrix U = - Amat.llt().matrixL(); + P.llt().matrixL(); env->ReleaseDoubleArrayElements(src, arrayBody, 0); - env->SetDoubleArrayRegion(dst, 0, rows * rows, U.data()); + env->SetDoubleArrayRegion(dst, 0, rows * columns, U.data()); } /* From de330b47502b7e3d7426fd1f476471bb8f1824af Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 24 Oct 2020 00:50:03 -0400 Subject: [PATCH 23/23] Add naive test that basically recreates swerve --- .../SwerveDrivePoseEstimatorTest.java | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java index 44df6704f82..94d82b07c21 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java @@ -11,6 +11,14 @@ import java.util.List; import java.util.Random; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N4; import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -214,4 +222,44 @@ public void testAccuracy() { } // */ } + + @Test + public void testNaive() { + + var traj = TrajectoryGenerator.generateTrajectory(List.of(new Pose2d(), new Pose2d(5, 5, new Rotation2d())), new TrajectoryConfig(2, 2)); + + Matrix x = VecBuilder.fill(0, 0, 1, 0); + var ukf = new UnscentedKalmanFilter<>(Nat.N4(), Nat.N2(), this::f, (x_, u) -> x_.block(Nat.N2(), Nat.N1(), 2, 0), VecBuilder.fill(0.1, 0.1, 0.1, 0.1), VecBuilder.fill(0.1, 0.1), 0.020); + ukf.setXhat(VecBuilder.fill(0, 0, 1, 0)); + for (int i = 0; i < traj.getTotalTimeSeconds() / 0.020; i++) { + var state = traj.sample(i * 0.020); + var u = VecBuilder.fill(state.velocityMetersPerSecond * state.poseMeters.getRotation().getCos(), state.velocityMetersPerSecond * state.poseMeters.getRotation().getSin(), state.curvatureRadPerMeter * state.velocityMetersPerSecond); + x = RungeKutta.rungeKutta(this::f, x, u, 0.020); + ukf.predict(u, StateSpaceUtil.makeCovarianceMatrix(Nat.N4(), makeQDiagonals(VecBuilder.fill(0.1, 0.1, 0.1), ukf.getXhat())),0.020); + ukf.correct(Nat.N2(), u, x.block(Nat.N2(), Nat.N1(), 2, 0), (x_, u_) -> x_.block(Nat.N2(), Nat.N1(), 2, 0), StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), makeRDiagonals(VecBuilder.fill(0.1), ukf.getXhat()))); + +// System.out.printf("%s, %s, %s, %s\n", x.get(0,0), x.get(1,0), x.get(2,0), x.get(3,0)); + System.out.printf("%s, %s, %s, %s\n", ukf.getXhat(0), ukf.getXhat(1), ukf.getXhat(2), ukf.getXhat(3)); + } + } + + // U = vx, vy, omega in global + Matrix f(Matrix x, Matrix u) { + return VecBuilder.fill( + u.get(0, 0), u.get(1, 0), -x.get(3, 0) * u.get(2, 0), x.get(2, 0) * u.get(2, 0) + ); + } + + @SuppressWarnings("ParameterName") + private static Matrix makeQDiagonals(Matrix stdDevs, Matrix x) { + return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(1, 0), + stdDevs.get(2, 0) * x.get(2, 0), stdDevs.get(2, 0) * x.get(3, 0)); +// stdDevs.get(2, 0), stdDevs.get(2, 0)); + } + + @SuppressWarnings("ParameterName") + private static Matrix makeRDiagonals(Matrix stdDevs, Matrix x) { + return VecBuilder.fill(stdDevs.get(0, 0) * x.get(2, 0), stdDevs.get(0, 0) * x.get(3, 0)); +// return VecBuilder.fill(stdDevs.get(0, 0), stdDevs.get(0, 0)); + } }