Companion.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2010 Manuel Yguel <manuel.yguel@gmail.com>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_COMPANION_H
11 #define EIGEN_COMPANION_H
12 
13 // This file requires the user to include
14 // * Eigen/Core
15 // * Eigen/src/PolynomialSolver.h
16 
17 namespace Eigen {
18 
19 namespace internal {
20 
21 #ifndef EIGEN_PARSED_BY_DOXYGEN
22 
23 template <typename T>
24 T radix(){ return 2; }
25 
26 template <typename T>
27 T radix2(){ return radix<T>()*radix<T>(); }
28 
29 template<int Size>
31 {
32  enum {
33  ret = (Size == Dynamic) ? Dynamic : Size-1 };
34 };
35 
36 #endif
37 
38 template< typename _Scalar, int _Deg >
39 class companion
40 {
41  public:
43 
44  enum {
45  Deg = _Deg,
47  };
48 
49  typedef _Scalar Scalar;
52  //typedef DiagonalMatrix< Scalar, Deg_1, Deg_1 > BottomLeftDiagonal;
54 
59 
60  typedef DenseIndex Index;
61 
62  public:
63  EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col ) const
64  {
65  if( m_bl_diag.rows() > col )
66  {
67  if( 0 < row ){ return m_bl_diag[col]; }
68  else{ return 0; }
69  }
70  else{ return m_monic[row]; }
71  }
72 
73  public:
74  template<typename VectorType>
75  void setPolynomial( const VectorType& poly )
76  {
77  const Index deg = poly.size()-1;
78  m_monic = -1/poly[deg] * poly.head(deg);
79  //m_bl_diag.setIdentity( deg-1 );
80  m_bl_diag.setOnes(deg-1);
81  }
82 
83  template<typename VectorType>
84  companion( const VectorType& poly ){
85  setPolynomial( poly ); }
86 
87  public:
88  DenseCompanionMatrixType denseMatrix() const
89  {
90  const Index deg = m_monic.size();
91  const Index deg_1 = deg-1;
92  DenseCompanionMatrixType companion(deg,deg);
93  companion <<
94  ( LeftBlock(deg,deg_1)
95  << LeftBlockFirstRow::Zero(1,deg_1),
96  BottomLeftBlock::Identity(deg-1,deg-1)*m_bl_diag.asDiagonal() ).finished()
97  , m_monic;
98  return companion;
99  }
100 
101 
102 
103  protected:
110  bool balanced( Scalar colNorm, Scalar rowNorm,
111  bool& isBalanced, Scalar& colB, Scalar& rowB );
112 
119  bool balancedR( Scalar colNorm, Scalar rowNorm,
120  bool& isBalanced, Scalar& colB, Scalar& rowB );
121 
122  public:
131  void balance();
132 
133  protected:
134  RightColumn m_monic;
135  BottomLeftDiagonal m_bl_diag;
136 };
137 
138 
139 
140 template< typename _Scalar, int _Deg >
141 inline
143  bool& isBalanced, Scalar& colB, Scalar& rowB )
144 {
145  if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
146  else
147  {
148  //To find the balancing coefficients, if the radix is 2,
149  //one finds \f$ \sigma \f$ such that
150  // \f$ 2^{2\sigma-1} < rowNorm / colNorm \le 2^{2\sigma+1} \f$
151  // then the balancing coefficient for the row is \f$ 1/2^{\sigma} \f$
152  // and the balancing coefficient for the column is \f$ 2^{\sigma} \f$
153  rowB = rowNorm / radix<Scalar>();
154  colB = Scalar(1);
155  const Scalar s = colNorm + rowNorm;
156 
157  while (colNorm < rowB)
158  {
159  colB *= radix<Scalar>();
160  colNorm *= radix2<Scalar>();
161  }
162 
163  rowB = rowNorm * radix<Scalar>();
164 
165  while (colNorm >= rowB)
166  {
167  colB /= radix<Scalar>();
168  colNorm /= radix2<Scalar>();
169  }
170 
171  //This line is used to avoid insubstantial balancing
172  if ((rowNorm + colNorm) < Scalar(0.95) * s * colB)
173  {
174  isBalanced = false;
175  rowB = Scalar(1) / colB;
176  return false;
177  }
178  else{
179  return true; }
180  }
181 }
182 
183 template< typename _Scalar, int _Deg >
184 inline
186  bool& isBalanced, Scalar& colB, Scalar& rowB )
187 {
188  if( Scalar(0) == colNorm || Scalar(0) == rowNorm ){ return true; }
189  else
190  {
195  const _Scalar q = colNorm/rowNorm;
196  if( !isApprox( q, _Scalar(1) ) )
197  {
198  rowB = sqrt( colNorm/rowNorm );
199  colB = Scalar(1)/rowB;
200 
201  isBalanced = false;
202  return false;
203  }
204  else{
205  return true; }
206  }
207 }
208 
209 
210 template< typename _Scalar, int _Deg >
212 {
213  using std::abs;
214  EIGEN_STATIC_ASSERT( Deg == Dynamic || 1 < Deg, YOU_MADE_A_PROGRAMMING_MISTAKE );
215  const Index deg = m_monic.size();
216  const Index deg_1 = deg-1;
217 
218  bool hasConverged=false;
219  while( !hasConverged )
220  {
221  hasConverged = true;
222  Scalar colNorm,rowNorm;
223  Scalar colB,rowB;
224 
225  //First row, first column excluding the diagonal
226  //==============================================
227  colNorm = abs(m_bl_diag[0]);
228  rowNorm = abs(m_monic[0]);
229 
230  //Compute balancing of the row and the column
231  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
232  {
233  m_bl_diag[0] *= colB;
234  m_monic[0] *= rowB;
235  }
236 
237  //Middle rows and columns excluding the diagonal
238  //==============================================
239  for( Index i=1; i<deg_1; ++i )
240  {
241  // column norm, excluding the diagonal
242  colNorm = abs(m_bl_diag[i]);
243 
244  // row norm, excluding the diagonal
245  rowNorm = abs(m_bl_diag[i-1]) + abs(m_monic[i]);
246 
247  //Compute balancing of the row and the column
248  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
249  {
250  m_bl_diag[i] *= colB;
251  m_bl_diag[i-1] *= rowB;
252  m_monic[i] *= rowB;
253  }
254  }
255 
256  //Last row, last column excluding the diagonal
257  //============================================
258  const Index ebl = m_bl_diag.size()-1;
259  VectorBlock<RightColumn,Deg_1> headMonic( m_monic, 0, deg_1 );
260  colNorm = headMonic.array().abs().sum();
261  rowNorm = abs( m_bl_diag[ebl] );
262 
263  //Compute balancing of the row and the column
264  if( !balanced( colNorm, rowNorm, hasConverged, colB, rowB ) )
265  {
266  headMonic *= colB;
267  m_bl_diag[ebl] *= rowB;
268  }
269  }
270 }
271 
272 } // end namespace internal
273 
274 } // end namespace Eigen
275 
276 #endif // EIGEN_COMPANION_H
NumTraits< Scalar >::Real RealScalar
Definition: Companion.h:50
IntermediateState sqrt(const Expression &arg)
#define EIGEN_STRONG_INLINE
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
Matrix< Scalar, 1, Deg_1 > LeftBlockFirstRow
Definition: Companion.h:58
BottomLeftDiagonal m_bl_diag
Definition: Companion.h:135
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
Definition: StaticAssert.h:111
EIGEN_STRONG_INLINE const _Scalar operator()(Index row, Index col) const
Definition: Companion.h:63
void setPolynomial(const VectorType &poly)
Definition: Companion.h:75
Matrix< Scalar, Deg_1, 1 > BottomLeftDiagonal
Definition: Companion.h:53
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
Expression of a fixed-size or dynamic-size sub-vector.
bool balanced(Scalar colNorm, Scalar rowNorm, bool &isBalanced, Scalar &colB, Scalar &rowB)
Definition: Companion.h:142
DenseCompanionMatrixType denseMatrix() const
Definition: Companion.h:88
bool isApprox(const Scalar &x, const Scalar &y, typename NumTraits< Scalar >::Real precision=NumTraits< Scalar >::dummy_precision())
Matrix< Scalar, Deg, 1 > RightColumn
Definition: Companion.h:51
companion(const VectorType &poly)
Definition: Companion.h:84
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
RowXpr row(Index i)
Definition: BlockMethods.h:725
bool balancedR(Scalar colNorm, Scalar rowNorm, bool &isBalanced, Scalar &colB, Scalar &rowB)
Definition: Companion.h:185
Matrix< Scalar, Deg_1, Deg_1 > BottomLeftBlock
Definition: Companion.h:57
ColXpr col(Index i)
Definition: BlockMethods.h:708
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar, Size)
Matrix< Scalar, Deg, Deg > DenseCompanionMatrixType
Definition: Companion.h:55
Matrix< Scalar, _Deg, Deg_1 > LeftBlock
Definition: Companion.h:56


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:30