在 Eigen C++ 上使用单精度 (MatrixXf) 获取双精度 (MatrixXd) 答案

问题描述 投票：0回答：1

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

在此之前，我使用 Eigen C++ 库测试了 

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

我将所有 

MatrixXd
替换为 
MatrixXf
，并将 
double
替换为 
float
，以便以单精度解决问题，但我无法得到相同的答案。所以我想了解一下为什么当我使用 
MatrixXd
时我没有得到 
MatrixXf
所获得的相同答案。

以下是相关部分。最后的 

y
变量会生成 
-nan(ind); -nan(ind)
，因为当我用 
MatrixXf
声明变量和矩阵时，它是解决方案，但是当我使用 
MatrixXd
时，我得到了 
1; 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 

E
的值，以及相应的奇异值。

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

我无法将所有数字放在一行上，但行之间的空格用于区分不同的行。 

E
中
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
c++ eigen mpc
1个回答
0
投票

我也遇到了同样的问题，我们发现有一个问题，当我们从双精度转换为浮点数时，舍入带来的值有时是相同的。所以求解器结果为 nan。我们通过添加 1e-5 来区分每个 Matrix 节点来修复它。

D 苏雷什·巴布

最新问题
© www.soinside.com 2019 - 2024. All rights reserved.