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.
// 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:
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.
EDIT
Right now I am using the LLT decomposition and not LDLT (although they both give me the same answers). Regardless, I am posting the values of Matrix E in both double and float, with the corresponding singular values.
Matrix E in 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
I cannot put all the numbers on a single line but the space between the rows is used to distinguish between different rows. Singular values of E in double:
3.98569e+12 5.24887e+06 1363.09 174.56 166.311 159.098 58.9402 54.5173
Matrix E in 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
And the float singular values are:
3.98569e+12 5.08741e+06 62753.4 58133.2 26340.8 20529.1 15839.4 1050.96
I have also faced teh same problem, we found out once issue, when we are converting from double to float, the rounding bring values are same sometimes. So solver results with nan. We fixed it by adding 1e-5 to differentiate the each Matrix node.
D SURESH BABU