static void printBody(vsop87_body_et body, char *name, double T, DT v_e[3],
double e)
{
DT v[3], v_g[3], v_ec[3], v_eq[3];
sexangle_st ang;
if (vsop87_getCoords(v, s_datadir, VSOP87_HELIO_RECT_DATE, body, T)!=0)
return;
printf("%s\theliocentric:\t", name); printVect(v);
printf("\tgeocentric:\t"); VECT3_DIFF(v_g, v, v_e); printVect(v_g);
printf("\tecliptic geo:\t");
amsp_coord_rect2ecliptic(v_ec, v_g);
amsp_angle_deg2sex(&ang, v_ec[0]*CRG); amsp_angle_sexprint(&ang);
printf(" ");
amsp_angle_deg2sex(&ang, v_ec[1]*CRG); amsp_angle_sexprint(&ang);
printf(" ");
printf("%0.9lf\n", v_ec[2]);
printf("\tequatorial geo:\t");
amsp_coord_ecliptic2equatorial(v_eq, v_ec, e);
amsp_angle_deg2sex(&ang, v_eq[0]*CRG); amsp_angle_sexprint(&ang);
printf(" ");
amsp_angle_deg2sex(&ang, v_eq[1]*CRG); amsp_angle_sexprint(&ang);
printf(" ");
printf("%0.9lf\n", v_eq[2]);
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
struct FloatVect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
/* compute the residual of gravity vector in imu frame */
FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
} else {
FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
}
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
const float weight = 1.;
#endif
/* compute correction */
const float gravity_rate_update_gain = -5e-2; // -5e-2
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
const float gravity_bias_update_gain = 1e-5; // -5e-6
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
/* FIXME: saturate bias */
}
/**
* Incorporate one 3D vector measurement
*/
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
FLOAT_QUAT_VMULT(b_expected, ahrs_impl.ltp_to_imu_quat, *i_expected);
// S = HPH' + JRJ
float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
{ b_expected.z, 0., -b_expected.x, 0., 0., 0.},
{-b_expected.y, b_expected.x, 0., 0., 0., 0.}};
float tmp[3][6];
MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
float S[3][3];
MAT_MUL_T(3,6,3, S, tmp, H);
/* add the measurement noise */
S[0][0] += noise->x;
S[1][1] += noise->y;
S[2][2] += noise->z;
float invS[3][3];
MAT_INV33(invS, S);
// K = PH'invS
float tmp2[6][3];
MAT_MUL_T(6,6,3, tmp2, ahrs_impl.P, H);
float K[6][3];
MAT_MUL(6,3,3, K, tmp2, invS);
// P = (I-KH)P
float tmp3[6][6];
MAT_MUL(6,3,6, tmp3, K, H);
float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
{ 0., 1., 0., 0., 0., 0. },
{ 0., 0., 1., 0., 0., 0. },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }};
float tmp4[6][6];
MAT_SUB(6,6, tmp4, I6, tmp3);
float tmp5[6][6];
MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P);
memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P));
// X = X + Ke
struct FloatVect3 e;
VECT3_DIFF(e, *b_measured, b_expected);
ahrs_impl.gibbs_cor.qx += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z;
ahrs_impl.gibbs_cor.qy += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z;
ahrs_impl.gibbs_cor.qz += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z;
ahrs_impl.gyro_bias.p += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z;
ahrs_impl.gyro_bias.q += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z;
ahrs_impl.gyro_bias.r += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z;
}
void aos_compute_sensors(void)
{
struct FloatRates gyro;
RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
// printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
RATES_BFP_OF_REAL(imu.gyro, gyro);
RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
struct FloatVect3 g_ltp = {0., 0., 9.81};
struct FloatVect3 accelero_ltp;
VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
struct FloatVect3 accelero_imu;
float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp);
float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
#ifndef DISABLE_MAG_UPDATE
struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 h_imu;
float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth);
MAGS_BFP_OF_REAL(imu.mag, h_imu);
#endif
aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR2
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR
ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.gps_age = 0;
ahrs_update_gps();
//RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration));
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel));
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#endif
}
static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
struct Int32Vect3 diff;
VECT3_DIFF(diff, i1, i2);
if (diff.x < -threshold || diff.x > threshold ||
diff.y < -threshold || diff.y > threshold ||
diff.z < -threshold || diff.z > threshold) {
LED_ON(4);
return TRUE;
} else {
LED_OFF(4);
return FALSE;
}
}
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
{
// check if we had at least one propagation since last update
if (ahrs_icq.accel_cnt == 0) {
return;
}
// c2 = ltp z-axis in imu-frame
struct Int32RMat ltp_to_imu_rmat;
int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat);
struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2),
RMAT_ELMT(ltp_to_imu_rmat, 1, 2),
RMAT_ELMT(ltp_to_imu_rmat, 2, 2)
};
struct Int32Vect3 residual;
struct Int32Vect3 pseudo_gravity_measurement;
if (ahrs_icq.correct_gravity && ahrs_icq.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
// FIXME: check overflows !
#define COMPUTATION_FRAC 16
#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
const struct Int32Vect3 vel_tangential_body =
{ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0};
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
struct Int32Rates body_rate;
int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
struct Int32Vect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC);
/* convert centrifucal acceleration from body to imu frame */
struct Int32Vect3 acc_c_imu;
int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement
* of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu);
} else {
void ahrs_update_accel(void) {
struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
// FIXME: check overflow ?
const struct Int32Vect3 Xdd_imu = {
0,
((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
-((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
};
struct Int32Vect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
#else
INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
#endif
// residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^24 * 5e-2 = 1/81920
ahrs_impl.rate_correction.p += -residual.x/82000;
ahrs_impl.rate_correction.q += -residual.y/82000;
ahrs_impl.rate_correction.r += -residual.z/82000;
// residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^24 * 5e-6 = 1/3.05
// ahrs_impl.high_rez_bias.p += residual.x*3;
// ahrs_impl.high_rez_bias.q += residual.y*3;
// ahrs_impl.high_rez_bias.r += residual.z*3;
ahrs_impl.high_rez_bias.p += residual.x;
ahrs_impl.high_rez_bias.q += residual.y;
ahrs_impl.high_rez_bias.r += residual.z;
/* */
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) {
if (time < accel->next_update)
return;
/* transform gravity to body frame */
struct DoubleVect3 g_body;
FLOAT_QUAT_VMULT(g_body, fdm.ltp_to_body_quat, fdm.ltp_g);
// printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z);
// printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
/* substract gravity to acceleration in body frame */
struct DoubleVect3 accelero_body;
VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
// printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z);
/* transform to imu frame */
struct DoubleVect3 accelero_imu;
MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body );
/* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
VECT3_ADD(accel->value, accel->neutral);
/* Compute sensor error */
struct DoubleVect3 accelero_error;
/* constant bias */
VECT3_COPY(accelero_error, accel->bias);
/* white noise */
double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
/* scale */
struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ};
VECT3_EW_MUL(accelero_error, accelero_error, gain);
/* add error */
VECT3_ADD(accel->value, accelero_error);
/* round signal to account for adc discretisation */
DOUBLE_VECT3_ROUND(accel->value);
/* saturate */
VECT3_BOUND_CUBE(accel->value, 0, accel->resolution);
accel->next_update += NPS_ACCEL_DT;
accel->data_available = TRUE;
}
void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
struct EcefCoor_i delta;
VECT3_DIFF(delta, *ecef, def->ecef);
const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*delta.x +
(int64_t)def->ltp_of_ecef.m[1]*delta.y +
0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*delta.x +
(int64_t)def->ltp_of_ecef.m[4]*delta.y +
(int64_t)def->ltp_of_ecef.m[5]*delta.z;
enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*delta.x +
(int64_t)def->ltp_of_ecef.m[7]*delta.y +
(int64_t)def->ltp_of_ecef.m[8]*delta.z;
enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
}
int main(const int argc, char * const *argv)
{
dtm_st dtm;
time_t t;
DT T, e;
DT v_e[3], v_g[3];
sexangle_st ang;
if (argc!=2) {
printUsage();
return EXIT_FAILURE;
}
s_datadir = argv[1];
t = time(NULL);
gmtime_r(&t, &dtm.tm);
dtm.sec = 0.;
T = amsp_date_tm2julian(&dtm);
e = amsp_coord_earthEclipticAtDate(T);
printf("Earth ecliptic: %f ", e);
amsp_angle_deg2sex(&ang, e*CRG);
amsp_angle_sexprint(&ang); printf("\n");
if (vsop87_getCoords(v_e, s_datadir, VSOP87_HELIO_RECT_DATE, BODY_EARTH, T)!=0)
return EXIT_FAILURE;
printf("Earth heliocentric:\t"); printVect(v_e);
printf(" geocentric:\t");
VECT3_DIFF(v_g, v_e, v_e); printVect(v_g);
printBody(BODY_MERCURY, "Mercury", T, v_e, e);
printBody(BODY_VENUS, "Venus", T, v_e, e);
printBody(BODY_MARS, "Mars", T, v_e, e);
printBody(BODY_JUPITER, "Jupiter", T, v_e, e);
printBody(BODY_SATURN, "Saturn", T, v_e, e);
printBody(BODY_URANUS, "Uranus", T, v_e, e);
vsop87_clearDataCache();
return 0;
}
void aos_compute_sensors(void) {
struct FloatRates gyro;
RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
// printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
RATES_BFP_OF_REAL(imu.gyro, gyro);
RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
struct FloatVect3 g_ltp = {0., 0., 9.81};
struct FloatVect3 accelero_ltp;
VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
struct FloatVect3 accelero_imu;
FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp);
float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 h_imu;
FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth);
MAGS_BFP_OF_REAL(imu.mag, h_imu);
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
#endif
#endif
}
void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
struct EcefCoor_f delta;
VECT3_DIFF(delta, *ecef, def->ecef);
MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
}
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
{
// check if we had at least one propagation since last update
if (ahrs_fc.accel_cnt == 0) {
return;
}
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_fc.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_fc.weight, 0.15, 1.0);
} else {
ahrs_fc.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
* with ahrs_fc.accel_cnt beeing the number of propagations since last update
*/
const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
// reset accel propagation counter
ahrs_fc.accel_cnt = 0;
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
void ahrs_update_accel(void) {
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_impl.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_impl.weight, 0.15, 1.0);
}
else {
ahrs_impl.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
*/
const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {
/* cmd_x is positive to north = negative pitch
* cmd_y is positive to east = positive roll
*
* orientation vector describing simultaneous rotation of roll/pitch
*/
const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
float_quat_of_orientation_vect(&q_rp, &ov);
/* as rotation matrix */
struct FloatRMat R_rp;
float_rmat_of_quat(&R_rp, &q_rp);
/* body x-axis (before heading command) is first column */
struct FloatVect3 b_x;
VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
/* body z-axis (thrust vect) is last column */
struct FloatVect3 thrust_vect;
VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where the normal n is the thrust vector (i.e. both a and b lie in that plane)
*/
// desired heading vect in earth x-y plane
const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
/* projection of desired heading onto body x-y plane
* b = v - dot(v,n)*n
*/
float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
struct FloatVect3 dotn;
VECT3_SMUL(dotn, thrust_vect, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
VECT3_DIFF(b, psi_vect, dotn);
dot = VECT3_DOT_PRODUCT(b_x, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, b_x, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
// angle = atan2(norm(cross(a,b)), dot(a,b))
float yaw2 = atan2(nc, dot) / 2.0;
// negative angle if needed
// sign(dot(cross(a,b), n)
float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
if (dot_cross_ab < 0) {
yaw2 = -yaw2;
}
/* quaternion with yaw command */
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
float_quat_comp(quat, &q_rp, &q_yaw);
float_quat_normalize(quat);
float_quat_wrap_shortest(quat);
}