void setup_ahrs(void) { /* Set up IMU. */ setup_mpu(); /* Initialize DCMs as identity matrices. */ static uint8_t i, j; for (i=0; i<3; i++) { for (j=0; j<3; j++) { m_init_identity(dcm_gyro); m_init_identity(dcm_offset); m_init_identity(dcm_d); } } /** * Calculate DCM offset. * * We use an accelerometer to correct for gyro drift. This means that the * IMU will adjust dcm_gyro according to whatever the accelerometer thinks * is gravity. However, since it is nearly impossible to mount the * accelerometer perfectly horizontal to the chassis, this also means that * dcm_gyro will represent the orientation of the accelerometer, not the * chassis, along the X and Y rotational axes. * * The rotational difference between the orientations of the accelerometer * (dcm_gyro) and the chassis (dcm_out) is constant and can be approximated * by another rotation matrix we will call dcm_offset, which we populate with * trim angles obtained during hover calibration. We rotate dcm_gyro by * dcm_offset to obtain dcm_out. * * Keep in mind, however, that we still update the IMU based on dcm_gyro and * not dcm_out. This is because it is dcm_gyro we are correcting with the * accelerometer's measurement of the gravity vector, and dcm_out is only * a transformation of that DCM based on dcm_offset. We use dcm_out for * flight calculations, but dcm_gyro is what we keep track of within the * IMU. * * As a final note, this is a small-angle approximation! We could do * something fancy with trigonometry, but if the hardware is so poorly * built (or poorly designed) that small-angle approximations become * insufficient, we can't expect software to fix everything. */ #ifdef ACC_WEIGHT // TODO: Set trim angles. trim_angle[0] = TRIM_ANGLE_X; trim_angle[1] = TRIM_ANGLE_Y; v_acc[0] = 0; v_acc[1] = 0; v_acc[2] = 1; for (i=0; i<3; i++) { v_acc_last[i] = v_acc[i]; } //accVar = 100.; #endif // ACC_WEIGHT }
static void process_noise_covariance( const kalman_robot_handle_t * handle, float delta_t, covariance_t * dest) { float base_factor = handle->_process_noise_proportionality * handle->_max_acc; m_init_identity(&(dest->_cov_a)); m_init_identity(&(dest->_cov_b)); m_init_identity(&(dest->_cov_c)); m_init_identity(&(dest->_cov_d)); float factor = 0.25f * delta_t * delta_t * delta_t * delta_t; m_scalar_mult(factor * base_factor, &(dest->_cov_a), &(dest->_cov_a)); factor = 0.5f * delta_t * delta_t * delta_t; m_scalar_mult(factor * base_factor, &(dest->_cov_b), &(dest->_cov_b)); m_scalar_mult(factor * base_factor, &(dest->_cov_c), &(dest->_cov_c)); factor = delta_t * delta_t; m_scalar_mult(factor * base_factor, &(dest->_cov_d), &(dest->_cov_d)); }