/* 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;
}
/* This function estimates a quaternion from a set of observations
*
* B is the "attitude profile matrix". Use the other functions to fill it with the attitude observations.
*
* The function solves Wahba's problem using Paul Davenport's solution.
* Unfortunatly Davenport unpublished his solution, but you can still find descriptions of it in the web
* ( e.g: home.comcast.net/~mdshuster2/PUB_2006c_J_GenWahba_AAS.pdf )
*
*/
struct DoubleQuat estimated_attitude(struct DoubleMat33 B, unsigned int maximum_iterations, double precision,
struct DoubleMat33 sigma_B, struct DoubleQuat *sigma_q)
{
double traceB,
z1, z2, z3;
struct DoubleMat44 K, sigma_K;
struct DoubleQuat q_guessed;
K = generate_K_matrix(B);
sigma_K = generate_K_matrix(sigma_B);
/* compute the estimated quaternion */
q_guessed = dominant_Eigenvector(square_skaled(K), maximum_iterations, precision, sigma_K,
sigma_q); // K² is tricky. I'm mean, I know
/* Final scaling, because the eigenvector hat not the length = 1 */
double scale = 1 / NORM_VECT4(q_guessed);
QUAT_SMUL(q_guessed, q_guessed, scale);
QUAT_SMUL(*sigma_q, *sigma_q, scale);
return q_guessed;
}
void stabilization_attitude_ref_update(void) {
/* integrate reference attitude */
#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
struct FloatQuat qdot;
FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(stab_att_ref_quat, qdot);
#else // use finite step (involves trig)
struct FloatQuat delta_q;
FLOAT_QUAT_DIFFERENTIAL(delta_q, stab_att_ref_rate, DT_UPDATE);
/* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
struct FloatQuat new_ref_quat;
FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat);
QUAT_COPY(stab_att_ref_quat, new_ref_quat);
#endif
FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_rate);
/* compute reference angular accelerations */
struct FloatQuat err;
/* compute reference attitude error */
FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
- stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
- stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
- stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate angular speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
void stabilization_attitude_ref_update() {
/* integrate reference attitude */
struct FloatQuat qdot;
FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(stab_att_ref_quat, qdot);
FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_rate);
/* compute reference angular accelerations */
struct FloatQuat err;
/* compute reference attitude error */
FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
- stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
- stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
- stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
