1

我在 pixhawk 硬件上编写了一个模型预测控制器来控制四轴飞行器的姿态（角度）。我与 pixhawk 的一位开发人员交换了信息，他建议我使用单精度。我的代码是双精度的。

在此之前，我使用 Eigen C++ Library 测试了一个（双精度）数值问题，MatrixXd我的代码能够得到相同的答案。我将ldlt()Cholesky 求解器用于密集线性系统（所有其他求解器方法都导致错误答案）。

为了以单精度解决问题，我MatrixXd用MatrixXf和double替换了所有，但我无法得到相同的答案。float因此，我想了解为什么我在MatrixXd使用MatrixXf.

以下是相关部分。当我声明我的变量和矩阵时，最后的变量产生了它的解决方案，但是y当我使用时，我得到了所需的解决方案。-nan(ind); -nan(ind)MatrixXfMatrixXd1; 0.9999

// 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;

下面是 QPhild 函数：

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;
}

我知道主要问题来自上面的函数，因为我放了很多cout行来检查其中的各种矩阵的输出。他们开始于0然后去inf然后到nan。

编辑

现在我使用的是 LLT 分解而不是 LDLT（尽管它们都给了我相同的答案）。无论如何，我将 Matrix 的值以Edouble 和 float 形式发布，并带有相应的奇异值。

Matrix E在double：

1.84805e+12 1.65144e+12   7.557e+11 6.73531e+11 3.08645e+11 2.73966e+11 1.25821e+11 1.10981e+11
1.65144e+12 1.47576e+12 6.75306e+11 6.01881e+11 2.75811e+11 2.44823e+11 1.12436e+11 9.91757e+10
  7.557e+11 6.75306e+11  3.0902e+11  2.7542e+11 1.26211e+11  1.1203e+11 5.14507e+10 4.53824e+10
6.73531e+11 6.01881e+11  2.7542e+11 2.45475e+11 1.12488e+11 9.98504e+10 4.58567e+10 4.04488e+10
3.08645e+11 2.75811e+11 1.26211e+11 1.12488e+11 5.15474e+10  4.5756e+10 2.10137e+10 1.85354e+10
2.73966e+11 2.44823e+11  1.1203e+11 9.98504e+10  4.5756e+10 4.06159e+10 1.86529e+10 1.64534e+10
1.25821e+11 1.12436e+11 5.14507e+10 4.58567e+10 2.10137e+10 1.86529e+10 8.56643e+09  7.5562e+09
1.10981e+11 9.91757e+10 4.53824e+10 4.04488e+10 1.85354e+10 1.64534e+10  7.5562e+09 6.66534e+09

我不能将所有数字放在一行上，但行之间的空间用于区分不同的行。Ein 的奇异值double：

3.98569e+12 5.24887e+06  1363.09  174.56  166.311  159.098  58.9402  54.5173

Matrix E在float：

1.84805e+12 1.65144e+12   7.557e+11 6.73531e+11 3.08645e+11 2.73966e+11 1.25821e+11 1.10981e+11
1.65144e+12 1.47576e+12 6.75307e+11 6.01881e+11 2.75811e+11 2.44823e+11 1.12436e+11 9.91757e+10
  7.557e+11 6.75307e+11  3.0902e+11  2.7542e+11 1.26211e+11  1.1203e+11 5.14507e+10 4.53824e+10
6.73531e+11 6.01881e+11  2.7542e+11 2.45475e+11 1.12488e+11 9.98505e+10 4.58567e+10 4.04488e+10
3.08645e+11 2.75811e+11 1.26211e+11 1.12488e+11 5.15474e+10  4.5756e+10 2.10137e+10 1.85354e+10
2.73966e+11 2.44823e+11  1.1203e+11 9.98505e+10  4.5756e+10 4.06159e+10 1.86529e+10 1.64534e+10
1.25821e+11 1.12436e+11 5.14507e+10 4.58567e+10 2.10137e+10 1.86529e+10 8.56643e+09  7.5562e+09
1.10981e+11 9.91757e+10 4.53824e+10 4.04488e+10 1.85354e+10 1.64534e+10  7.5562e+09 6.66534e+09

float奇异值是：

3.98569e+12 5.08741e+06  62753.4  58133.2  26340.8  20529.1 15839.4 1050.96
4

0 回答 0