Esempio n. 1
0
void ahrs_update_accel(void) {

  // c2 = ltp z-axis in imu-frame
  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;

  struct Int32Vect3 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
     */

    // FIXME: check overflows !
    const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0};
    struct Int32Vect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
    INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC);

    /* convert centrifucal acceleration from body to imu frame */
    struct Int32Vect3 acc_c_imu;
    INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);

    /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
    INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu);
  } else {
Esempio n. 2
0
void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag)
{

  /* project mag on local tangeant plane */
  struct FloatEulers ltp_to_imu_euler;
  float_eulers_of_rmat(&ltp_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
  struct FloatVect3 magf;
  MAGS_FLOAT_OF_BFP(magf, *mag);
  const float cphi   = cosf(ltp_to_imu_euler.phi);
  const float sphi   = sinf(ltp_to_imu_euler.phi);
  const float ctheta = cosf(ltp_to_imu_euler.theta);
  const float stheta = sinf(ltp_to_imu_euler.theta);
  const float mn = ctheta * magf.x + sphi * stheta * magf.y + cphi * stheta * magf.z;
  const float me =     0. * magf.x + cphi       * magf.y - sphi       * magf.z;

  const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me +
                         RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
          RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1),
          RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
  };
  const float mag_rate_update_gain = 2.5;
  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
  const float mag_bias_update_gain = -2.5e-4;
  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));

}
Esempio n. 3
0
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 */

}
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(&ltp_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 {
Esempio n. 5
0
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);


}
Esempio n. 6
0
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
    }
  }
}
Esempio n. 7
0
void ahrs_fc_update_heading(float heading)
{

  FLOAT_ANGLE_NORMALIZE(heading);

  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
  struct FloatQuat ltp_to_body_quat;
  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
  struct FloatRMat ltp_to_body_rmat;
  float_rmat_of_quat(&ltp_to_body_rmat, &ltp_to_body_quat);
  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
  // we only consider x and y
  struct FloatVect2 expected_ltp = {
    RMAT_ELMT(ltp_to_body_rmat, 0, 0),
    RMAT_ELMT(ltp_to_body_rmat, 0, 1)
  };

  // expected_heading cross measured_heading
  struct FloatVect3 residual_ltp = {
    0,
    0,
    expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
  };

  struct FloatVect3 residual_imu;
  float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);

  const float heading_rate_update_gain = 2.5;
  RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);

  float heading_bias_update_gain;
  /* crude attempt to only update bias if deviation is small
   * e.g. needed when you only have gps providing heading
   * and the inital heading is totally different from
   * the gps course information you get once you have a gps fix.
   * Otherwise the bias will be falsely "corrected".
   */
  if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
    heading_bias_update_gain = -2.5e-4;
  } else {
    heading_bias_update_gain = 0.0;
  }
  RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
}
Esempio n. 8
0
void ahrs_update_mag_full_2d_dumb(void) {

  /* project mag on local tangeant plane */
  struct FloatVect3 magf;
  MAGS_FLOAT_OF_BFP(magf, imu.mag);
  const float cphi   = cosf(ahrs_float.ltp_to_imu_euler.phi);
  const float sphi   = sinf(ahrs_float.ltp_to_imu_euler.phi);
  const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta);
  const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta);
  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
  const float me =     0. * magf.x + cphi       *magf.y - sphi       *magf.z;

  const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
  const float mag_rate_update_gain = 2.5;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
  const float mag_bias_update_gain = -2.5e-4;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));

}
Esempio n. 9
0
float float_rmat_reorthogonalize(struct FloatRMat *rm)
{
  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
          RMAT_ELMT(*rm, 0, 1),
          RMAT_ELMT(*rm, 0, 2)
  };
  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
          RMAT_ELMT(*rm, 1, 1),
          RMAT_ELMT(*rm, 1, 2)
  };
  float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
  struct FloatVect3 r0_t;
  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
  struct FloatVect3 r1_t;
  VECT3_SUM_SCALED(r1_t,  r1, r0, _err);
  struct FloatVect3 r2_t;
  VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
  float s = renorm_factor(VECT3_NORM2(r0_t));
  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
  s = renorm_factor(VECT3_NORM2(r1_t));
  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
  s = renorm_factor(VECT3_NORM2(r2_t));
  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);

  return _err;
}
Esempio n. 10
0
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
{
  const float sphi   = sinf(e->phi);
  const float cphi   = cosf(e->phi);
  const float stheta = sinf(e->theta);
  const float ctheta = cosf(e->theta);
  const float spsi   = sinf(e->psi);
  const float cpsi   = cosf(e->psi);

  RMAT_ELMT(*rm, 0, 0) =  ctheta * cpsi - sphi * stheta * spsi;
  RMAT_ELMT(*rm, 0, 1) =  ctheta * spsi + sphi * stheta * cpsi;
  RMAT_ELMT(*rm, 0, 2) = -cphi * stheta;
  RMAT_ELMT(*rm, 1, 0) = -cphi * spsi;
  RMAT_ELMT(*rm, 1, 1) =  cphi * cpsi;
  RMAT_ELMT(*rm, 1, 2) =  sphi;
  RMAT_ELMT(*rm, 2, 0) =  stheta * cpsi + sphi * ctheta * spsi;
  RMAT_ELMT(*rm, 2, 1) =  stheta * spsi - sphi * ctheta * cpsi;
  RMAT_ELMT(*rm, 2, 2) =  cphi * ctheta;
}
Esempio n. 11
0
void ahrs_update_heading(float heading) {

    FLOAT_ANGLE_NORMALIZE(heading);

    struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
    // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
    // we only consider x and y
    struct FloatVect2 expected_ltp =
    {   RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
        RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
    };

    // expected_heading cross measured_heading
    struct FloatVect3 residual_ltp =
    {   0,
        0,
        expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
    };

    struct FloatVect3 residual_imu;
    FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);

    const float heading_rate_update_gain = 2.5;
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);

    float heading_bias_update_gain;
    /* crude attempt to only update bias if deviation is small
     * e.g. needed when you only have gps providing heading
     * and the inital heading is totally different from
     * the gps course information you get once you have a gps fix.
     * Otherwise the bias will be falsely "corrected".
     */
    if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
        heading_bias_update_gain = -2.5e-4;
    else
        heading_bias_update_gain = 0.0;
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
}
Esempio n. 12
0
void ahrs_update_accel(void) {

  struct FloatVect3 accel_float;
  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);

