Drake
system_identification.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <set>
5 #include <stdexcept>
6 #include <string>
7 #include <tuple>
8 #include <utility>
9 #include <vector>
10 
13 #include "drake/common/trig_poly.h"
14 
15 namespace drake {
16 namespace solvers {
17 
18 /// Utility functions for system identification.
19 /**
20  * This class is a holder for templated utility methods. It should not be
21  * constructed. It must be template-instantiated (in its cpp file) for each
22  * supported variant of Polynomial (currently only Polynomial<double>).
23  *
24  * For the purposes of system identification we require here that the set of
25  * variables in a polynomial can be divided into two groups:
26  *
27  * * "Parameter variables" are unchanged through many evaluations of the
28  * Polynomial and so may be factored and renamed (lumped) at will. In
29  * effect parameter variables are treated as constants.
30  *
31  * * "Active variables" are those that may vary between evaluations of the
32  * Polynomial, for instance because they are inputs or state variables
33  * of the system.
34  *
35  * Note: The term "system identification" used throughout here refers to the
36  * process of simplifying the equations defining a physical system to a
37  * minimum number of "lumped" parameters and then estimating the values of
38  * those parameters based on empirical data.
39  */
40 template <typename CoefficientType>
42  public:
43  typedef ::Polynomial<CoefficientType> PolyType;
44  typedef typename PolyType::Monomial MonomialType;
45  typedef typename PolyType::Term TermType;
46  typedef typename PolyType::VarType VarType;
47  typedef std::map<PolyType, VarType> LumpingMapType;
48  typedef std::map<VarType, CoefficientType> PartialEvalType;
49 
50  /// Extract lumped parameters from a given polynomial.
51  /**
52  * Given a Polynomial, poly, and a set of variables of poly that should be
53  * treated as parameters (that is, variables eligible to be lumped), obtain
54  * all of the unique expressions by which combinations of the remaining
55  * active variables are multiplied to form the monomials of the Polynomial.
56  *
57  * For instance, if we have the polynomial:
58  * a*x + b*x + a*c*y + a*c*y**2
59  * And our parameters are a, b, and c, then our lumped parameters are:
60  * lump1 == a+b ; lump2 == a*c
61  * and we return:
62  * { (a + b) -> VarType("lump", 1); (a * c) -> VarType("lump", 2) }
63  *
64  * Note however that this function provides no guarantees of the lumped
65  * parameter names generated except that they are unique -- "lump1" and
66  * "lump2" here are examples.
67  */
68  static LumpingMapType GetLumpedParametersFromPolynomial(
69  const PolyType& poly,
70  const std::set<VarType>& parameter_vars);
71 
72  /// Same as GetLumpedParametersFromPolynomial but for multiple Polynomials.
73  /**
74  * It is preferrable to use this if you have multiple Polynomials as it
75  * saves you from having to union the resulting LumpingMapType results
76  * together.
77  */
78  static LumpingMapType GetLumpedParametersFromPolynomials(
79  const std::vector<PolyType>& polys,
80  const std::set<VarType>& parameter_vars);
81 
82  /// Rewrite a Polynomial in terms of lumped parameters.
83  /**
84  * For instance, if we have the polynomial:
85  * a*x + b*x + a*c*y + a*c*y**2
86  * And our lumped parameters are:
87  * lump1 == a+b ; lump2 == a*c
88  * And our polynomial is now:
89  * lump1*x + lump2*y + lump2*y**2
90  */
92  const PolyType& poly,
93  const LumpingMapType& lumped_parameters);
94 
95  /** Estimate some parameters of a polynomial based on empirical data.
96  *
97  * Given one or more polynomial equations P[i](a, b, ... x, y, ...) = 0, and
98  * measured values of some its arguments (x, y, ..., referred to as the
99  * "active variables"), estimate values for the remaining arguments (a, b,
100  * ..., referred to as the "parameters").
101  *
102  * Measured x, y, ... is provided in a list of maps, active_var_values.
103  *
104  * The return value is a pair, {estimates, error}, where:
105  * * estimates is a map of polynomial VarTypes (a, b, ...) to their
106  * estimated values, suitable as input for Polynomial::evaluatePartial.
107  * * error is the root-mean-square error of the estimates.
108  */
109  static std::pair<PartialEvalType, CoefficientType> EstimateParameters(
110  const VectorXPoly& polys,
111  const std::vector<PartialEvalType>& active_var_values);
112 
113  /** A helper struct to hold System ID results */
115  /// The lumped parameters that were used in system ID.
116  LumpingMapType lumped_parameters;
117 
118  /// The input polynomials, rewritten using the lumped parameters so that
119  /// only active variable and first-order lumped variable terms remain.
121 
122  /// The estimated value for each lumped parameter.
123  PartialEvalType lumped_parameter_values;
124 
125  /// The input polynomials, with all estimates substituted in so that
126  /// only active variables remain.
128 
129  /// The root-mean-square error of the estimation step.
130  CoefficientType rms_error;
131  };
132 
133  /** Performs full lumped-parameter identification of a system of TrigPolys.
134  *
135  * This is a convenience method meant to capture what is expected to be the
136  * most common usage pattern of system ID.
137  *
138  * Given one or more TrigPoly expressions to be set equal to zero, and a
139  * list of observed values for a subset of the variables, lumps up the
140  * remaining variables into lumped parameters and estimates a value for each
141  * of those. This is broadly equivalent to calling the following methods
142  * in sequence:
143  *
144  * * GetLumpedParametersFromPolynomials
145  * * RewritePolynomialWithLumpedParameters
146  * * EstimateParameters
147  */
149  const VectorXTrigPoly& polys,
150  const std::vector<PartialEvalType>& active_var_values);
151 
152 
153  private:
154  /// This class is not constructable.
155  SystemIdentification() = delete;
157 
158  /// Return every combination of the given variables in the polynomials.
159  /**
160  * For instance, if we want combinations of x and y in the polynomial
161  * a * x + b * x*y + b * y^2 + c * y^2,
162  * then return x, x*y, and y^2.
163  *
164  * NOTE: This will also return the empty combination iff there are terms for
165  * the polynomial that do not contain any of the requested variables. This
166  * behaviour is slightly surprising but in practice matches what we want to
167  * do with the combinations.
168  */
169  static std::set<MonomialType>
170  GetAllCombinationsOfVars(
171  const std::vector<PolyType>& polys,
172  const std::set<VarType>& vars);
173 
174  /// Test if one monomial is a product of parameters times another monomial.
175  /**
176  * Return true iff monomial "haystack" consists only of the monomial "needle"
177  * times variables not in "active_vars".
178  *
179  * For instance, with active vars x, y:
180  * haystack 3 * a * x * x * y matches needle x * x * y
181  * haystack x * x * y * y does not match needle x * x * y
182  */
183  static bool MonomialMatches(
184  const MonomialType& haystack,
185  const MonomialType& needle,
186  const std::set<VarType>& active_vars);
187 
188  /// Factor out the smallest coefficient in a Polynomial.
189  /**
190  * The resulting pair is the factored coefficient and a polynomial whose
191  * smallest coefficient is 1. This allows lumped parameter polynomials to
192  * be compared to determine when they differ only by coefficient.
193  *
194  * For instance, given the polynomial:
195  * 2 * x + 3 * y
196  * this will return
197  * {2, x + 1.5 * y}
198  */
199  static std::pair<CoefficientType, PolyType>
200  CanonicalizePolynomial(const PolyType& poly);
201 
202  /// Obtain a new variable ID not already in vars_in_use. The string part of
203  /// the variable's name will be prefix.
204  static VarType CreateUnusedVar(const std::string& prefix,
205  const std::set<VarType>& vars_in_use);
206 
207  /** Classify the variables in a group of polynomials and measurements.
208  *
209  * Given a system of polynomials and a set of variable values, return the
210  * sets of all the values, the parameter values (ie those with values not
211  * given) and the active values (those with values given).
212  *
213  * Currently only the second return (the parameter values) is used; the
214  * other returns have proven useful in previous versions of this code and
215  * are retained for that reason.
216  */
217  static std::tuple<const std::set<VarType>,
218  const std::set<VarType>,
219  const std::set<VarType>> ClassifyVars(
220  const std::vector<Polynomiald>& polys,
221  const std::vector<PartialEvalType>& active_var_values);
222 };
223 } // namespace solvers
224 } // namespace drake
static PolyType RewritePolynomialWithLumpedParameters(const PolyType &poly, const LumpingMapType &lumped_parameters)
Rewrite a Polynomial in terms of lumped parameters.
Definition: system_identification.cc:165
PartialEvalType lumped_parameter_values
The estimated value for each lumped parameter.
Definition: system_identification.h:123
static std::pair< PartialEvalType, CoefficientType > EstimateParameters(const VectorXPoly &polys, const std::vector< PartialEvalType > &active_var_values)
Estimate some parameters of a polynomial based on empirical data.
Definition: system_identification.cc:243
Eigen::Matrix< Polynomiald, Eigen::Dynamic, 1 > VectorXPoly
A column vector of polynomials; used in several optimization classes.
Definition: polynomial.h:482
Definition: automotive_demo.cc:88
std::map< VarType, CoefficientType > PartialEvalType
Definition: system_identification.h:48
STL namespace.
LumpingMapType lumped_parameters
The lumped parameters that were used in system ID.
Definition: system_identification.h:116
Utility functions for system identification.
Definition: system_identification.h:41
A scalar multi-variate polynomial, modeled after the msspoly in spotless.
Definition: polynomial.h:42
CoefficientType rms_error
The root-mean-square error of the estimation step.
Definition: system_identification.h:130
std::vector< double > vector
Definition: translator_test.cc:20
static LumpingMapType GetLumpedParametersFromPolynomials(const std::vector< PolyType > &polys, const std::set< VarType > &parameter_vars)
Same as GetLumpedParametersFromPolynomial but for multiple Polynomials.
Definition: system_identification.cc:87
PolyType::Term TermType
Definition: system_identification.h:45
::Polynomial< CoefficientType > PolyType
Definition: system_identification.h:43
A helper struct to hold System ID results.
Definition: system_identification.h:114
static LumpingMapType GetLumpedParametersFromPolynomial(const PolyType &poly, const std::set< VarType > &parameter_vars)
Extract lumped parameters from a given polynomial.
Definition: system_identification.cc:77
VectorXTrigPoly lumped_polys
The input polynomials, rewritten using the lumped parameters so that only active variable and first-o...
Definition: system_identification.h:120
const VectorXDecisionVariable * vars
Definition: nlopt_solver.cc:103
std::map< PolyType, VarType > LumpingMapType
Definition: system_identification.h:47
static SystemIdentificationResult LumpedSystemIdentification(const VectorXTrigPoly &polys, const std::vector< PartialEvalType > &active_var_values)
Performs full lumped-parameter identification of a system of TrigPolys.
Definition: system_identification.cc:332
Eigen::Matrix< TrigPolyd, Eigen::Dynamic, 1 > VectorXTrigPoly
A column vector of TrigPoly; used in several optimization classes.
Definition: trig_poly.h:463
PolyType::Monomial MonomialType
Definition: system_identification.h:44
#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
VectorXTrigPoly partially_evaluated_polys
The input polynomials, with all estimates substituted in so that only active variables remain...
Definition: system_identification.h:127
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
PolyType::VarType VarType
Definition: system_identification.h:46