diff --git a/src/main/java/com/team766/odometry/KalmanFilter.java b/src/main/java/com/team766/odometry/KalmanFilter.java index 5e49e11e..8c401dbd 100644 --- a/src/main/java/com/team766/odometry/KalmanFilter.java +++ b/src/main/java/com/team766/odometry/KalmanFilter.java @@ -21,64 +21,76 @@ public class KalmanFilter { private Matrix odometryCovariancePerDist; private Matrix visionCovariance; private TreeMap inputLog; + private double velocityInputDeletionTime; // in seconds - private Matrix observationMatrix = MatBuilder.fill(Nat.N2(), Nat.N4(), -1, 0, 0, 0, - 0, 1, 0, 0); + private static final Matrix CUR_STATE_DEFAULT = MatBuilder.fill(Nat.N4(), Nat.N1(), 0, 0, 0, 0); - private final double VELOCITY_INPUT_DELETION_TIME = 1; + private static final Matrix COVARIANCE_DEFAULT = Matrix.eye(Nat.N4()); + + private static final Matrix NOISE_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N4(), Nat.N4(), + 0.03, 0, 0, 0, + 0, 0.03, 0, 0, + 0, 0, 0.01, 0, + 0, 0, 0, 0.01); + + private static final Matrix ODOMETRY_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N2(), Nat.N2(), + 0.2, 0, + 0, 0.05); + + private static final Matrix VISION_COVARIANCE_DEFAULT = MatBuilder.fill(Nat.N2(), Nat.N2(), + 0.1, 0, + 0, 0.1); + + private static final Matrix OBSERVATION_MATRIX = MatBuilder.fill(Nat.N2(), Nat.N4(), + 1, 0, 0, 0, + 0, 1, 0, 0); - public KalmanFilter(Matrix curState, Matrix covariance, Matrix odometryCovariancePerDist, Matrix visionCovariance) { + private static final double VELOCITY_INPUT_DELETION_TIME_DEFAULT = 1; // in seconds + + public KalmanFilter(Matrix curState, Matrix covariance, Matrix odometryCovariancePerDist, Matrix visionCovariance, Matrix noiseCovariance, double velocityInputDeletionTime) { this.curState = curState; this.curCovariance = covariance; this.odometryCovariancePerDist = odometryCovariancePerDist; this.visionCovariance = visionCovariance; - noiseCovariance = MatBuilder.fill(Nat.N4(), Nat.N4(), - 0.0003, 0, 0, 0, - 0, 0.0003, 0, 0, - 0, 0, 0.0001, 0, - 0, 0, 0, 0.0001); + this.noiseCovariance = noiseCovariance; + this.velocityInputDeletionTime = velocityInputDeletionTime; inputLog = new TreeMap<>(); } - public KalmanFilter(Matrix curState, Matrix covariance, Matrix visionCovariance) { - this(curState, - covariance, - MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.05), - visionCovariance); + public KalmanFilter(Matrix curState, Matrix covariance, Matrix odometryCovariancePerDist, Matrix visionCovariance) { + this(curState, covariance, odometryCovariancePerDist, visionCovariance, NOISE_COVARIANCE_DEFAULT, VELOCITY_INPUT_DELETION_TIME_DEFAULT); } - public KalmanFilter(Matrix curState, Matrix covariance) { - this(curState, - covariance, - MatBuilder.fill(Nat.N2(), Nat.N2(), 0.05, 0, 0, 0.05)); + public KalmanFilter(Matrix odometryCovariancePerDist, Matrix visionCovariance) { + this(CUR_STATE_DEFAULT, COVARIANCE_DEFAULT, odometryCovariancePerDist, visionCovariance); } public KalmanFilter() { - this(MatBuilder.fill(Nat.N4(), Nat.N1(), 0, 0, 0, 0), Matrix.eye(Nat.N4())); + this(ODOMETRY_COVARIANCE_DEFAULT, VISION_COVARIANCE_DEFAULT); } public void addVelocityInput(Translation2d velocityInput, double time) { inputLog.put(time, velocityInput); predictCurrentState(inputLog.lowerKey(time)); - if(time - inputLog.firstKey() > VELOCITY_INPUT_DELETION_TIME) { + if(time - inputLog.firstKey() > velocityInputDeletionTime) { inputLog.remove(inputLog.firstKey()); // delete old velocityInput values } - SmartDashboard.putNumber("Cur input x velocity", velocityInput.getX()); - SmartDashboard.putNumber("Cur State x velocity", curState.get(2, 0)); - SmartDashboard.putNumber("Number of entries inputLog", inputLog.size()); - Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, "pos cov: " + getCovariance().toString()); - SmartDashboard.putString("Pos Covariance", "time: " + time + ", gain: " + getCovariance().toString()); - SmartDashboard.putString("Full Covariance", "time: " + time + ", gain: " + curCovariance); + + // SmartDashboard.putNumber("Cur input x velocity", velocityInput.getX()); + // SmartDashboard.putNumber("Cur State x velocity", curState.get(2, 0)); + // SmartDashboard.putNumber("Number of entries inputLog", inputLog.size()); + // Logger.get(Category.LOCALIZATION).logRaw(Severity.INFO, "pos cov: " + getCovariance().toString()); + // SmartDashboard.putString("Pos Covariance", "time: " + time + ", gain: " + getCovariance().toString()); + // SmartDashboard.putString("Full Covariance", "time: " + time + ", gain: " + curCovariance); } private void predict(double time, double nextStepTime, double dt) { Translation2d velocityChange; if (inputLog.containsKey(time)) { - velocityChange = inputLog.get(nextStepTime).minus(inputLog.get(time)); // scalar multiplied to account for decreased velocity change if input targetTime is between two input entries + velocityChange = inputLog.get(nextStepTime).minus(getVelocity()); } else { - velocityChange = inputLog.get(nextStepTime).minus(getVelocity()).times(dt/(nextStepTime - time)); + velocityChange = inputLog.get(nextStepTime).minus(getVelocity()).times(dt/(nextStepTime - time)); // scalar multiplied to account for decreased velocity change if input targetTime is between two input entries } Matrix transition = MatBuilder.fill(Nat.N4(), Nat.N4(), @@ -101,7 +113,7 @@ private void predict(double time, double nextStepTime, double dt) { * changes curState and curCovariance to what it was at targetTime through backcalculation * @param targetTime in seconds */ - public void findPrevState(double targetTime) { + private void findPrevState(double targetTime) { double time = inputLog.lastKey(); double prevTime; double dt; @@ -113,12 +125,9 @@ public void findPrevState(double targetTime) { predict(time, prevTime, dt); - // curState = transition.inv().times(curState.minus(input)); // can also do using inverse - // curCovariance = transition.inv().times(curCovariance.minus(noiseCovariance).times(transition.transpose().inv())); - time += dt; } catch (Exception e) { - Logger.get(Category.JAVA_EXCEPTION).logRaw(Severity.ERROR, "kalman filter inputTimes too short"); + Logger.get(Category.ODOMETRY).logRaw(Severity.ERROR, "inputLog does not go back far enough"); break; } } @@ -128,103 +137,88 @@ public void findPrevState(double targetTime) { * predicts the current state based on the input time of a previous state * @param initialTime in seconds */ - public void predictCurrentState(double initialTime) { + private void predictCurrentState(double initialTime) { double time = initialTime; - // try { - // time = inputLog.ceilingKey(initialTime); - // } catch (Exception e) { - // Logger.get(Category.JAVA_EXCEPTION).logRaw(Severity.WARNING, "no ceiling key"); - // time = inputLog.floorKey(initialTime); // for updates, the time is sometimes slightly after the most recent velocity is input - // } - double currentTime = inputLog.lastKey(); double nextTime; double dt; - double counter = 0; while (time < currentTime) { try { nextTime = inputLog.higherKey(time); - dt = nextTime - time; // going forward, the target time (currentTime) will always be a key exactly since it is defined that way + // going forward, the target time (currentTime) will always be a key exactly since it is defined as the last key + // that means that finding the minimum between current time and next time, similar to in findPrevState, is not necessary + dt = nextTime - time; predict(time, nextTime, dt); time += dt; - counter++; } catch (Exception e) { - e.printStackTrace(); - Logger.get(Category.JAVA_EXCEPTION).logRaw(Severity.ERROR, "no higher key, counter: " + counter + ", execption: " + e.toString()); - SmartDashboard.putString("predictCurrentState error", e.toString()); + Logger.get(Category.ODOMETRY).logRaw(Severity.ERROR, "no higher key"); break; } } - SmartDashboard.putNumber("predictCurrentState iterations", counter); } /** - * - * @param measurement - * @param measurementCovariance - * @param time in seconds + * Updates the esimated position using any sensor measurement returning an x and y position + * @param measurement the position measurement + * @param measurementCovariance the covariance matrix of this position measurement + * @param time the time that the measurement took place, in seconds */ - public void updateWithPositionMeasurement(Translation2d measurement, Matrix measurementCovariance, double time) { + private void updateWithPositionMeasurement(Translation2d measurement, Matrix measurementCovariance, double time) { findPrevState(time); - SmartDashboard.putNumber("prev X value", getPos().getX()); - SmartDashboard.putNumber("Prev state x velocity", curState.get(2, 0)); - SmartDashboard.putString("prev covariance", getCovariance().toString()); - Matrix kalmanGain = curCovariance.times(observationMatrix.transpose().times( - observationMatrix.times(curCovariance.times(observationMatrix.transpose())).plus(measurementCovariance).inv())); - // Matrix posKalmanGain = getCovariance().times(getCovariance().plus(measurementCovariance).inv()); - SmartDashboard.putString("Kalman Gain", "time: " + time + ", gain: " + kalmanGain.toString()); - - // curState.assignBlock(0, 0, posKalmanGain.times(measurement.toVector().minus(getPos().toVector())).plus(getPos().toVector())); // updates values for ONLY position - // curCovariance.assignBlock(0, 0, Matrix.eye(Nat.N2()).minus(posKalmanGain).times(getCovariance())); // updates values for ONLY position - - curState = kalmanGain.times(measurement.toVector().minus(observationMatrix.times(curState))).plus(curState); - curCovariance = Matrix.eye(Nat.N4()).minus(kalmanGain.times(observationMatrix)).times(curCovariance.times(Matrix.eye(Nat.N4()).minus(kalmanGain.times(observationMatrix)).transpose())).plus( + + // SmartDashboard.putNumber("prev X value", getPos().getX()); + // SmartDashboard.putNumber("Prev state x velocity", curState.get(2, 0)); + // SmartDashboard.putString("prev covariance", getCovariance().toString()); + + Matrix kalmanGain = curCovariance.times(OBSERVATION_MATRIX.transpose().times( + OBSERVATION_MATRIX.times(curCovariance.times(OBSERVATION_MATRIX.transpose())).plus(measurementCovariance).inv())); + + // SmartDashboard.putString("Kalman Gain", "time: " + time + ", gain: " + kalmanGain.toString()); + + curState = kalmanGain.times(measurement.toVector().minus(OBSERVATION_MATRIX.times(curState))).plus(curState); + curCovariance = Matrix.eye(Nat.N4()).minus(kalmanGain.times(OBSERVATION_MATRIX)).times(curCovariance.times(Matrix.eye(Nat.N4()).minus(kalmanGain.times(OBSERVATION_MATRIX)).transpose())).plus( kalmanGain.times(measurementCovariance.times(kalmanGain.transpose()))); - SmartDashboard.putNumber("Updated prev state x velocity", curState.get(2, 0)); + + // SmartDashboard.putNumber("Updated prev state x velocity", curState.get(2, 0)); predictCurrentState(time); - SmartDashboard.putNumber("Predicted Cur State x velocity", curState.get(2, 0)); + + // SmartDashboard.putNumber("Predicted Cur State x velocity", curState.get(2, 0)); } /** - * + * Updates the estimated position using a * @param measurement * @param time in seconds */ public void updateWithVisionMeasurement(Translation2d measurement, double time) { updateWithPositionMeasurement(measurement, visionCovariance, time); - // Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, "cov: " + getCovariance().toString()); - // SmartDashboard.putString("Covariance", "time: " + time + ", gain: " + getCovariance().toString()); } /** - * - * @param odometryInput - * @param initialTime in seconds - * @param finalTime in seconds + * Updates the estimated position using a change in position since the last update + * Assumes that this update is happening right after the previous velocity input is added and that all odometry calculations have negligible latency + * @param odometryInput change in position between the previous update and now */ - public void updateWithOdometry(Translation2d odometryInput, double initialTime, double finalTime) { - + public void updateWithOdometry(Translation2d odometryInput) { + double initialTime = inputLog.lowerKey(inputLog.lastKey()); + findPrevState(initialTime); Translation2d curPos = getPos().plus(odometryInput); predictCurrentState(initialTime); - // Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, odometryInput.toString()); - // Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, curPos.toString()); - double angleRad = odometryInput.getAngle().getRadians(); Matrix track = MatBuilder.fill(Nat.N2(), Nat.N2(), Math.cos(angleRad), -Math.sin(angleRad), Math.sin(angleRad), Math.cos(angleRad)); Matrix odomCovariance = track.times(odometryInput.getNorm()).times(odometryCovariancePerDist.times(track.transpose())).plus(getCovariance()); - // Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, "odom: " + odomCovariance.toString()); - Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, "cov: " + getCovariance().toString()); + // Logger.get(Category.ODOMETRY).logRaw(Severity.INFO, "cov: " + getCovariance().toString()); - updateWithPositionMeasurement(curPos, odomCovariance, finalTime); + updateWithPositionMeasurement(curPos, odomCovariance, inputLog.lastKey()); } public Translation2d getPos() { @@ -242,4 +236,4 @@ public Matrix getCovariance() { public void setPos(Translation2d pos) { curState.assignBlock(0, 0, pos.toVector()); } -} +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java index 77a7579d..6646aa61 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java +++ b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java @@ -513,13 +513,13 @@ public void runSim() { simPrevPoses.remove(simPrevPoses.firstKey()); // delete old values } - kalmanFilter.updateWithOdometry(swerveOdometry.predictCurrentPositionChange(), now - dt, now); + // kalmanFilter.updateWithOdometry(swerveOdometry.predictCurrentPositionChange(), now - dt, now); - if (Math.random() < 0.03) { - double delay = Math.random() * 0.5; + if (Math.random() < 0.5) { + double delay = 0.05; // Math.random() * 0.5; Pose2d prevPose = simPrevPoses.ceilingEntry(now - delay).getValue(); - double randX = prevPose.getX() + 0.2 * (Math.random() - 0.5); - double randY = prevPose.getY() + 0.2 * (Math.random() - 0.5); + double randX = prevPose.getX() + 0.1 * (Math.random() - 0.5); + double randY = prevPose.getY() + 0.1 * (Math.random() - 0.5); kalmanFilter.updateWithVisionMeasurement(new Translation2d(randX, randY), now - delay /* - 0.1 * Math.random() */); SmartDashboard.putNumber("sensor X measurement", randX); } diff --git a/src/main/java/com/team766/robot/common/procedures/FollowPath.java b/src/main/java/com/team766/robot/common/procedures/FollowPath.java index b9ef5bd9..953e3640 100644 --- a/src/main/java/com/team766/robot/common/procedures/FollowPath.java +++ b/src/main/java/com/team766/robot/common/procedures/FollowPath.java @@ -11,6 +11,8 @@ import com.team766.robot.common.mechanisms.SwerveDrive; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -23,6 +25,9 @@ public class FollowPath extends Procedure { private final Timer timer = new Timer(); private PathPlannerTrajectory generatedTrajectory; + private final StructPublisher pathPlannerPosePublisher = + NetworkTableInstance.getDefault().getStructTopic("PathPlannerTargetPose", Pose2d.struct).publish(); + public FollowPath( PathPlannerPath path, ReplanningConfig replanningConfig, @@ -108,6 +113,7 @@ public void run(Context context) { "input rotational velocity", targetSpeeds.omegaRadiansPerSecond); org.littletonrobotics.junction.Logger.recordOutput( "targetState", targetState.getTargetHolonomicPose()); + pathPlannerPosePublisher.set(targetState.getTargetHolonomicPose()); drive.controlRobotOriented(targetSpeeds); context.yield(); } diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 8ee06be0..9887121e 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -25,7 +25,7 @@ public class PathSequenceAuto extends Procedure { private final SwerveDrive drive; private final Pose2d initialPosition; private final PPHolonomicDriveController controller; - private VisionSpeakerHelper visionSpeakerHelper; + // private VisionSpeakerHelper visionSpeakerHelper; /** * Sequencer for using path following with other procedures @@ -37,7 +37,7 @@ public PathSequenceAuto(SwerveDrive drive, Pose2d initialPosition) { this.drive = drive; this.controller = createDriveController(drive); this.initialPosition = initialPosition; - visionSpeakerHelper = new VisionSpeakerHelper(drive); + // visionSpeakerHelper = new VisionSpeakerHelper(drive); } private PPHolonomicDriveController createDriveController(SwerveDrive drive) {