-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
first_order_low_pass_filter.h
97 lines (84 loc) · 3.81 KB
/
first_order_low_pass_filter.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#pragma once
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/systems/framework/vector_system.h"
namespace drake {
namespace systems {
/// An element-wise first order low pass filter system that filters the i-th
/// input uᵢ into the i-th output zᵢ. This system has one continuous state per
/// filtered input signal. Therefore, the i-th state of the system zᵢ evolves
/// according to: <pre>
/// żᵢ = -1/τᵢ (zᵢ - uᵢ)
/// </pre>
/// where τᵢ is the time constant of the i-th filter.
/// The i-th output of the system is given by: <pre>
/// yᵢ = zᵢ
/// </pre>
///
/// The transfer function for the i-th filter corresponds to: <pre>
/// H(s) = 1 / (1 + τᵢ s)
/// </pre>
///
/// The Bode plot for the i-th filter exhibits a cutoff frequency (angular
/// frequency) at 1/τᵢ and a gain of one. For frequencies higher than the cutoff
/// frequency, the Bode plot approaches a 20 dB per decade negative slope.
/// The Bode plot in phase exhibits a -90 degrees shift (lag) for frequencies
/// much larger than the cutoff frequency and a zero shift for low frequencies.
///
/// @system
/// name: FirstOrderLowPassFilter
/// input_ports:
/// - u0
/// output_ports:
/// - y0
/// @endsystem
///
/// @tparam_default_scalar
/// @ingroup primitive_systems
template <typename T>
class FirstOrderLowPassFilter final : public VectorSystem<T> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FirstOrderLowPassFilter)
/// Constructs a %FirstOrderLowPassFilter system that filters all input
/// signals with the same time constant, i.e. τᵢ = τ, ∀ i.
///
/// @param[in] time_constant the time constant τ of the filter.
/// It must be a positive number.
/// @param[in] size number of elements in the signal to be processed.
explicit FirstOrderLowPassFilter(double time_constant, int size = 1);
/// Constructs a %FirstOrderLowPassFilter so that the i-th component of the
/// input signal vector is low pass filtered with a time constant given in the
/// i-th component τᵢ of the input `time_constants` vector.
///
/// @param[in] time_constants Vector of time constants. Each entry in this
/// vector must be positive.
explicit FirstOrderLowPassFilter(const VectorX<double>& time_constants);
/// Scalar-converting copy constructor. See @ref system_scalar_conversion.
template <typename U>
explicit FirstOrderLowPassFilter(const FirstOrderLowPassFilter<U>&);
/// Returns the time constant of the filter for filters that have the same
/// time constant τ for all signals.
/// This method aborts if called on filters if with different time constants
/// per input signal. @see get_time_constants_vector().
double get_time_constant() const;
/// Returns the vector of time constants for `this` filter.
const VectorX<double>& get_time_constants_vector() const;
/// Sets the initial conditions on the output value of the filtered signal.
/// @param[in] context The current system context.
/// @param[in] z0 The vector on initial conditions on the output value.
void set_initial_output_value(Context<T>* context,
const Eigen::Ref<const VectorX<T>>& z0) const;
private:
void DoCalcVectorTimeDerivatives(
const Context<T>& context,
const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* derivatives) const final;
void DoCalcVectorOutput(const Context<T>& context,
const Eigen::VectorBlock<const VectorX<T>>& input,
const Eigen::VectorBlock<const VectorX<T>>& state,
Eigen::VectorBlock<VectorX<T>>* output) const final;
const VectorX<double> time_constants_;
};
} // namespace systems
} // namespace drake