covar.h
Go to the documentation of this file.
00001 namespace Eigen { 
00002 
00003 namespace internal {
00004 
00005 template <typename Scalar>
00006 void covar(
00007         Matrix< Scalar, Dynamic, Dynamic > &r,
00008         const VectorXi &ipvt,
00009         Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
00010 {
00011     using std::abs;
00012     typedef DenseIndex Index;
00013 
00014     /* Local variables */
00015     Index i, j, k, l, ii, jj;
00016     bool sing;
00017     Scalar temp;
00018 
00019     /* Function Body */
00020     const Index n = r.cols();
00021     const Scalar tolr = tol * abs(r(0,0));
00022     Matrix< Scalar, Dynamic, 1 > wa(n);
00023     eigen_assert(ipvt.size()==n);
00024 
00025     /* form the inverse of r in the full upper triangle of r. */
00026     l = -1;
00027     for (k = 0; k < n; ++k)
00028         if (abs(r(k,k)) > tolr) {
00029             r(k,k) = 1. / r(k,k);
00030             for (j = 0; j <= k-1; ++j) {
00031                 temp = r(k,k) * r(j,k);
00032                 r(j,k) = 0.;
00033                 r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
00034             }
00035             l = k;
00036         }
00037 
00038     /* form the full upper triangle of the inverse of (r transpose)*r */
00039     /* in the full upper triangle of r. */
00040     for (k = 0; k <= l; ++k) {
00041         for (j = 0; j <= k-1; ++j)
00042             r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
00043         r.col(k).head(k+1) *= r(k,k);
00044     }
00045 
00046     /* form the full lower triangle of the covariance matrix */
00047     /* in the strict lower triangle of r and in wa. */
00048     for (j = 0; j < n; ++j) {
00049         jj = ipvt[j];
00050         sing = j > l;
00051         for (i = 0; i <= j; ++i) {
00052             if (sing)
00053                 r(i,j) = 0.;
00054             ii = ipvt[i];
00055             if (ii > jj)
00056                 r(ii,jj) = r(i,j);
00057             if (ii < jj)
00058                 r(jj,ii) = r(i,j);
00059         }
00060         wa[jj] = r(j,j);
00061     }
00062 
00063     /* symmetrize the covariance matrix in r. */
00064     r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
00065     r.diagonal() = wa;
00066 }
00067 
00068 } // end namespace internal
00069 
00070 } // end namespace Eigen


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:56