#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
  accel_float.y -=  v * ahrs_float.imu_rate.r;
  accel_float.z -= -v * ahrs_float.imu_rate.q;
#endif

  struct FloatVect3  c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
  struct FloatVect3 residual;
  FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  /* heuristic on acceleration norm */
  const float acc_norm = FLOAT_VECT3_NORM(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);
#if 1
  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);
#else
  const float alpha = 5e-4;
  FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
#endif
  /* FIXME: saturate bias */

}
void ahrs_update_mag(void) {

  /* project mag on local tangeant plane */
  struct FloatVect3 magf;
  MAGS_FLOAT_OF_BFP(magf, imu.mag);
  const float cphi   = cosf(ahrs_float.ltp_to_imu_euler.phi);
  const float sphi   = sinf(ahrs_float.ltp_to_imu_euler.phi);
  const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta);
  const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta);
  const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
  const float me =     0. * magf.x + cphi       *magf.y - sphi       *magf.z;

  const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
  printf("res norm %f\n", res_norm);
  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
  DISPLAY_FLOAT_VECT3("r2", (*r2));
  const float mag_rate_update_gain = 2.5;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
  DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction);
  const float mag_bias_update_gain = -2.5e-4;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
  /* FIXME: saturate bias */

}
Esempio n. 14
0
/* C n->b rotation matrix */
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
{
  const float _a = M_SQRT2 * q->qi;
  const float _b = M_SQRT2 * q->qx;
  const float _c = M_SQRT2 * q->qy;
  const float _d = M_SQRT2 * q->qz;
  const float a2_1 = _a * _a - 1;
  const float ab = _a * _b;
  const float ac = _a * _c;
  const float ad = _a * _d;
  const float bc = _b * _c;
  const float bd = _b * _d;
  const float cd = _c * _d;
  RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
  RMAT_ELMT(*rm, 0, 1) = bc + ad;
  RMAT_ELMT(*rm, 0, 2) = bd - ac;
  RMAT_ELMT(*rm, 1, 0) = bc - ad;
  RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
  RMAT_ELMT(*rm, 1, 2) = cd + ab;
  RMAT_ELMT(*rm, 2, 0) = bd + ac;
  RMAT_ELMT(*rm, 2, 1) = cd - ab;
  RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
}
Esempio n. 15
0
void guidance_indi_calcG(struct FloatMat33 *Gmat) {

  struct FloatEulers *euler = stateGetNedToBodyEulers_f();

  float sphi = sinf(euler->phi);
  float cphi = cosf(euler->phi);
  float stheta = sinf(euler->theta);
  float ctheta = cosf(euler->theta);
  float spsi = sinf(euler->psi);
  float cpsi = cosf(euler->psi);
  float T = -9.81; //minus gravity is a good guesstimate of the thrust force
//   float T = (filt_accelzn-9.81)/(cphi*ctheta); //calculate specific force in body z axis by using the accelerometer
//   float T = filt_accelzbody; //if body acceleration is available, use that!

  RMAT_ELMT(*Gmat, 0, 0) = (cphi*spsi - sphi*cpsi*stheta)*T;
  RMAT_ELMT(*Gmat, 1, 0) = (-sphi*spsi*stheta - cpsi*cphi)*T;
  RMAT_ELMT(*Gmat, 2, 0) = -ctheta*sphi*T;
  RMAT_ELMT(*Gmat, 0, 1) = (cphi*cpsi*ctheta)*T;
  RMAT_ELMT(*Gmat, 1, 1) = (cphi*spsi*ctheta)*T;
  RMAT_ELMT(*Gmat, 2, 1) = -stheta*cphi*T;
  RMAT_ELMT(*Gmat, 0, 2) = sphi*spsi + cphi*cpsi*stheta;
  RMAT_ELMT(*Gmat, 1, 2) = cphi*spsi*stheta - cpsi*sphi;
  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
}
void ahrs_update_accel(void) {

  struct FloatVect3 accel_float;
  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
  struct FloatVect3 residual;
  FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2));
  /* heuristic on acceleration norm */
  const float acc_norm = FLOAT_VECT3_NORM(accel_float);
  const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.);
  //const float weight = 1.;
  /* compute correction */
  const float gravity_rate_update_gain = 5e-2;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
  const float gravity_bias_update_gain = -5e-6;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
  /* FIXME: saturate bias */

}
Esempio n. 17
0
/** initialises a rotation matrix from unit vector axis and angle */
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
{
  const float ux2  = uv->x * uv->x;
  const float uy2  = uv->y * uv->y;
  const float uz2  = uv->z * uv->z;
  const float uxuy = uv->x * uv->y;
  const float uyuz = uv->y * uv->z;
  const float uxuz = uv->x * uv->z;
  const float can  = cosf(angle);
  const float san  = sinf(angle);
  const float one_m_can = (1. - can);

  RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
  RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->z * san;
  RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->y * san;
  RMAT_ELMT(*rm, 1, 0) = RMAT_ELMT(*rm, 0, 1);
  RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
  RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->x * san;
  RMAT_ELMT(*rm, 2, 0) = RMAT_ELMT(*rm, 0, 2);
  RMAT_ELMT(*rm, 2, 1) = RMAT_ELMT(*rm, 1, 2);
  RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
}
Esempio n. 18
0
/*
 * Propagate our dynamic system in time
 *
 * Run the strapdown computation and the predict step of the filter
 *
 *
 *      quat_dot = Wxq(pqr) * quat
 *      bias_dot = 0
 *
 * Wxq is the quaternion omega matrix:
 *
 *              [ 0, -p, -q, -r ]
 *      1/2 *   [ p,  0,  r, -q ]
 *              [ q, -r,  0,  p ]
 *              [ r,  q, -p,  0 ]
 *
 */
