fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 15:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add several functions that are useful for coavriance matrices: (#114)

Browse Source 
* uncorrelateCovariance
	* uncorrelateCovarianceSetVariance
	* makeBlockSymmetric
	* makeRowColSymmetric
	* isBlockSymmetric
	* isRowColSymmetric
This commit is contained in:
kritz
2019-12-06 12:03:26 +01:00
committed by Julian Kent
parent 5cbcf6035a
commit 2f6398168d
2 changed files with 228 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/squareMatrix.cpp
+103
View File
@@ -38,7 +38,110 @@ int main()
TEST(isEqual(A_bottomright, bottomright_check));
TEST(isEqual(A_bottomright2, bottomright_check));
// test diagonal functions
float data_4x4[16] = {1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14,15, 16
};
SquareMatrix<float, 4> B(data_4x4);
B.uncorrelateCovariance<1>(1);
float data_B_check[16] = {1, 0, 3, 4,
0, 6, 0, 0,
9, 0, 11, 12,
13, 0,15, 16
};
SquareMatrix<float, 4> B_check(data_B_check);
TEST(isEqual(B, B_check))
SquareMatrix<float, 4> C(data_4x4);
C.uncorrelateCovariance<2>(1);
float data_C_check[16] = {1, 0, 0, 4,
0, 6, 0, 0,
0, 0, 11, 0,
13, 0,0, 16
};
SquareMatrix<float, 4> C_check(data_C_check);
TEST(isEqual(C, C_check))
SquareMatrix<float, 4> D(data_4x4);
D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20,21});
float data_D_check[16] = {20, 0, 0, 0,
0, 21, 0, 0,
0, 0, 11, 12,
0, 0,15, 16
};
SquareMatrix<float, 4> D_check(data_D_check);
TEST(isEqual(D, D_check))
SquareMatrix<float, 4> E(data_4x4);
E.uncorrelateCovarianceSetVariance<3>(1, 33);
float data_E_check[16] = {1, 0, 0, 0,
0, 33, 0, 0,
0, 0, 33, 0,
0, 0,0, 33
};
SquareMatrix<float, 4> E_check(data_E_check);
TEST(isEqual(E, E_check))
// test symmetric functions
SquareMatrix<float, 4> F(data_4x4);
F.makeBlockSymmetric<2>(1);
float data_F_check[16] = {1, 2, 3, 4,
5, 6, 8.5, 8,
9, 8.5, 11, 12,
13, 14,15, 16
};
SquareMatrix<float, 4> F_check(data_F_check);
TEST(isEqual(F, F_check))
TEST(F.isBlockSymmetric<2>(1));
TEST(!F.isRowColSymmetric<2>(1));
SquareMatrix<float, 4> G(data_4x4);
G.makeRowColSymmetric<2>(1);
float data_G_check[16] = {1, 3.5, 6, 4,
3.5, 6, 8.5, 11,
6, 8.5, 11, 13.5,
13, 11,13.5, 16
};
SquareMatrix<float, 4> G_check(data_G_check);
TEST(isEqual(G, G_check));
TEST(G.isBlockSymmetric<2>(1));
TEST(G.isRowColSymmetric<2>(1));
SquareMatrix<float, 4> H(data_4x4);
H.makeBlockSymmetric<1>(1);
float data_H_check[16] = {1, 2, 3, 4,
5, 6, 7, 8,
9, 10, 11, 12,
13, 14,15, 16
};
SquareMatrix<float, 4> H_check(data_H_check);
TEST(isEqual(H, H_check))
TEST(H.isBlockSymmetric<1>(1));
TEST(!H.isRowColSymmetric<1>(1));
SquareMatrix<float, 4> J(data_4x4);
J.makeRowColSymmetric<1>(1);
float data_J_check[16] = {1, 3.5, 3, 4,
3.5, 6, 8.5, 11,
9, 8.5, 11, 12,
13, 11,15, 16
};
SquareMatrix<float, 4> J_check(data_J_check);
TEST(isEqual(J, J_check));
TEST(J.isBlockSymmetric<1>(1));
TEST(J.isRowColSymmetric<1>(1));
TEST(!J.isBlockSymmetric<3>(1));
float data_K[16] = {1, 2, 3, 4,
2, 3, 4, 11,
3, 4, 11, 12,
4, 11,15, 16
};
SquareMatrix<float, 4> K(data_K);
TEST(!K.isRowColSymmetric<1>(2));
return 0;
}