I have coded a model predictive controller on a pixhawk hardware to control the attitude (angles) of the quadcopter. I exchanged messages with one of the developers of pixhawk and he advised me to use single precision. My code is in double precision.

Before this, I tested a numerical problem with MatrixXd (double precision) using Eigen C++ Library and my code was able to get the same answer. I used the `ldlt()` Cholesky solver for a dense linear systems (all the other solver methods resulted in the wrong answer).

I replaced all MatrixXd with MatrixXf and double with float in order to solve the problem with single precision but I was unable to get the same answer. So I want to get some insight as to why I am not getting the same answer achieved by MatrixXd when I use MatrixXf.

Below is the relevant section. The y variable at the end produces the -nan(ind); -nan(ind) as it's solution when I declare my variables and matrices with MatrixXf but when I use MatrixXd I get the desired solution of 1; 0.9999.

Code: Select all

   // Hildreth's Quadratic Programming Loop

   for (int i = 0; i < 3; i++)//i < r_cols - 1; i++)
   {

      MatrixXf F = -2 * (H.transpose())*(Rs*r.col(i) - P*Xf);

      MatrixXf d = dd + dupast*uin;

      MatrixXf DeltaU = QPhild(E, F, CC, d);

      MatrixXf DeltaU_1(4, 2);
      DeltaU_1 << DeltaU(0, 0), DeltaU(1, 0),
         DeltaU(2, 0), DeltaU(3, 0),
         DeltaU(4, 0), DeltaU(5, 0),
         DeltaU(6, 0), DeltaU(7, 0);

      MatrixXf deltau = DeltaU_1.row(0);

      MatrixXf deltau_tran = deltau.transpose();
      u = u + deltau_tran;

      // Process
      x.col(i + 1) = Ad*x.col(i) + Bd*u;
      y = Cd*x.col(i + 1) + dist.col(i);

      // Model
      xh.col(i + 1) = A*xh.col(i) + B*deltau_tran + L*(y - C*xh.col(i));
      yh = C*xh.col(i + 1);

      Xf << x.col(i + 1) - x.col(i),
         y;
     }

    cout << y << endl << endl;


Below is the QPhild function:

Code: Select all

    MatrixXf QPhild(MatrixXf E, MatrixXf F, MatrixXf CC, MatrixXf d)
    {
      MatrixXf CC_trans = CC.transpose();
      MatrixXf T = CC*(E.ldlt().solve(CC_trans));
      MatrixXf K = (CC*(E.ldlt().solve(F)) + d);

      int k_row = K.rows();
      int k_col = K.cols();

      MatrixXf lambda(k_row, k_col);
       lambda.setZero(k_row, k_col);

      MatrixXf al(0, 0);
      al.setConstant(10.0f);

      for (int km = 0; km < 40; km++)
      {
         MatrixXf lambda_p = lambda;

        // loop to determine lambda values for respective iterations
        for (int i = 0; i < k_row; i++)
        {
         MatrixXf t1 = T.row(i)*lambda;
   
         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.0f)   lambda(i, 0) = 0.0f;
         else lambda(i, 0) = la;
         }
         al = (lambda - lambda_p).transpose() * (lambda - lambda_p);

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

         if (all < tol) break;
      }

      MatrixXf DeltaU = -E.ldlt().solve(F) - (E.ldlt().solve(CC_trans))*lambda;

      return DeltaU;
    }

I know that the main problem is from the function above because I put various cout lines to check outputs of various matrices within it. They start off at 0 then go to inf then to nan.

I know that the post should be MCV but I have close to 600 lines of code and posting all that is obviously not an option. These are the crucial parts of the code, I do hope it helps some how. Because it is missing other function definitions and variable initialization which will make the post less MCV.

Please let me know if this makes more sense now or if you need more clarity from my end. Thank you

Hard to help without knowing the contents of the matrix E. If only LDLT is giving you a satisfactory result when using double, then this means there is something fishy about it. Since it seems to be small you could past it's content here (with full accuracy, both using double and float), and also print its singular-values: cout << E.jacobiSvd().singularValues().transpose() << endl;, again both in double and single precision.

btw, better factorize the matrix E only once:

Code: Select all

LDLT<MatrixXd> solver(E); // only once
if(solver.info()!=Success) // check if not OK
 ...

// then:
x = solver.solve(b);

// if E changes, update solver:
solver.compute(E);
if(solver.info()!=Success) // check if not OK
 ...


you can then replace LDLT by other solvers, like PartialPivLU, HouseholderQR, JacobiSVD, etc.

Let's continue there: https://stackoverflow.com/questions/45896354