Hello guys,

So I know how to program in C++ but it was only this year that I had to consider tricks in order
to optimize my code performance. The other little problems I coded were not as intensive as
programming an attitude controller for a quadcopter.

As you can imagine, the associated computations have to be fast. I did some profile performance analysis in
Visual Studio and it pointed out that one my functions was being called 17,000+ times . That definitely cannot be
good for the microcontroller.

The function is included below, it is run in a do-while loop that iterates 50 times; I opted for do-while instead a for loop as
I saw somewhere that it is more performance friendly.

Code: Select all


Matrix<float, 12, 1> QPhild(const Matrix<float, 12, 12> &E, Matrix<float, 12, 1> &F, const Matrix<float, 48, 12> &CC, Matrix<float, 48, 1> &d)
{
   const LLT<MatrixXf> lltOfE(E); // Compute decomposition of E
   // const ColPivHouseholderQR<MatrixXf> ColofE(E);
   // FIND WAY TO CONVERT LLTOFE TO FIXED MATRIX

   static Matrix<float, 12, 48> CC_transd = CC.transpose();
   static Matrix<float, 48, 48> T = CC*(lltOfE.solve(CC_transd));
       Matrix<float, 48, 1> K = (CC*(lltOfE.solve(F)) + d);

//   Matrix<float, 48,48> T = CC*(ColofE.solve(CC_transd));
//   Matrix<float, 48, 1> K = (CC*(ColofE.solve(F)) + d);

   int k_row = 48;
   int k_col = 1;

   Matrix<float, 48, 1> lambda;
   lambda.setZero(48, 1);

   Matrix<float, 1, 1> al;
   al.setConstant(3.0f);

   for (int km = 0; km < 15; km++)
   {
      Matrix<float, 48, 1> lambda_p = lambda;

      // loop to determine lambda values for respective iterations
      for (int i = 0; i < k_row; i++)
      {
         Matrix<float, 1, 1 > t1 = T.row(i)*lambda;

         //      double t2 = T(i, i)*lambda(i, 0);
         float t2 = T(i, i)*lambda(i, 0);

         float w = t1(0, 0) - t2;

         w = w + K(i, 0);

         float la = -w / T(i, i);

         if (la < 0.0)   lambda(i, 0) = 0.0;
         else lambda(i, 0) = la;
      }

      al = (lambda - lambda_p).transpose() * (lambda - lambda_p);

      float all = al(0, 0);
      float tol = 0.000001f;

      if (all < tol) break;
   }

   Matrix<float,12,1> DeltU = -lltOfE.solve(F) - (lltOfE.solve(CC_transd))*lambda;

   // Matrix<float,12,1> DeltU = -ColofE.solve(F) - (ColofE.solve(CC_transd))*lambda;
   return DeltU;
}



I have read a lot of pages and watched a lot of videos. They have helped with some improved performance but I figured that
looking at what I have done might be best. I tremendously appreciate any advice you might have about this or coding styles in general
. Time is not on my side, deadline
coming up

PS, this is the function name from visual studio (I can't make sense of it) and some other details:

Code: Select all

Eigen::DenseBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<float,float>,Eigen::Transpose<Eigen::Block<Eigen::Matrix<float,48,48,0,48,48>,1,48,0> const > const ,Eigen::Matrix<float,48,1,0,48,1> const > >::redux<Eigen::internal::scalar_sum_op<float,float> >   

17,328 (Number of calls)   

3.22 (Elapsed Inclusive Time %)

3.22 (Elapsed Exclusive Time %)


Thank you!!!

Hi, some advices:

Code: Select all

// Use static allocation:
const LLT<MatrixXf> lltOfE(E);   -> LLT<Matrix<float,12,12> > lltOfE(E);
// or create a LLT<MatrixXf> object only once, and call .compute(E) to update it at each call.


I don't get the static declarations, they will be computed only at the first call, is it really on purpose?
Otherwise you can optimize T:

Code: Select all

T = CC*(lltOfE.solve(CC_transd))
->
CC_transd = lltOfE.matrixL().solve(CC_transd);
T = CC_transd.transpose()*CC_transd;


For vectors, better call v(i) instead of v(i,0).

Don't use 1x1 matrices, and since T is symmetric better use its columns (sequential access):

Code: Select all

float al = 3.f;
float t1 = T.col(i).transpose()*lambda;
             // or T.col(i).dot(lambda);


For the loop you could try:

Code: Select all

float Tii=T(i,i);
T(i,i) = 0;
float la = -(T.col(i).dot(lambda) + K(i)) / Tii;
T(i,i) = Tii;
if (la < 0.0)   lambda(i) = 0.f;
else lambda(i) = la;


To compute al:

Code: Select all

al = (lambda - lambda_p).squaredNorm();