Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
ecef13c190fdf05a2b4d9cbb573c9c08ee8d64bd
YAGSL/docs/swervelib/math/SwervePoseEstimator2.html
thenetworkgrinch ecef13c190 Updated docs
2023-04-08 13:35:46 -05:00

503 lines
35 KiB
HTML
Raw Blame History

<!DOCTYPE HTML>
<html lang="en">
<head>
<!-- Generated by javadoc (17) on Sat Apr 08 13:35:03 CDT 2023 -->
<title>SwervePoseEstimator2</title>
<meta name="viewport" content="width=device-width, initial-scale=1">
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
<meta name="dc.created" content="2023-04-08">
<meta name="description" content="declaration: package: swervelib.math, class: SwervePoseEstimator2">
<meta name="generator" content="javadoc/ClassWriterImpl">
<link rel="stylesheet" type="text/css" href="../../stylesheet.css" title="Style">
<link rel="stylesheet" type="text/css" href="../../script-dir/jquery-ui.min.css" title="Style">
<link rel="stylesheet" type="text/css" href="../../jquery-ui.overrides.css" title="Style">
<script type="text/javascript" src="../../script.js"></script>
<script type="text/javascript" src="../../script-dir/jquery-3.5.1.min.js"></script>
<script type="text/javascript" src="../../script-dir/jquery-ui.min.js"></script>
</head>
<body class="class-declaration-page">
<script type="text/javascript">var evenRowColor = "even-row-color";
var oddRowColor = "odd-row-color";
var tableTab = "table-tab";
var activeTableTab = "active-table-tab";
var pathtoroot = "../../";
loadScripts(document, 'script');</script>
<noscript>
<div>JavaScript is disabled on your browser.</div>
</noscript>
<div class="flex-box">
<header role="banner" class="flex-header">
<nav role="navigation">
<!-- ========= START OF TOP NAVBAR ======= -->
<div class="top-nav" id="navbar-top">
<div class="skip-nav"><a href="#skip-navbar-top" title="Skip navigation links">Skip navigation links</a></div>
<ul id="navbar-top-firstrow" class="nav-list" title="Navigation">
<li><a href="../../index.html">Overview</a></li>
<li><a href="package-summary.html">Package</a></li>
<li class="nav-bar-cell1-rev">Class</li>
<li><a href="package-tree.html">Tree</a></li>
<li><a href="../../index-files/index-1.html">Index</a></li>
<li><a href="../../help-doc.html#class">Help</a></li>
</ul>
</div>
<div class="sub-nav">
<div>
<ul class="sub-nav-list">
<li>Summary:&nbsp;</li>
<li><a href="#nested-class-summary">Nested</a>&nbsp;|&nbsp;</li>
<li><a href="#field-summary">Field</a>&nbsp;|&nbsp;</li>
<li><a href="#constructor-summary">Constr</a>&nbsp;|&nbsp;</li>
<li><a href="#method-summary">Method</a></li>
</ul>
<ul class="sub-nav-list">
<li>Detail:&nbsp;</li>
<li><a href="#field-detail">Field</a>&nbsp;|&nbsp;</li>
<li><a href="#constructor-detail">Constr</a>&nbsp;|&nbsp;</li>
<li><a href="#method-detail">Method</a></li>
</ul>
</div>
<div class="nav-list-search"><label for="search-input">SEARCH:</label>
<input type="text" id="search-input" value="search" disabled="disabled">
<input type="reset" id="reset-button" value="reset" disabled="disabled">
</div>
</div>
<!-- ========= END OF TOP NAVBAR ========= -->
<span class="skip-nav" id="skip-navbar-top"></span></nav>
</header>
<div class="flex-content">
<main role="main">
<!-- ======== START OF CLASS DATA ======== -->
<div class="header">
<div class="sub-title"><span class="package-label-in-type">Package</span>&nbsp;<a href="package-summary.html">swervelib.math</a></div>
<h1 title="Class SwervePoseEstimator2" class="title">Class SwervePoseEstimator2</h1>
</div>
<div class="inheritance" title="Inheritance Tree"><a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">java.lang.Object</a>
<div class="inheritance">swervelib.math.SwervePoseEstimator2</div>
</div>
<section class="class-description" id="class-description">
<hr>
<div class="type-signature"><span class="modifiers">public class </span><span class="element-name type-name-label">SwervePoseEstimator2</span>
<span class="extends-implements">extends <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">Object</a></span></div>
<div class="block">Clone of <code>SwerveDrivePoseEstimator</code> which takes into account gyroscope pitch and roll to achieve a more
accurate estimation, originally made by Team 1466.</div>
</section>
<section class="summary">
<ul class="summary-list">
<!-- ======== NESTED CLASS SUMMARY ======== -->
<li>
<section class="nested-class-summary" id="nested-class-summary">
<h2>Nested Class Summary</h2>
<div class="caption"><span>Nested Classes</span></div>
<div class="summary-table three-column-summary">
<div class="table-header col-first">Modifier and Type</div>
<div class="table-header col-second">Class</div>
<div class="table-header col-last">Description</div>
<div class="col-first even-row-color"><code>private class&nbsp;</code></div>
<div class="col-second even-row-color"><code><a href="SwervePoseEstimator2.InterpolationRecord.html" class="type-name-link" title="class in swervelib.math">SwervePoseEstimator2.InterpolationRecord</a></code></div>
<div class="col-last even-row-color">
<div class="block">Represents an odometry record.</div>
</div>
</div>
</section>
</li>
<!-- =========== FIELD SUMMARY =========== -->
<li>
<section class="field-summary" id="field-summary">
<h2>Field Summary</h2>
<div class="caption"><span>Fields</span></div>
<div class="summary-table three-column-summary">
<div class="table-header col-first">Modifier and Type</div>
<div class="table-header col-second">Field</div>
<div class="table-header col-last">Description</div>
<div class="col-first even-row-color"><code>private final edu.wpi.first.math.kinematics.SwerveDriveKinematics</code></div>
<div class="col-second even-row-color"><code><a href="#m_kinematics" class="member-name-link">m_kinematics</a></code></div>
<div class="col-last even-row-color">
<div class="block">Swerve drive kinematics.</div>
</div>
<div class="col-first odd-row-color"><code>private final int</code></div>
<div class="col-second odd-row-color"><code><a href="#m_numModules" class="member-name-link">m_numModules</a></code></div>
<div class="col-last odd-row-color">
<div class="block">Number of swerve modules.</div>
</div>
<div class="col-first even-row-color"><code>private final <a href="SwerveDriveOdometry2.html" title="class in swervelib.math">SwerveDriveOdometry2</a></code></div>
<div class="col-second even-row-color"><code><a href="#m_odometry" class="member-name-link">m_odometry</a></code></div>
<div class="col-last even-row-color">
<div class="block">Enhanced swerve drive odometry.</div>
</div>
<div class="col-first odd-row-color"><code>private final edu.wpi.first.math.interpolation.TimeInterpolatableBuffer&lt;<a href="SwervePoseEstimator2.InterpolationRecord.html" title="class in swervelib.math">SwervePoseEstimator2.InterpolationRecord</a>&gt;</code></div>
<div class="col-second odd-row-color"><code><a href="#m_poseBuffer" class="member-name-link">m_poseBuffer</a></code></div>
<div class="col-last odd-row-color">
<div class="block">Interpolation buffer.</div>
</div>
<div class="col-first even-row-color"><code>private final edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;</code></div>
<div class="col-second even-row-color"><code><a href="#m_q" class="member-name-link">m_q</a></code></div>
<div class="col-last even-row-color">
<div class="block">Matrix quotient.</div>
</div>
<div class="col-first odd-row-color"><code>private final edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N3&gt;</code></div>
<div class="col-second odd-row-color"><code><a href="#m_visionK" class="member-name-link">m_visionK</a></code></div>
<div class="col-last odd-row-color">
<div class="block">Vision standard deviations.</div>
</div>
</div>
</section>
</li>
<!-- ======== CONSTRUCTOR SUMMARY ======== -->
<li>
<section class="constructor-summary" id="constructor-summary">
<h2>Constructor Summary</h2>
<div class="caption"><span>Constructors</span></div>
<div class="summary-table two-column-summary">
<div class="table-header col-first">Constructor</div>
<div class="table-header col-last">Description</div>
<div class="col-constructor-name even-row-color"><code><a href="#%3Cinit%3E(edu.wpi.first.math.kinematics.SwerveDriveKinematics,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition%5B%5D,edu.wpi.first.math.geometry.Pose2d)" class="member-name-link">SwervePoseEstimator2</a><wbr>(edu.wpi.first.math.kinematics.SwerveDriveKinematics&nbsp;kinematics,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;initialPoseMeters)</code></div>
<div class="col-last even-row-color">
<div class="block">Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.</div>
</div>
<div class="col-constructor-name odd-row-color"><code><a href="#%3Cinit%3E(edu.wpi.first.math.kinematics.SwerveDriveKinematics,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition%5B%5D,edu.wpi.first.math.geometry.Pose2d,edu.wpi.first.math.Matrix,edu.wpi.first.math.Matrix)" class="member-name-link">SwervePoseEstimator2</a><wbr>(edu.wpi.first.math.kinematics.SwerveDriveKinematics&nbsp;kinematics,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;initialPoseMeters,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;stateStdDevs,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</code></div>
<div class="col-last odd-row-color">
<div class="block">Constructs a SwerveDrivePoseEstimator.</div>
</div>
</div>
</section>
</li>
<!-- ========== METHOD SUMMARY =========== -->
<li>
<section class="method-summary" id="method-summary">
<h2>Method Summary</h2>
<div id="method-summary-table">
<div class="table-tabs" role="tablist" aria-orientation="horizontal"><button id="method-summary-table-tab0" role="tab" aria-selected="true" aria-controls="method-summary-table.tabpanel" tabindex="0" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table', 3)" class="active-table-tab">All Methods</button><button id="method-summary-table-tab2" role="tab" aria-selected="false" aria-controls="method-summary-table.tabpanel" tabindex="-1" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table-tab2', 3)" class="table-tab">Instance Methods</button><button id="method-summary-table-tab4" role="tab" aria-selected="false" aria-controls="method-summary-table.tabpanel" tabindex="-1" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table-tab4', 3)" class="table-tab">Concrete Methods</button></div>
<div id="method-summary-table.tabpanel" role="tabpanel">
<div class="summary-table three-column-summary" aria-labelledby="method-summary-table-tab0">
<div class="table-header col-first">Modifier and Type</div>
<div class="table-header col-second">Method</div>
<div class="table-header col-last">Description</div>
<div class="col-first even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>void</code></div>
<div class="col-second even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d,double)" class="member-name-link">addVisionMeasurement</a><wbr>(edu.wpi.first.math.geometry.Pose2d&nbsp;visionRobotPoseMeters,
double&nbsp;timestampSeconds)</code></div>
<div class="col-last even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Adds a vision measurement to the Kalman Filter.</div>
</div>
<div class="col-first odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>void</code></div>
<div class="col-second odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d,double,edu.wpi.first.math.Matrix)" class="member-name-link">addVisionMeasurement</a><wbr>(edu.wpi.first.math.geometry.Pose2d&nbsp;visionRobotPoseMeters,
double&nbsp;timestampSeconds,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</code></div>
<div class="col-last odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Adds a vision measurement to the Kalman Filter.</div>
</div>
<div class="col-first even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>edu.wpi.first.math.geometry.Pose2d</code></div>
<div class="col-second even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#getEstimatedPosition()" class="member-name-link">getEstimatedPosition</a>()</code></div>
<div class="col-last even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Gets the estimated robot pose.</div>
</div>
<div class="col-first odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>void</code></div>
<div class="col-second odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#resetPosition(edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition%5B%5D,edu.wpi.first.math.geometry.Pose2d)" class="member-name-link">resetPosition</a><wbr>(edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;poseMeters)</code></div>
<div class="col-last odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Resets the robot's position on the field.</div>
</div>
<div class="col-first even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>void</code></div>
<div class="col-second even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#setVisionMeasurementStdDevs(edu.wpi.first.math.Matrix)" class="member-name-link">setVisionMeasurementStdDevs</a><wbr>(edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</code></div>
<div class="col-last even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Sets the pose estimator's trust of global measurements.</div>
</div>
<div class="col-first odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>edu.wpi.first.math.geometry.Pose2d</code></div>
<div class="col-second odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#update(edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition%5B%5D)" class="member-name-link">update</a><wbr>(edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroPitch,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroRoll,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions)</code></div>
<div class="col-last odd-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Updates the pose estimator with wheel encoder and gyro information.</div>
</div>
<div class="col-first even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code>edu.wpi.first.math.geometry.Pose2d</code></div>
<div class="col-second even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4"><code><a href="#updateWithTime(double,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition%5B%5D)" class="member-name-link">updateWithTime</a><wbr>(double&nbsp;currentTimeSeconds,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroPitch,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroRoll,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions)</code></div>
<div class="col-last even-row-color method-summary-table method-summary-table-tab2 method-summary-table-tab4">
<div class="block">Updates the pose estimator with wheel encoder and gyro information.</div>
</div>
</div>
</div>
</div>
<div class="inherited-list">
<h3 id="methods-inherited-from-class-java.lang.Object">Methods inherited from class&nbsp;java.lang.<a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">Object</a></h3>
<code><a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#clone()" title="class or interface in java.lang" class="external-link">clone</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#equals(java.lang.Object)" title="class or interface in java.lang" class="external-link">equals</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#finalize()" title="class or interface in java.lang" class="external-link">finalize</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#getClass()" title="class or interface in java.lang" class="external-link">getClass</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#hashCode()" title="class or interface in java.lang" class="external-link">hashCode</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#notify()" title="class or interface in java.lang" class="external-link">notify</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#notifyAll()" title="class or interface in java.lang" class="external-link">notifyAll</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#toString()" title="class or interface in java.lang" class="external-link">toString</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait()" title="class or interface in java.lang" class="external-link">wait</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait(long)" title="class or interface in java.lang" class="external-link">wait</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait(long,int)" title="class or interface in java.lang" class="external-link">wait</a></code></div>
</section>
</li>
</ul>
</section>
<section class="details">
<ul class="details-list">
<!-- ============ FIELD DETAIL =========== -->
<li>
<section class="field-details" id="field-detail">
<h2>Field Details</h2>
<ul class="member-list">
<li>
<section class="detail" id="m_kinematics">
<h3>m_kinematics</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type">edu.wpi.first.math.kinematics.SwerveDriveKinematics</span>&nbsp;<span class="element-name">m_kinematics</span></div>
<div class="block">Swerve drive kinematics.</div>
</section>
</li>
<li>
<section class="detail" id="m_odometry">
<h3>m_odometry</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type"><a href="SwerveDriveOdometry2.html" title="class in swervelib.math">SwerveDriveOdometry2</a></span>&nbsp;<span class="element-name">m_odometry</span></div>
<div class="block">Enhanced swerve drive odometry.</div>
</section>
</li>
<li>
<section class="detail" id="m_q">
<h3>m_q</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type">edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;</span>&nbsp;<span class="element-name">m_q</span></div>
<div class="block">Matrix quotient.</div>
</section>
</li>
<li>
<section class="detail" id="m_numModules">
<h3>m_numModules</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type">int</span>&nbsp;<span class="element-name">m_numModules</span></div>
<div class="block">Number of swerve modules.</div>
</section>
</li>
<li>
<section class="detail" id="m_poseBuffer">
<h3>m_poseBuffer</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type">edu.wpi.first.math.interpolation.TimeInterpolatableBuffer&lt;<a href="SwervePoseEstimator2.InterpolationRecord.html" title="class in swervelib.math">SwervePoseEstimator2.InterpolationRecord</a>&gt;</span>&nbsp;<span class="element-name">m_poseBuffer</span></div>
<div class="block">Interpolation buffer.</div>
</section>
</li>
<li>
<section class="detail" id="m_visionK">
<h3>m_visionK</h3>
<div class="member-signature"><span class="modifiers">private final</span>&nbsp;<span class="return-type">edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N3&gt;</span>&nbsp;<span class="element-name">m_visionK</span></div>
<div class="block">Vision standard deviations.</div>
</section>
</li>
</ul>
</section>
</li>
<!-- ========= CONSTRUCTOR DETAIL ======== -->
<li>
<section class="constructor-details" id="constructor-detail">
<h2>Constructor Details</h2>
<ul class="member-list">
<li>
<section class="detail" id="&lt;init&gt;(edu.wpi.first.math.kinematics.SwerveDriveKinematics,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition[],edu.wpi.first.math.geometry.Pose2d)">
<h3>SwervePoseEstimator2</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="element-name">SwervePoseEstimator2</span><wbr><span class="parameters">(edu.wpi.first.math.kinematics.SwerveDriveKinematics&nbsp;kinematics,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;initialPoseMeters)</span></div>
<div class="block">Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
<p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9
meters for y, and 0.9 radians for heading.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>kinematics</code> - A correctly-configured kinematics object for your drivetrain.</dd>
<dd><code>gyroAngle</code> - The current gyro angle.</dd>
<dd><code>modulePositions</code> - The current distance measurements and rotations of the swerve modules.</dd>
<dd><code>initialPoseMeters</code> - The starting pose estimate.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="&lt;init&gt;(edu.wpi.first.math.kinematics.SwerveDriveKinematics,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition[],edu.wpi.first.math.geometry.Pose2d,edu.wpi.first.math.Matrix,edu.wpi.first.math.Matrix)">
<h3>SwervePoseEstimator2</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="element-name">SwervePoseEstimator2</span><wbr><span class="parameters">(edu.wpi.first.math.kinematics.SwerveDriveKinematics&nbsp;kinematics,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;initialPoseMeters,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;stateStdDevs,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</span></div>
<div class="block">Constructs a SwerveDrivePoseEstimator.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>kinematics</code> - A correctly-configured kinematics object for your drivetrain.</dd>
<dd><code>gyroAngle</code> - The current gyro angle.</dd>
<dd><code>modulePositions</code> - The current distance and rotation measurements of the swerve modules.</dd>
<dd><code>initialPoseMeters</code> - The starting pose estimate.</dd>
<dd><code>stateStdDevs</code> - Standard deviations of the pose estimate (x position in meters, y position in
meters, and heading in radians). Increase these numbers to trust your state
estimate less.</dd>
<dd><code>visionMeasurementStdDevs</code> - Standard deviations of the vision pose measurement (x position in meters, y
position in meters, and heading in radians). Increase these numbers to trust the
vision pose measurement less.</dd>
</dl>
</section>
</li>
</ul>
</section>
</li>
<!-- ============ METHOD DETAIL ========== -->
<li>
<section class="method-details" id="method-detail">
<h2>Method Details</h2>
<ul class="member-list">
<li>
<section class="detail" id="setVisionMeasurementStdDevs(edu.wpi.first.math.Matrix)">
<h3>setVisionMeasurementStdDevs</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">void</span>&nbsp;<span class="element-name">setVisionMeasurementStdDevs</span><wbr><span class="parameters">(edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</span></div>
<div class="block">Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements
after the autonomous period, or to change trust as distance to a vision target increases.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>visionMeasurementStdDevs</code> - Standard deviations of the vision measurements. Increase these numbers to trust
global measurements from vision less. This matrix is in the form [x, y, theta]áµ€,
with units in meters and radians.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="resetPosition(edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition[],edu.wpi.first.math.geometry.Pose2d)">
<h3>resetPosition</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">void</span>&nbsp;<span class="element-name">resetPosition</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions,
edu.wpi.first.math.geometry.Pose2d&nbsp;poseMeters)</span></div>
<div class="block">Resets the robot's position on the field.
<p>The gyroscope angle does not need to be reset in the user's robot code. The library
automatically takes care of offsetting the gyro angle.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>gyroAngle</code> - The angle reported by the gyroscope.</dd>
<dd><code>modulePositions</code> - The current distance measurements and rotations of the swerve modules.</dd>
<dd><code>poseMeters</code> - The position on the field that your robot is at.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="getEstimatedPosition()">
<h3>getEstimatedPosition</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">edu.wpi.first.math.geometry.Pose2d</span>&nbsp;<span class="element-name">getEstimatedPosition</span>()</div>
<div class="block">Gets the estimated robot pose.</div>
<dl class="notes">
<dt>Returns:</dt>
<dd>The estimated robot pose in meters.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d,double)">
<h3>addVisionMeasurement</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">void</span>&nbsp;<span class="element-name">addVisionMeasurement</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Pose2d&nbsp;visionRobotPoseMeters,
double&nbsp;timestampSeconds)</span></div>
<div class="block">Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
for measurement noise.
<p>This method can be called as infrequently as you want, as long as you are calling <code>SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[])</code> every loop.
<p>To promote stability of the pose estimate and make it robust to bad vision data, we
recommend only adding vision measurements that are already within one meter or so of the current pose estimate.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>visionRobotPoseMeters</code> - The pose of the robot as measured by the vision camera.</dd>
<dd><code>timestampSeconds</code> - The timestamp of the vision measurement in seconds. Note that if you don't use your
own time source by calling
<code>SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[])</code> then you must use a timestamp with an epoch since FPGA
startup (i.e., the epoch of this timestamp is the same epoch as
<code>Timer.getFPGATimestamp()</code>.) This means that you should
use <code>Timer.getFPGATimestamp()</code> as your time source or sync
the epochs.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d,double,edu.wpi.first.math.Matrix)">
<h3>addVisionMeasurement</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">void</span>&nbsp;<span class="element-name">addVisionMeasurement</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Pose2d&nbsp;visionRobotPoseMeters,
double&nbsp;timestampSeconds,
edu.wpi.first.math.Matrix&lt;edu.wpi.first.math.numbers.N3,<wbr>edu.wpi.first.math.numbers.N1&gt;&nbsp;visionMeasurementStdDevs)</span></div>
<div class="block">Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
for measurement noise.
<p>This method can be called as infrequently as you want, as long as you are calling <code>SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[])</code> every loop.
<p>To promote stability of the pose estimate and make it robust to bad vision data, we
recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
<p>Note that the vision measurement standard deviations passed into this method will continue
to apply to future measurements until a subsequent call to
<code>SwerveDrivePoseEstimator.setVisionMeasurementStdDevs(Matrix)</code> or this method.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>visionRobotPoseMeters</code> - The pose of the robot as measured by the vision camera.</dd>
<dd><code>timestampSeconds</code> - The timestamp of the vision measurement in seconds. Note that if you don't use your
own time source by calling
<code>SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[])</code>, then you must use a timestamp with an epoch since FPGA
startup (i.e., the epoch of this timestamp is the same epoch as
<code>Timer.getFPGATimestamp()</code>). This means that you should
use <code>Timer.getFPGATimestamp()</code> as your time source in
this case.</dd>
<dd><code>visionMeasurementStdDevs</code> - Standard deviations of the vision pose measurement (x position in meters, y
position in meters, and heading in radians). Increase these numbers to trust the
vision pose measurement less.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="update(edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition[])">
<h3>update</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">edu.wpi.first.math.geometry.Pose2d</span>&nbsp;<span class="element-name">update</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroPitch,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroRoll,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions)</span></div>
<div class="block">Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>gyroAngle</code> - The current gyro angle.</dd>
<dd><code>gyroPitch</code> - The current gyro pitch.</dd>
<dd><code>gyroRoll</code> - The current gyro roll.</dd>
<dd><code>modulePositions</code> - The current distance measurements and rotations of the swerve modules.</dd>
<dt>Returns:</dt>
<dd>The estimated pose of the robot in meters.</dd>
</dl>
</section>
</li>
<li>
<section class="detail" id="updateWithTime(double,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.geometry.Rotation2d,edu.wpi.first.math.kinematics.SwerveModulePosition[])">
<h3>updateWithTime</h3>
<div class="member-signature"><span class="modifiers">public</span>&nbsp;<span class="return-type">edu.wpi.first.math.geometry.Pose2d</span>&nbsp;<span class="element-name">updateWithTime</span><wbr><span class="parameters">(double&nbsp;currentTimeSeconds,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroAngle,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroPitch,
edu.wpi.first.math.geometry.Rotation2d&nbsp;gyroRoll,
edu.wpi.first.math.kinematics.SwerveModulePosition[]&nbsp;modulePositions)</span></div>
<div class="block">Updates the pose estimator with wheel encoder and gyro information. This should be called every loop.</div>
<dl class="notes">
<dt>Parameters:</dt>
<dd><code>currentTimeSeconds</code> - Time at which this method was called, in seconds.</dd>
<dd><code>gyroAngle</code> - The current gyro angle.</dd>
<dd><code>gyroPitch</code> - The current gyro pitch.</dd>
<dd><code>gyroRoll</code> - The current gyro roll.</dd>
<dd><code>modulePositions</code> - The current distance measurements and rotations of the swerve modules.</dd>
<dt>Returns:</dt>
<dd>The estimated pose of the robot in meters.</dd>
</dl>
</section>
</li>
</ul>
</section>
</li>
</ul>
</section>
<!-- ========= END OF CLASS DATA ========= -->
</main>
</div>
</div>
</body>
</html>
Reference in New Issue View Git Blame Copy Permalink