mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 13:02:25 +08:00
Add several functions that are useful for coavriance matrices: (#114)
* uncorrelateCovariance * uncorrelateCovarianceSetVariance * makeBlockSymmetric * makeRowColSymmetric * isBlockSymmetric * isRowColSymmetric
This commit is contained in:
kritz
Julian Kent
@@ -61,6 +61,18 @@ public:
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
const Slice<Type, P, Q, M, M> slice(size_t x0, size_t y0) const
|
||||
{
|
||||
return Slice<Type, P, Q, M, M>(x0, y0, this);
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Slice<Type, P, Q, M, M> slice(size_t x0, size_t y0)
|
||||
{
|
||||
return Slice<Type, P, Q, M, M>(x0, y0, this);
|
||||
}
|
||||
|
||||
// inverse alias
|
||||
inline SquareMatrix<Type, M> I() const
|
||||
{
|
||||
@@ -118,6 +130,119 @@ public:
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
// zero all offdiagonal elements and keep corresponding diagonal elements
|
||||
template <size_t Width>
|
||||
void uncorrelateCovariance(size_t first)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
Vector<Type, Width> diag_elements = self.slice<Width, Width>(first, first).diag();
|
||||
self.uncorrelateCovarianceSetVariance(first, diag_elements);
|
||||
}
|
||||
|
||||
template <size_t Width>
|
||||
void uncorrelateCovarianceSetVariance(size_t first, const Vector<Type, Width> &vec)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
// zero rows and columns
|
||||
self.slice<Width, M>(first, 0) = 0;
|
||||
self.slice<M, Width>(0, first) = 0;
|
||||
|
||||
// set diagonals
|
||||
unsigned vec_idx = 0;
|
||||
for (size_t idx = first; idx < first+Width; idx++) {
|
||||
self(idx,idx) = vec(vec_idx);
|
||||
vec_idx ++;
|
||||
}
|
||||
}
|
||||
|
||||
template <size_t Width>
|
||||
void uncorrelateCovarianceSetVariance(size_t first, Type val)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
// zero rows and columns
|
||||
self.slice<Width, M>(first, 0) = 0;
|
||||
self.slice<M, Width>(0, first) = 0;
|
||||
|
||||
// set diagonals
|
||||
for (size_t idx = first; idx < first+Width; idx++) {
|
||||
self(idx,idx) = val;
|
||||
}
|
||||
}
|
||||
|
||||
// make block diagonal symmetric by taking the average of the two corresponding off diagonal values
|
||||
template <size_t Width>
|
||||
void makeBlockSymmetric(size_t first)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
if(Width>1) {
|
||||
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
|
||||
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
||||
Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2;
|
||||
self(row_idx,col_idx) = tmp;
|
||||
self(col_idx,row_idx) = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// make rows and columns symmetric by taking the average of the two corresponding off diagonal values
|
||||
template <size_t Width>
|
||||
void makeRowColSymmetric(size_t first)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
self.makeBlockSymmetric<Width>(first);
|
||||
for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
|
||||
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
||||
Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2;
|
||||
self(row_idx,col_idx) = tmp;
|
||||
self(col_idx,row_idx) = tmp;
|
||||
}
|
||||
for (size_t col_idx = first+Width; col_idx < M; col_idx++) {
|
||||
Type tmp = self(row_idx,col_idx) + (self(col_idx,row_idx) - self(row_idx,col_idx)) / 2;
|
||||
self(row_idx,col_idx) = tmp;
|
||||
self(col_idx,row_idx) = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// checks if block diagonal is symmetric
|
||||
template <size_t Width>
|
||||
bool isBlockSymmetric(size_t first, const Type eps = 1e-8f)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
if(Width>1) {
|
||||
for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
|
||||
for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
|
||||
if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// checks if rows and columns are symmetric
|
||||
template <size_t Width>
|
||||
bool isRowColSymmetric(size_t first, const Type eps = 1e-8f)
|
||||
{
|
||||
SquareMatrix<Type, M> &self = *this;
|
||||
for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
|
||||
for (size_t col_idx = 0; col_idx < first; col_idx++) {
|
||||
if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (size_t col_idx = first+Width; col_idx < M; col_idx++) {
|
||||
if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return self.isBlockSymmetric<Width>(first, eps);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
typedef SquareMatrix<float, 3> SquareMatrix3f;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user