Drake
linear_system.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
9 
10 namespace drake {
11 namespace systems {
12 
13 /// A discrete OR continuous linear system.
14 ///
15 /// If time_period>0.0, then the linear system will have the following discrete-
16 /// time state update:
17 /// @f[ x[n+1] = A x[n] + B u[n], @f]
18 ///
19 /// or if time_period==0.0, then the linear system will have the following
20 /// continuous-time state update:
21 /// @f[\dot{x} = A x + B u. @f]
22 ///
23 /// In both cases, the system will have the output:
24 /// @f[y = C x + D u, @f]
25 /// where `u` denotes the input vector, `x` denotes the state vector, and
26 /// `y` denotes the output vector.
27 ///
28 /// @tparam T The vector element type, which must be a valid Eigen scalar.
29 ///
30 /// Instantiated templates for the following kinds of T's are provided:
31 /// - double
32 /// - AutoDiffXd
33 /// - symbolic::Expression
34 ///
35 /// They are already available to link against in the containing library.
36 /// No other values for T are currently supported.
37 ///
38 /// @ingroup primitive_systems
39 ///
40 /// @see AffineSystem
41 /// @see MatrixGain
42 template <typename T>
43 class LinearSystem : public AffineSystem<T> {
44  public:
46 
47  /// Constructs a %LinearSystem with a fixed set of coefficient matrices `A`,
48  /// `B`,`C`, and `D`.
49  /// The coefficient matrices must obey the following dimensions:
50  /// | Matrix | Num Rows | Num Columns |
51  /// |:-------:|:-----------:|:-----------:|
52  /// | A | num states | num states |
53  /// | B | num states | num inputs |
54  /// | C | num outputs | num states |
55  /// | D | num outputs | num inputs |
56  ///
57  /// Subclasses must use the protected constructor, not this one.
58  LinearSystem(const Eigen::Ref<const Eigen::MatrixXd>& A,
59  const Eigen::Ref<const Eigen::MatrixXd>& B,
60  const Eigen::Ref<const Eigen::MatrixXd>& C,
61  const Eigen::Ref<const Eigen::MatrixXd>& D,
62  double time_period = 0.0);
63 
64  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
65  template <typename U>
66  explicit LinearSystem(const LinearSystem<U>&);
67 
68  /// Creates a unique pointer to LinearSystem<T> by decomposing @p dynamics and
69  /// @p outputs using @p state_vars and @p input_vars.
70  ///
71  /// @throws runtime_error if either @p dynamics or @p outputs is not linear in
72  /// @p state_vars and @p input_vars.
73  static std::unique_ptr<LinearSystem<T>> MakeLinearSystem(
74  const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics,
75  const Eigen::Ref<const VectorX<symbolic::Expression>>& output,
76  const Eigen::Ref<const VectorX<symbolic::Variable>>& state_vars,
77  const Eigen::Ref<const VectorX<symbolic::Variable>>& input_vars,
78  double time_period = 0.0);
79 
80  protected:
81  /// Constructor that specifies scalar-type conversion support.
82  /// @param converter scalar-type conversion support helper (i.e., AutoDiff,
83  /// etc.); pass a default-constructed object if such support is not desired.
84  /// See @ref system_scalar_conversion for detailed background and examples
85  /// related to scalar-type conversion support.
87  const Eigen::Ref<const Eigen::MatrixXd>& A,
88  const Eigen::Ref<const Eigen::MatrixXd>& B,
89  const Eigen::Ref<const Eigen::MatrixXd>& C,
90  const Eigen::Ref<const Eigen::MatrixXd>& D,
91  double time_period);
92 };
93 
94 /// Base class for a discrete or continuous linear time-varying (LTV) system.
95 ///
96 /// If `time_period > 0.0`, the system will have the following discrete-time
97 /// state update:
98 /// @f[ x(t+h) = A(t) x(t) + B(t) u(t), @f]
99 /// where `h` is the time_period. If `time_period == 0.0`, the system will have
100 /// the following continuous-time state update:
101 /// @f[ \dot{x}(t) = A(t) x(t) + B(t) u(t), @f]
102 ///
103 /// both with the output:
104 /// @f[ y(t) = C(t) x(t) + D(t) u(t). @f]
105 ///
106 /// @tparam T The vector element type, which must be a valid Eigen scalar.
107 ///
108 /// Instantiated templates for the following kinds of T's are provided:
109 /// - double
110 /// - AutoDiffXd
111 /// - symbolic::Expression
112 ///
113 /// They are already available to link against in the containing library.
114 /// No other values for T are currently supported.
115 ///
116 /// @ingroup primitive_systems
117 ///
118 template <typename T>
120  public:
122 
123  protected:
124  /// Constructor.
125  ///
126  /// @param converter scalar-type conversion support helper (i.e., AutoDiff,
127  /// etc.); pass a default-constructed object if such support is not desired.
128  /// See @ref system_scalar_conversion for detailed background and examples
129  /// related to scalar-type conversion support.
130  /// @param num_states size of the system's state vector
131  /// @param num_inputs size of the system's input vector
132  /// @param num_outputs size of the system's output vector
133  /// @param time_period discrete update period, or 0.0 to use continuous time
135  int num_states, int num_inputs, int num_outputs,
137  std::move(converter), num_states, num_inputs, num_outputs,
138  time_period) {}
139 
140  private:
141  // N.B. A linear system is simply a restricted form of an affine system with
142  // the affine terms set to zero. The following adds this restriction.
143  VectorX<T> f0(const T&) const final {
144  return VectorX<T>::Zero(this->num_states());
145  }
146  VectorX<T> y0(const T&) const final {
147  return VectorX<T>::Zero(this->num_outputs());
148  }
149 };
150 
151 /// Takes the first-order Taylor expansion of a System around a nominal
152 /// operating point (defined by the Context).
153 ///
154 /// @param system The system or subsystem to linearize.
155 /// @param context Defines the nominal operating point about which the system
156 /// should be linearized. See note below.
157 /// @param equilibrium_check_tolerance Specifies the tolerance on ensuring that
158 /// the derivative vector isZero at the nominal operating point. @default 1e-6.
159 /// @returns A LinearSystem that approximates the original system in the
160 /// vicinity of the operating point. See note below.
161 /// @throws std::runtime_error if the system the operating point is not an
162 /// equilibrium point of the system (within the specified tolerance)
163 ///
164 /// Note: The inputs in the Context must be connected, either to the
165 /// output of some upstream System within a Diagram (e.g., if system is a
166 /// reference to a subsystem in a Diagram), or to a constant value using, e.g.
167 /// context->FixInputPort(0,default_input);
168 ///
169 /// Note: The inputs, states, and outputs of the returned system are NOT the
170 /// same as the original system. Denote x0,u0 as the nominal state and input
171 /// defined by the Context, and y0 as the value of the output at (x0,u0),
172 /// then the created systems inputs are (u-u0), states are (x-x0), and
173 /// outputs are (y-y0).
174 ///
175 /// @ingroup primitive_systems
176 ///
177 std::unique_ptr<LinearSystem<double>> Linearize(
178  const System<double>& system, const Context<double>& context,
179  double equilibrium_check_tolerance = 1e-6);
180 
181 /// A first-order Taylor series approximation to a @p system in the neighborhood
182 /// of an arbitrary point. When Taylor-expanding a system at a non-equilibrium
183 /// point, it may be represented either of the form:
184 /// @f[ \dot{x} - \dot{x0} = A (x - x0) + B (u - u0), @f]
185 /// for continuous time, or
186 /// @f[ x[n+1] - x0[n+1] = A (x[n] - x0[n]) + B (u[n] - u0[n]), @f]
187 /// for discrete time. As above, we denote x0, u0 to be the nominal state and
188 /// input at the provided @p context. The system description is affine when the
189 /// terms @f$ \dot{x0} - A x0 - B u0 @f$ and @f$ x0[n+1] - A x0[n] - B u0[n] @f$
190 /// are nonzero.
191 ///
192 /// More precisely, let x be a state and u be an input. This function returns
193 /// an AffineSystem of the form:
194 /// @f[ \dot{x} = A x + B u + f0, @f] (CT)
195 /// @f[ x[n+1] = A x[n] + B u[n] + f0, @f] (DT)
196 /// where @f$ f0 = \dot{x0} - A x0 - B u0 @f$ (CT) and
197 /// @f$ f0 = x0[n+1] - A x[n] - B u[n] @f$ (DT).
198 ///
199 /// @param system The system or subsystem to linearize.
200 /// @param context Defines the nominal operating point about which the system
201 /// should be linearized.
202 /// @returns An AffineSystem at this linearization point.
203 ///
204 /// Note that x, u and y are in the same coordinate system as the original
205 /// @p system, since the terms involving x0, u0 reside in f0.
206 ///
207 /// @ingroup primitive_systems
208 ///
209 std::unique_ptr<AffineSystem<double>> FirstOrderTaylorApproximation(
210  const System<double>& system, const Context<double>& context);
211 
212 /// Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B].
213 /// @ingroup control_systems
214 Eigen::MatrixXd ControllabilityMatrix(const LinearSystem<double>& sys);
215 
216 /// Returns true iff the controllability matrix is full row rank.
217 /// @ingroup control_systems
218 bool IsControllable(const LinearSystem<double>& sys,
219  double threshold = Eigen::Default);
220 
221 /// Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
222 /// @ingroup estimator_systems
223 Eigen::MatrixXd ObservabilityMatrix(const LinearSystem<double>& sys);
224 
225 /// Returns true iff the observability matrix is full column rank.
226 /// @ingroup estimator_systems
227 bool IsObservable(const LinearSystem<double>& sys,
228  double threshold = Eigen::Default);
229 
230 } // namespace systems
231 } // namespace drake
int num_states() const
Definition: affine_system.h:57
int num_outputs() const
Definition: affine_system.h:59
Definition: automotive_demo.cc:88
const Eigen::MatrixXd & C() const
Definition: affine_system.h:182
STL namespace.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
double time_period() const
Definition: affine_system.h:56
bool IsControllable(const LinearSystem< double > &sys, double threshold)
Returns true iff the controllability matrix is full row rank.
Definition: linear_system.cc:302
static std::unique_ptr< LinearSystem< T > > MakeLinearSystem(const Eigen::Ref< const VectorX< symbolic::Expression >> &dynamics, const Eigen::Ref< const VectorX< symbolic::Expression >> &output, const Eigen::Ref< const VectorX< symbolic::Variable >> &state_vars, const Eigen::Ref< const VectorX< symbolic::Variable >> &input_vars, double time_period=0.0)
Creates a unique pointer to LinearSystem<T> by decomposing dynamics and outputs using state_vars and ...
Definition: linear_system.cc:59
A discrete OR continuous linear system.
Definition: linear_system.h:43
std::unique_ptr< AffineSystem< double > > FirstOrderTaylorApproximation(const System< double > &system, const Context< double > &context)
A first-order Taylor series approximation to a system in the neighborhood of an arbitrary point...
Definition: linear_system.cc:279
const Eigen::VectorXd & f0() const
Definition: affine_system.h:181
Base class for a discrete or continuous linear time-varying (LTV) system.
Definition: linear_system.h:119
const Eigen::VectorXd & y0() const
Definition: affine_system.h:184
Base class for a discrete- or continuous-time, time-varying affine system, with potentially time-vary...
Definition: affine_system.h:32
const Eigen::MatrixXd & D() const
Definition: affine_system.h:183
const Eigen::MatrixXd & B() const
Definition: affine_system.h:180
Eigen::MatrixXd ObservabilityMatrix(const LinearSystem< double > &sys)
Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
Definition: linear_system.cc:310
int num_inputs() const
Definition: affine_system.h:58
Eigen::MatrixXd ControllabilityMatrix(const LinearSystem< double > &sys)
Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B].
Definition: linear_system.cc:287
bool IsObservable(const LinearSystem< double > &sys, double threshold)
Returns true iff the observability matrix is full column rank.
Definition: linear_system.cc:325
const Eigen::MatrixXd & A() const
Definition: affine_system.h:179
Provides public header files of Drake&#39;s symbolic library.
Helper class to convert a System<U> into a System<T>, intended for internal use by the System framewo...
Definition: system_scalar_converter.h:35
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
LinearSystem(const LinearSystem &)=delete
std::unique_ptr< LinearSystem< double > > Linearize(const System< double > &system, const Context< double > &context, double equilibrium_check_tolerance)
Takes the first-order Taylor expansion of a System around a nominal operating point (defined by the C...
Definition: linear_system.cc:268
A discrete OR continuous affine system (with constant coefficients).
Definition: affine_system.h:134
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
TimeVaryingLinearSystem(SystemScalarConverter converter, int num_states, int num_inputs, int num_outputs, double time_period)
Constructor.
Definition: linear_system.h:134