END_TEST
START_TEST(test_matrix_reconstruct_udu)
{
double U[4][4] = {
{1, 2, 3, 4},
{0, 1, 5, 6},
{0, 0, 1, 7},
{0, 0, 0, 1}
};
double D[4] = {1, 2, 3, 4};
double M[4][4] = {{0}};
double M_[4][4] = {
{100, 145, 121, 16},
{145, 221, 183, 24},
{121, 183, 199, 28},
{ 16, 24, 28, 4}
};
matrix_reconstruct_udu(4, (double *)U, D, (double *)M);
for (u32 i=0; i<4; i++) {
for (u32 j=0; j<4; j++) {
fail_unless(M[i][j] == M_[i][j], "reconstructed result != test matrix");
}
}
}
END_TEST
START_TEST(test_matrix_udu_2)
{
u32 n = sizerand(MSIZE_MAX);
double M[n][n];
double M_orig[n][n];
for (u32 i=0; i<n; i++) {
for (u32 j=0; j<=i; j++) {
M[i][j] = M[j][i] = mrand;
}
}
/* Square the random matrix to ensure it is positive semi-definite. */
matrix_multiply(n, n, n, (double *)M, (double *)M, (double *)M_orig);
memcpy(M, M_orig, n * n * sizeof(double));
double U[n][n];
memset(U, 0, n * n * sizeof(double));
double D[n];
memset(D, 0, n * sizeof(double));
matrix_udu(n, (double *)M, (double *)U, D);
/* Check U is unit upper triangular. */
for (u32 i=0; i<n; i++) {
for (u32 j=0; j<n; j++) {
if (i == j) {
fail_unless(fabs(U[i][j] - 1) < LINALG_TOL,
"U diagonal element != 1 (was %f)", M[i][j]);
}
if (i > j) {
fail_unless(fabs(U[i][j]) < LINALG_TOL, "U lower triangle element != 0");
}
}
}
/* Check reconstructed matrix is correct. */
double M_[n][n];
memset(M_, 0, n * n * sizeof(double));
matrix_reconstruct_udu(n, (double *)U, D, (double *)M_);
for (u32 i=0; i<n; i++) {
for (u32 j=0; j<n; j++) {
fail_unless(fabs(M_orig[i][j] - M_[i][j]) < LINALG_TOL * MATRIX_MAX,
"reconstructed result != original matrix, delta[%d][%d] = %f",
i, j, fabs(M_orig[i][j] - M_[i][j]));
}
}
}
u8 get_amb_kf_cov(double *cov)
{
u8 num_dds = CLAMP_DIFF(sats_management.num_sats, 1);
matrix_reconstruct_udu(num_dds, nkf.state_cov_U, nkf.state_cov_D, cov);
return num_dds;
}
