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