covar.h
Go to the documentation of this file.
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
6 void covar(
8  const VectorXi &ipvt,
9  Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
10 {
11  using std::abs;
12  typedef DenseIndex Index;
13 
14  /* Local variables */
15  Index i, j, k, l, ii, jj;
16  bool sing;
17  Scalar temp;
18 
19  /* Function Body */
20  const Index n = r.cols();
21  const Scalar tolr = tol * abs(r(0,0));
23  eigen_assert(ipvt.size()==n);
24 
25  /* form the inverse of r in the full upper triangle of r. */
26  l = -1;
27  for (k = 0; k < n; ++k)
28  if (abs(r(k,k)) > tolr) {
29  r(k,k) = 1. / r(k,k);
30  for (j = 0; j <= k-1; ++j) {
31  temp = r(k,k) * r(j,k);
32  r(j,k) = 0.;
33  r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
34  }
35  l = k;
36  }
37 
38  /* form the full upper triangle of the inverse of (r transpose)*r */
39  /* in the full upper triangle of r. */
40  for (k = 0; k <= l; ++k) {
41  for (j = 0; j <= k-1; ++j)
42  r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
43  r.col(k).head(k+1) *= r(k,k);
44  }
45 
46  /* form the full lower triangle of the covariance matrix */
47  /* in the strict lower triangle of r and in wa. */
48  for (j = 0; j < n; ++j) {
49  jj = ipvt[j];
50  sing = j > l;
51  for (i = 0; i <= j; ++i) {
52  if (sing)
53  r(i,j) = 0.;
54  ii = ipvt[i];
55  if (ii > jj)
56  r(ii,jj) = r(i,j);
57  if (ii < jj)
58  r(jj,ii) = r(i,j);
59  }
60  wa[jj] = r(j,j);
61  }
62 
63  /* symmetrize the covariance matrix in r. */
64  r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
65  r.diagonal() = wa;
66 }
67 
68 } // end namespace internal
69 
70 } // end namespace Eigen
IntermediateState sqrt(const Expression &arg)
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
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
void covar(Matrix< Scalar, Dynamic, Dynamic > &r, const VectorXi &ipvt, Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
Definition: covar.h:6
EIGEN_STRONG_INLINE Index cols() const
#define eigen_assert(x)


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