void ahrs_propagate(void) {
	int i, j, k;

    ahrs_lowpass_accel();

	/* compute unbiased rates */
	RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
	RATES_SUB(bafl_rates, bafl_bias);


	/* run strapdown computation
	 *
	 */

	/* multiplicative quaternion update */
	/* compute correction quaternion */
	QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
	/* normalize it. maybe not necessary?? */
	float q_sq;
	q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
	if (q_sq > 1) { /* this should actually never happen */
		FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
	} else {
		bafl_qr.qi = sqrtf(1 - q_sq);
	}
	/* propagate correction quaternion */
	FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);

	bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
	//TODO check if broot force normalization is good, use lagrange normalization?
	FLOAT_QUAT_NORMALISE(bafl_quat);


	/*
	 *  compute all representations
	 */
	/* maintain rotation matrix representation */
	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
	/* maintain euler representation */
	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
	AHRS_TO_BFP();
	AHRS_LTP_TO_BODY();


	/* error KF-Filter
	 * propagate only the error covariance matrix since error is corrected after each measurement
	 *
	 * F = [ 0  0  0          ]
	 *     [ 0  0  0   -Cbn   ]
	 *     [ 0  0  0          ]
	 *     [ 0  0  0  0  0  0 ]
	 *     [ 0  0  0  0  0  0 ]
	 *     [ 0  0  0  0  0  0 ]
	 *
	 * T = e^(dt * F)
	 *
	 * T = [ 1   0   0             ]
	 *     [ 0   1   0   -Cbn*dt   ]
	 *     [ 0   0   1             ]
	 *     [ 0   0   0   1   0   0 ]
	 *     [ 0   0   0   0   1   0 ]
	 *     [ 0   0   0   0   0   1 ]
	 *
	 * P_prio = T * P * T_T + Q
	 *
	 */

	/*
	 *  compute state transition matrix T
	 *
	 *  diagonal elements of T are always one
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
		}
	}


	/*
	 * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
	 */
	/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6)      */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_tempP[i][j] = 0.;
			for (k = 0; k < BAFL_SSIZE; k++) {
				bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
			}
		}
	}

	/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q  */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_P[i][j] = 0.;
			for (k = 0; k < BAFL_SSIZE; k++) {
				bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
			}
		}
		if (i<3) {
			bafl_P[i][i] += bafl_Q_att;
		} else {
			bafl_P[i][i] += bafl_Q_gyro;
		}
	}

