クワッドコプターの姿勢 (角度) を制御するために、pixhawk ハードウェアでモデル予測コントローラーをコーディングしました。pixhawk の開発者の 1 人とメッセージを交換したところ、彼は単精度を使用するようにアドバイスしてくれました。私のコードは倍精度です。
MatrixXdこの前に、 Eigen C++ ライブラリを使用して (倍精度)の数値問題をテストしたところ、私のコードは同じ答えを得ることができました。密な線形システムにはコレスキー ソルバーを使用しldlt()ました (他のすべてのソルバー メソッドでは間違った結果が得られました)。
単精度で問題を解決するためにすべて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。
編集
現在、LDLT ではなく LLT 分解を使用しています (ただし、どちらも同じ答えが得られます)。いずれにせよ、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
すべての数字を 1 行に並べることはできませんが、行間のスペースを使用して異なる行を区別しています。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