mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
30 lines
1.3 KiB
C++
30 lines
1.3 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "frc/estimator/MecanumDrivePoseEstimator.h"
|
|
|
|
#include <wpi/timestamp.h>
|
|
|
|
#include "frc/StateSpaceUtil.h"
|
|
#include "frc/estimator/AngleStatistics.h"
|
|
#include "wpimath/MathShared.h"
|
|
|
|
using namespace frc;
|
|
|
|
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
|
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
|
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
|
|
: MecanumDrivePoseEstimator{kinematics, gyroAngle,
|
|
wheelPositions, initialPose,
|
|
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {}
|
|
|
|
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
|
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
|
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
|
|
const wpi::array<double, 3>& stateStdDevs,
|
|
const wpi::array<double, 3>& visionMeasurementStdDevs)
|
|
: PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
|
|
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
|
|
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
|