mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-11 17:35:27 +00:00
106 lines
2.1 KiB
C++
106 lines
2.1 KiB
C++
// Copyright 2019, Collabora, Ltd.
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
/*!
|
|
* @file
|
|
* @brief Low-pass IIR filter on vectors
|
|
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
|
|
* @ingroup aux_tracking
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#ifndef __cplusplus
|
|
#error "This header is C++-only."
|
|
#endif
|
|
|
|
#include "tracking/t_lowpass.hpp"
|
|
|
|
#include <Eigen/Core>
|
|
|
|
|
|
namespace xrt_fusion {
|
|
|
|
/*!
|
|
* A very simple low-pass filter, using a "one-pole infinite impulse response"
|
|
* design (one-pole IIR).
|
|
*
|
|
* Configurable in dimension and scalar type.
|
|
*/
|
|
template <size_t Dim, typename Scalar> class LowPassIIRVectorFilter
|
|
{
|
|
public:
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
|
|
|
|
/*!
|
|
* Constructor
|
|
*
|
|
* @param cutoff_hz A cutoff frequency in Hertz: signal changes much
|
|
* lower in frequency will be passed through the filter, while signal
|
|
* changes much higher in frequency will be blocked.
|
|
*/
|
|
explicit LowPassIIRVectorFilter(Scalar cutoff_hz) noexcept
|
|
: impl_(cutoff_hz, Vector::Zero())
|
|
{}
|
|
|
|
|
|
/*!
|
|
* Reset the filter to just-created state.
|
|
*/
|
|
void
|
|
reset() noexcept
|
|
{
|
|
impl_.reset(Vector::Zero());
|
|
}
|
|
|
|
/*!
|
|
* Filter a sample, with an optional weight.
|
|
*
|
|
* @param sample The value to filter
|
|
* @param timestamp_ns The time that this sample was measured.
|
|
* @param weight An optional value between 0 and 1. The smaller this
|
|
* value, the less the current sample influences the filter state. For
|
|
* the first call, this is always assumed to be 1.
|
|
*/
|
|
void
|
|
addSample(Vector const &sample,
|
|
std::uint64_t timestamp_ns,
|
|
Scalar weight = 1)
|
|
{
|
|
impl_.addSample(sample, timestamp_ns, weight);
|
|
}
|
|
|
|
/*!
|
|
* Access the filtered value.
|
|
*/
|
|
Vector const &
|
|
getState() const noexcept
|
|
{
|
|
return impl_.state;
|
|
}
|
|
|
|
/*!
|
|
* Access the time of last update.
|
|
*/
|
|
std::uint64_t
|
|
getTimestampNs() const noexcept
|
|
{
|
|
return impl_.filter_timestamp_ns;
|
|
}
|
|
|
|
/*!
|
|
* Access whether we have initialized state.
|
|
*/
|
|
bool
|
|
isInitialized() const noexcept
|
|
{
|
|
return impl_.initialized;
|
|
}
|
|
|
|
private:
|
|
implementation::LowPassIIR<Vector, Scalar> impl_;
|
|
};
|
|
|
|
} // namespace xrt_fusion
|