void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu) {
if (time < mag->next_update)
return;
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h);
/* transform to imu frame */
struct DoubleVect3 h_imu;
MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body );
/* transform to sensor frame */
struct DoubleVect3 h_sensor;
MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu );
/* compute magnetometer reading */
MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
VECT3_ADD(mag->value, mag->neutral);
/* FIXME: ADD error reading */
/* round signal to account for adc discretisation */
DOUBLE_VECT3_ROUND(mag->value);
/* saturate */
// VECT3_BOUND_CUBE(mag->value, 0, mag->resolution);
mag->next_update += NPS_MAG_DT;
mag->data_available = TRUE;
}
/**
* 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;
}
float test_quat(void) {
struct FloatVect3 u = { 1., 2., 3.};
FLOAT_VECT3_NORMALIZE(u);
float angle = RadOfDeg(30.);
struct FloatQuat q;
FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
DISPLAY_FLOAT_QUAT("q ", q);
struct FloatQuat iq;
FLOAT_QUAT_INVERT(iq, q);
DISPLAY_FLOAT_QUAT("iq", iq);
struct FloatQuat q1;
FLOAT_QUAT_COMP(q1, q, iq);
DISPLAY_FLOAT_QUAT("q1", q1);
struct FloatQuat q2;
FLOAT_QUAT_COMP(q2, q, iq);
DISPLAY_FLOAT_QUAT("q2", q2);
struct FloatQuat qe;
QUAT_EXPLEMENTARY(qe, q);
DISPLAY_FLOAT_QUAT("qe", qe);
struct FloatVect3 a = { 2., 1., 3.};
DISPLAY_FLOAT_VECT3("a ", a);
struct FloatVect3 a1;
FLOAT_QUAT_VMULT(a1, q, a);
DISPLAY_FLOAT_VECT3("a1", a1);
struct FloatVect3 a2;
FLOAT_QUAT_VMULT(a2, qe, a);
DISPLAY_FLOAT_VECT3("a2", a2);
return 0.;
}
static void test_2(void) {
struct Int32Vect3 v1 = { 5000, 5000, 5000 };
DISPLAY_INT32_VECT3("v1", v1);
struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
DISPLAY_FLOAT_EULERS("euler_f", euler_f);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, euler_f);
DISPLAY_INT32_EULERS("euler_i", euler_i);
struct Int32Quat quat_i;
INT32_QUAT_OF_EULERS(quat_i, euler_i);
DISPLAY_INT32_QUAT("quat_i", quat_i);
INT32_QUAT_NORMALIZE(quat_i);
DISPLAY_INT32_QUAT("quat_i_n", quat_i);
struct Int32Vect3 v2;
INT32_QUAT_VMULT(v2, quat_i, v1);
DISPLAY_INT32_VECT3("v2", v2);
struct Int32RMat rmat_i;
INT32_RMAT_OF_QUAT(rmat_i, quat_i);
DISPLAY_INT32_RMAT("rmat_i", rmat_i);
struct Int32Vect3 v3;
INT32_RMAT_VMULT(v3, rmat_i, v1);
DISPLAY_INT32_VECT3("v3", v3);
struct Int32RMat rmat_i2;
INT32_RMAT_OF_EULERS(rmat_i2, euler_i);
DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);
struct Int32Vect3 v4;
INT32_RMAT_VMULT(v4, rmat_i2, v1);
DISPLAY_INT32_VECT3("v4", v4);
struct FloatQuat quat_f;
FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
DISPLAY_FLOAT_QUAT("quat_f", quat_f);
struct FloatVect3 v5;
VECT3_COPY(v5, v1);
DISPLAY_FLOAT_VECT3("v5", v5);
struct FloatVect3 v6;
FLOAT_QUAT_VMULT(v6, quat_f, v5);
DISPLAY_FLOAT_VECT3("v6", v6);
}
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 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;
}