#ifdef LKF_PRINT_P
	printf("Pp =");
	for (i = 0; i < 6; i++) {
		printf("[");
		for (j = 0; j < 6; j++) {
			printf("%f\t", bafl_P[i][j]);
		}
		printf("]\n    ");
	}
	printf("\n");
#endif
}
Esempio n. 19
0
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 */
}
Esempio n. 20
0
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
{
  RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
  RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
  RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
  RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
  RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);
  RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
  RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
  RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
  RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
}
Esempio n. 21
0
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
{
  const float tr = RMAT_TRACE(*rm);
  if (tr > 0) {
    const float two_qi = sqrtf(1. + tr);
    const float four_qi = 2. * two_qi;
    q->qi = 0.5 * two_qi;
    q->qx = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qi;
    q->qy = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qi;
    q->qz = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qi;
    /*printf("tr > 0\n");*/
  } else {
    if (RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1) &&
        RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2)) {
      const float two_qx = sqrtf(RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1)
                                 - RMAT_ELMT(*rm, 2, 2) + 1);
      const float four_qx = 2. * two_qx;
      q->qi = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qx;
      q->qx = 0.5 * two_qx;
      q->qy = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qx;
      q->qz = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qx;
      /*printf("m00 largest\n");*/
    } else if (RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2)) {
      const float two_qy =
        sqrtf(RMAT_ELMT(*rm, 1, 1) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 2, 2) + 1);
      const float four_qy = 2. * two_qy;
      q->qi = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qy;
      q->qx = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qy;
      q->qy = 0.5 * two_qy;
      q->qz = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qy;
      /*printf("m11 largest\n");*/
    } else {
      const float two_qz =
        sqrtf(RMAT_ELMT(*rm, 2, 2) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1) + 1);
      const float four_qz = 2. * two_qz;
      q->qi = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qz;
      q->qx = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qz;
      q->qy = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qz;
      q->qz = 0.5 * two_qz;
      /*printf("m22 largest\n");*/
    }
  }
}
Esempio n. 22
0
static void ahrs_do_update_accel(void) {
	int i, j, k;

#ifdef BAFL_DEBUG2
	printf("Accel update.\n");
#endif

	/* P_prio = P */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_Pprio[i][j] = bafl_P[i][j];
		}
	}

	/*
	 * set up measurement matrix
	 *
	 * g = 9.81
	 * gx = [ 0 -g  0
	 *        g  0  0
	 *        0  0  0 ]
	 * H = [          0 0 0 ]
	 *	   [ -Cnb*gx  0 0 0 ]
	 *     [          0 0 0 ]
	 *
	 * */
	bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
	bafl_H[0][1] =  RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
	bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
	bafl_H[1][1] =  RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
	bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
	bafl_H[2][1] =  RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
	/* rest is zero
	bafl_H[0][2] = 0.;
	bafl_H[1][2] = 0.;
	bafl_H[2][2] = 0.;*/

	/**********************************************
	 * compute Kalman gain K
	 *
	 * K = P_prio * H_T * inv(S)
	 * S = H * P_prio * HT + R
	 *
	 * K = P_prio * HT * inv(H * P_prio * HT + R)
	 *
	 **********************************************/

	/* covariance residual S = H * P_prio * HT + R    */

	/* temp_S(3x6) = H(3x6) * P_prio(6x6)
	 * last 4 columns of H are zero
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_tempS[i][j]  = bafl_H[i][0] * bafl_Pprio[0][j];
			bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
		}
	}

	/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
	 *
	 * bottom 4 rows of HT are zero
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			bafl_S[i][j]  = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
			bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
		}
		bafl_S[i][i] += bafl_R_accel;
	}

	/* invert S
	 */
	FLOAT_MAT33_INVERT(bafl_invS, bafl_S);


	/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
	 *
	 * bottom 4 rows of HT are zero
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < 3; j++) {
			/* not computing zero elements */
			bafl_tempK[i][j]  = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
			bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
		}
	}

	/* K(6x3) = temp_K(6x3) * invS(3x3)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < 3; j++) {
			bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
			bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
			bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
		}
	}

	/**********************************************
	 * Update filter state.
	 *
	 *  The a priori filter state is zero since the errors are corrected after each update.
	 *
	 *  X = X_prio + K (y - H * X_prio)
	 *  X = K * y
	 **********************************************/

	/*printf("R = ");
	for (i = 0; i < 3; i++) {
		printf("[");
		for (j = 0; j < 3; j++) {
			printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
		}
		printf("]\n    ");
	}
	printf("\n");*/

	/* innovation
	 * y = Cnb * -[0; 0; g] - accel
	 */
	bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
	bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
	bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;

	/* X(6) = K(6x3) * y(3)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		bafl_X[i]  = bafl_K[i][0] * bafl_ya.x;
		bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
		bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
	}

	/**********************************************
	 * Update the filter covariance.
	 *
	 *  P = P_prio - K * H * P_prio
	 *  P = ( I - K * H ) * P_prio
	 *
	 **********************************************/

	/*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
	 *
	 *  last 4 columns of H are zero
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			if (i == j) {
				bafl_tempP[i][j] = 1.;
			} else {
				bafl_tempP[i][j] = 0.;
			}
			if (j < 2) { /* omit the parts where H is zero */
				for (k = 0; k < 3; k++) {
					bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
				}
			}
		}
	}

	/*  P(6x6) = temp(6x6) * P_prio(6x6)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
			for (k = 1; k < BAFL_SSIZE; k++) {
				bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
			}
		}
	}

#ifdef LKF_PRINT_P
	printf("Pua=");
	for (i = 0; i < 6; i++) {
		printf("[");
		for (j = 0; j < 6; j++) {
			printf("%f\t", bafl_P[i][j]);
		}
		printf("]\n    ");
	}
	printf("\n");
#endif

	/**********************************************
	 *  Correct errors.
	 *
	 *
	 **********************************************/

	/*  Error quaternion.
	 */
	QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
	FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);

	/* normalize
	 * Is this needed? Or is the approximation good enough?*/
	float q_sq;
	q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
	if (q_sq > 1) { /* this should actually never happen */
		FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
		printf("Accel error quaternion q_sq > 1!!\n");
	} else {
		bafl_q_a_err.qi = sqrtf(1 - q_sq);
	}

	/*  correct attitude
	 */
	FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);


	/*  correct gyro bias
	 */
	RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
	RATES_SUB(bafl_bias, bafl_b_a_err);

	/*
	 *  compute all representations
	 */
	/* maintain rotation matrix representation */
	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
	/* maintain euler representation */
	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
	AHRS_TO_BFP();
	AHRS_LTP_TO_BODY();
}
Esempio n. 23
0
static void ahrs_do_update_mag(void) {
	int i, j, k;

#ifdef BAFL_DEBUG2
	printf("Mag update.\n");
#endif

	MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);

	/* P_prio = P */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_Pprio[i][j] = bafl_P[i][j];
		}
	}

	/*
	 * set up measurement matrix
	 *
	 * h = [236.; -2.; 396.];
	 * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
	 * Hm = Cnb * hx;
	 * H = [ 0  0        0  0  0
	 *       0  0 Cnb*hx 0  0  0
	 *       0  0        0  0  0 ];
	 * */
	/*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
	bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
	bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
	bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
	bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
	bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
	//rest is zero

	/**********************************************
	 * compute Kalman gain K
	 *
	 * K = P_prio * H_T * inv(S)
	 * S = H * P_prio * HT + R
	 *
	 * K = P_prio * HT * inv(H * P_prio * HT + R)
	 *
	 **********************************************/

	/* covariance residual S = H * P_prio * HT + R    */

	/* temp_S(3x6) = H(3x6) * P_prio(6x6)
	 *
	 * only third column of H is non-zero
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
		}
	}

	/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
	 *
	 * only third row of HT is non-zero
	 */
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
		}
		bafl_S[i][i] += bafl_R_mag;
	}

	/* invert S
	 */
	FLOAT_MAT33_INVERT(bafl_invS, bafl_S);

	/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
	 *
	 * only third row of HT is non-zero
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < 3; j++) {
			/* not computing zero elements */
			bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
		}
	}

	/* K(6x3) = temp_K(6x3) * invS(3x3)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < 3; j++) {
			bafl_K[i][j]  = bafl_tempK[i][0] * bafl_invS[0][j];
			bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
			bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
		}
	}

	/**********************************************
	 * Update filter state.
	 *
	 *  The a priori filter state is zero since the errors are corrected after each update.
	 *
	 *  X = X_prio + K (y - H * X_prio)
	 *  X = K * y
	 **********************************************/

	/*  innovation
	 *  y = Cnb * [hx; hy; hz] - mag
	 */
	FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
	FLOAT_VECT3_SUB(bafl_ym, bafl_mag);

	/* X(6) = K(6x3) * y(3)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		bafl_X[i]  = bafl_K[i][0] * bafl_ym.x;
		bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
		bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
	}

	/**********************************************
	 * Update the filter covariance.
	 *
	 *  P = P_prio - K * H * P_prio
	 *  P = ( I - K * H ) * P_prio
	 *
	 **********************************************/

	/*  temp(6x6) = I(6x6) - K(6x3)*H(3x6)
	 *
	 *  last 3 columns of H are zero
	 */
	for (i = 0; i < 6; i++) {
		for (j = 0; j < 6; j++) {
			if (i == j) {
				bafl_tempP[i][j] = 1.;
			} else {
				bafl_tempP[i][j] = 0.;
			}
			if (j == 2) { /* omit the parts where H is zero */
				for (k = 0; k < 3; k++) {
					bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
				}
			}
		}
	}

	/*  P(6x6) = temp(6x6) * P_prio(6x6)
	 */
	for (i = 0; i < BAFL_SSIZE; i++) {
		for (j = 0; j < BAFL_SSIZE; j++) {
			bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
			for (k = 1; k < BAFL_SSIZE; k++) {
				bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
			}
		}
	}

#ifdef LKF_PRINT_P
	printf("Pum=");
	for (i = 0; i < 6; i++) {
		printf("[");
		for (j = 0; j < 6; j++) {
			printf("%f\t", bafl_P[i][j]);
		}
		printf("]\n    ");
	}
	printf("\n");
#endif

	/**********************************************
	 *  Correct errors.
	 *
	 *
	 **********************************************/

	/*  Error quaternion.
	 */
	QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
	FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
	/* normalize */
	float q_sq;
	q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
	if (q_sq > 1) { /* this should actually never happen */
		FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
		printf("mag error quaternion q_sq > 1!!\n");
	} else {
		bafl_q_m_err.qi = sqrtf(1 - q_sq);
	}

	/*  correct attitude
	 */
	FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
	FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);

	/*  correct gyro bias
	 */
	RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
	RATES_SUB(bafl_bias, bafl_b_m_err);

	/*
	 *  compute all representations
	 */
	/* maintain rotation matrix representation */
	FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
	/* maintain euler representation */
	FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
	AHRS_TO_BFP();
	AHRS_LTP_TO_BODY();
}
Esempio n. 24
0
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 */

}