/* 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); }