Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

Browse Source 
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2022-12-02 11:36:10 -05:00
committed by GitHub
parent 68dba92630
commit e22d8cc343
35 changed files with 2288 additions and 1884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -27,8 +27,6 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(),
{units::meters_per_second_t{m_leftEncoder.GetRate()},
units::meters_per_second_t{m_rightEncoder.GetRate()}},
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});

4
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
View File

@@ -79,12 +79,12 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::DifferentialDrivePoseEstimator m_poseEstimator{
m_kinematics,
m_gyro.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()},
frc::Pose2d{},
{0.01, 0.01, 0.01, 0.01, 0.01},
{0.1, 0.1, 0.1},
{0.01, 0.01, 0.01},
{0.1, 0.1, 0.1}};
// Gains are for example purposes only - must be determined for your own

3
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -56,8 +56,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState(),
GetCurrentDistances());
m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an
// example -- on a real robot, this must be calculated based either on latency

9
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -77,11 +77,6 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{
m_gyro.GetRotation2d(),
GetCurrentDistances(),
frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1}};
m_kinematics, m_gyro.GetRotation2d(), GetCurrentDistances(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -28,8 +28,6 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(),
{m_frontLeft.GetState(), m_frontRight.GetState(),
m_backLeft.GetState(), m_backRight.GetState()},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()});

5
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
View File

@@ -50,12 +50,11 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
m_kinematics,
frc::Rotation2d{},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()},
frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1},
{0.1, 0.1, 0.1}};
};