float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display)
{
struct FloatQuat qa2b_f;
float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f);
struct Int32Quat qa2c_i;
QUAT_BFP_OF_REAL(qa2c_i, qa2c_f);
struct Int32Quat qb2c_i;
QUAT_BFP_OF_REAL(qb2c_i, qb2c_f);
struct Int32Quat qa2b_i;
int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i);
struct FloatQuat err;
QUAT_DIFF(err, qa2b_f, qa2b_i);
float norm_err = FLOAT_QUAT_NORM(err);
if (display) {
printf("quat comp_inv\n");
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2bf", qa2b_f);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2bi", qa2b_i);
}
return norm_err;
}
/* the following solver uses the Power Iteration
*
* It is rather simple:
* 1. You choose a starting vektor x_0 (shouldn't be zero)
* 2. apply it on the Matrix
* x_(k+1) = A * x_k
* 3. Repeat this very often.
*
* The vector converges to the dominat eigenvector, which belongs to the eigenvalue with the biggest absolute value.
*
* But there is a problem:
* With every step, the norm of vector grows. Therefore it's necessary to scale it with every step.
*
* Important warnings:
* I. This function does not converge if the Matrix is singular
* II. Pay attention to the loop-condition! It does not end if it's close to the eigenvector!
* It ends if the steps are getting too close to each other.
*
*/
DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iterations, double precision,
struct DoubleMat44 sigma_A, DoubleVect4 *sigma_x)
{
unsigned int k;
DoubleVect4 x_k,
x_kp1;
double delta = 1,
scale;
FLOAT_QUAT_ZERO(x_k);
//for(k=0; (k<maximum_iterations) && (delta>precision); k++){
for (k = 0; k < maximum_iterations; k++) {
// Next step
DOUBLE_MAT_VMULT4(x_kp1, A, x_k);
// Scale the vector
scale = 1 / INFTY_NORM4(x_kp1);
QUAT_SMUL(x_kp1, x_kp1, scale);
// Calculate the difference between to steps for the loop condition. Store temporarily in x_k
QUAT_DIFF(x_k, x_k, x_kp1);
delta = INFTY_NORM4(x_k);
// Update the next step
x_k = x_kp1;
if (delta <= precision) {
DOUBLE_MAT_VMULT4(*sigma_x, sigma_A, x_k);
QUAT_SMUL(*sigma_x, *sigma_x, scale);
break;
}
}
#ifdef EKNAV_FROM_LOG_DEBUG
printf("Number of iterations: %4i\n", k);
#endif
if (k == maximum_iterations) {
printf("Orientation did not converge. Using maximum uncertainty\n");
//FLOAT_QUAT_ZERO(x_k);
QUAT_ASSIGN(*sigma_x, 0, M_PI_2, M_PI_2, M_PI_2);
}
return x_k;
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);
struct FloatQuat qf;
FLOAT_QUAT_OF_RMAT(qf, rmat_f);
FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) DISPLAY_FLOAT_QUAT("qf", qf);
struct Int32Quat qi;
INT32_QUAT_OF_RMAT(qi, rmat_i);
INT32_QUAT_WRAP_SHORTEST(qi);
if (display) DISPLAY_INT32_QUAT("qi", qi);
if (display) DISPLAY_INT32_QUAT_2("qi", qi);
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
struct FloatQuat qerr;
QUAT_DIFF(qerr, qif, qf);
float err_norm = FLOAT_QUAT_NORM(qerr);
if (display) printf("err %f\n", err_norm);
if (display) printf("\n");
return err_norm;
}
