fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 17:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rework rank-detection tolerance in pseudoinverse

Browse Source
This commit is contained in:
Julian Kent
2020-11-04 13:08:23 +01:00
committed by Julian Kent
parent d9a5e3dec2
commit 15e54ceda1
2 changed files with 20 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/pseudoInverse.cpp
+15 -17
View File
@@ -127,28 +127,26 @@ int main()
TEST((retM1 - retM_check).abs().max() < 1e-5);
TEST((retN1 - retN_check).abs().max() < 1e-5);
float float_scale = 1.f;
fullRankCholeskyTolerance(float_scale);
double double_scale = 1.;
fullRankCholeskyTolerance(double_scale);
TEST(static_cast<double>(float_scale) > double_scale);
// Real-world test case
const float real_alloc[5][6] = {{ 0.794079, 0.794079, 0.794079, 0.794079, 0.0000, 0.0000},
{ 0.607814, 0.607814, 0.607814, 0.607814, 1.0000, 1.0000},
{-0.672516, 0.915642, -0.915642, 0.672516, 0.0000, 0.0000},
{ 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500},
{ 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000}};
const float real_alloc[5][6] = {
{ 0.794079, 0.794079, 0.794079, 0.794079, 0.0000, 0.0000},
{ 0.607814, 0.607814, 0.607814, 0.607814, 1.0000, 1.0000},
{-0.672516, 0.915642, -0.915642, 0.672516, 0.0000, 0.0000},
{ 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500},
{ 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000}
};
Matrix<float, 5, 6> real ( real_alloc);
Matrix<float, 6, 5> real_pinv = geninv(real);
// from SVD-based inverse
const float real_pinv_expected_alloc[6][5] = {{ 2.096205, -2.722267, 2.056547, 1.503279, 3.098087},
{ 1.612621, -1.992694, 2.056547, 1.131090, 2.275467},
{-1.062688, 2.043479, -2.056547, -0.927950, -2.275467},
{-1.546273, 2.773052, -2.056547, -1.300139, -3.098087},
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000},
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000}};
const float real_pinv_expected_alloc[6][5] = {
{ 2.096205, -2.722267, 2.056547, 1.503279, 3.098087},
{ 1.612621, -1.992694, 2.056547, 1.131090, 2.275467},
{-1.062688, 2.043479, -2.056547, -0.927950, -2.275467},
{-1.546273, 2.773052, -2.056547, -1.300139, -3.098087},
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000},
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000}
};
Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);