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/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}};
};