Class KalmanFilterLatencyCompensator<S extends Num,​I extends Num,​O extends Num>

    • Method Detail

      • reset

        public void reset()
        Clears the observer snapshot buffer.
      • addObserverState

        public void addObserverState​(edu.wpi.first.math.estimator.KalmanTypeFilter<S,​I,​O> observer,
                                     Matrix<I,​N1> u,
                                     Matrix<O,​N1> localY,
                                     double timestampSeconds)
        Add past observer states to the observer snapshots list.
        Parameters:
        observer - The observer.
        u - The input at the timestamp.
        localY - The local output at the timestamp
        timestampSeconds - The timesnap of the state.
      • applyPastGlobalMeasurement

        public <R extends Num> void applyPastGlobalMeasurement​(Nat<R> rows,
                                                               edu.wpi.first.math.estimator.KalmanTypeFilter<S,​I,​O> observer,
                                                               double nominalDtSeconds,
                                                               Matrix<R,​N1> y,
                                                               BiConsumer<Matrix<I,​N1>,​Matrix<R,​N1>> globalMeasurementCorrect,
                                                               double timestampSeconds)
        Add past global measurements (such as from vision)to the estimator.
        Type Parameters:
        R - The rows in the global measurement vector.
        Parameters:
        rows - The rows in the global measurement vector.
        observer - The observer to apply the past global measurement.
        nominalDtSeconds - The nominal timestep.
        y - The measurement.
        globalMeasurementCorrect - The function take calls correct() on the observer.
        timestampSeconds - The timestamp of the measurement.