Comments 21
Я хотел бы задать немного вопросов по теме:

Насколько я понял из статьи, принципиальная разница в фильтрах Махони и в Мэджвика в том, что в одном случае корректируется угловая скорость, а во втором кватернион? Если еще значительные различия в принципе работы?

У кого из этих двух лучше с быстродействием?

Как так получилось, что ардуинщики большинстве своём знают только о фильтре Мэджвика?
1. Для программиста отличия два: а) у Махони — матрица, у Маджвика — кватернион; б) у Махони корректируется угловая скорость, у Маджвика — скорость изменения кватерниона. Для математика важно ещё и то отличие, что у Махони решение строится геометрически, у Маджвика — аналитически.

2. Не сравнивал, но предполагаю, что у Маджвика может выйти чуть быстрее за счёт меньшей избыточности (4 параметра ориентации вместо 9). Но Маджвик очевидно проиграет на этапе построения своих градиентов, якобианов и т.п., особенно если векторов больше одного и нужно ещё оценить уход гироскопов.

3. Кроме смутных технических соображений из п. 2, могу предположить, что Маджвик больше занят саморекламой. Он коммерсант, а Махони — университетский учёный и уже давно сменил тему исследований. А ведь кроме Махони и Маджвика, было ещё много народу, кто занимался подобными вещами: и Боннабель, и тот же Салычев. О них вообще редко вспоминают.
Понято… Чтож, когда в следующий раз дойдут руки до навигации, попробую фильтр Махони…
Оффтоп: С Салычевым я даже шапошно знаком :)… И книга у него весьма подробная. Фактически настольная. А про Боннабеля впервые слышу. Надо будет ознакомится.
У Боннабеля была очень амбициозная цель: построить общую «геометрическую» теорию таких фильтров, пригодных не только для навигации, но и вообще для всех задач, где используется, например, фильтр Калмана. Красиво размахнулся, но, увы, слабовато ударил.

А у Салычева я и учился, и работал, и диссер защищал.
то ли из-за высоких требований к вычислительным ресурсам, неприемлемых для дронов
Господа, я просто оставлю это здесь:
The Apollo computer used 2k of magnetic core RAM and 36k wire rope [...]. The CPU was built from ICs [...]. Clock speed was under 100 kHz [...]. The fact that the MIT engineers were able to pack such good software (one of the very first applications of the Kalman filter) into such a tiny computer is truly remarkable.
— Interview with Jack Crenshaw, by Matthew Reed, TRS-80.org (2009) [1]
Да, это был шедевр. Не знаю, удалось ли кому-то такое повторить. Кстати, там интегрировалось кинематическое уравнение для матрицы поворота, а малые ошибки дооценивались фильтром Калмана. Ассемблерные исходники выкладывались в интернете.
Интересно, какой фильтр реализован в DPS InvenSens MPU. (https://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/) Сталкивались?
Не сталкивался. Но судя по документации — никакого. Упомянутый там Digital Motion Processor, вероятно, предназначен только для предобработки сырых измерений. Нигде не упомянуты выходные данные об ориентации, нет и сведений об их точности. У меня создалось впечатление, что реализация фильтра возлагается на пользователя.
Спасибо за ответ.
Я с ним работал. Глубоко не копал. Мне нужна была стабилизация. Пробовал Магдвика на Ардуино. И их собственный DMP. Чисто на практике. Их соственный DMP оказался удобней. Не грузит Ардуино, а для меня было критичным передача данных с фиксированным (не переменным) лагом. Быстро поправляется, если обездвижить. Фильтр Магдвига, я так и не понял как настраивать). Он иногда давал необъяснимые большие всплески. И судя по вашей статье, возможно (может ошибаюсь), это «фича» алгоритма, в отсутствие внешних датчиков. Invensense вроде пишет, что их собственный алгоритм учитывает все датчики, и акселерометр и гироскопы (на одной вафле), а фильтр магдвига (судя по коду который я использовал) этого не делает.
Для меня так и осталось непонятным, оценивает ли он углы. По вашим словам выходит, что всё-таки оценивает. Однако в документации я ни слова о них по-прежнему не вижу.

Фильтр Маджвика, разумеется, может использовать и акселерометры, и магнитометры наряду с гироскопами. Тем не менее, каждая область применения (самолёт, дрон, автомобиль, пешеход), по-хорошему, требует своих настроек фильтра — хоть Калмана, хоть Махони, хоть Маджвика. Поэтому мораль своей статьи я вижу именно в том, чтобы сподвигнуть публику самостоятельно изобретать, дорабатывать и настраивать фильтры ориентации, а не полагаться на готовые решения.
Благородная цель. Однако публика низка и ленна). Но ваша статья безусловно облегчила понимание. Спасибо.
А вот еще вопрос в догонку.
Поведение системы ориентации на земле, когда есть реакция опоры и в воздухе — две разные вещи.

Как отлаживать фильтры для дронов, чтобы не получить много удивления в момент отрыва аппарата от земли?
Сложный вопрос. Боюсь, удивляться придётся. Именно для дронов я фильтры не делал, но делал для самолётов и вертолётов. Там самая большая проблема была не с наличием или отсутствием реакции опоры, а с ускорениями и вибрациями. Более или менее адекватную имитацию этих условий (в смысле погрешностей датчиков) давал автомобиль — этим я пользовался в своём диссере. Во всяком случае, автомобиль намного больше похож на самолёт, чем стол :)
Но точно не для дрона самолётного типа — там направленное движение и виражи играют слишком большую роль :)
Можно попробовать вначале вешать балластом на уже гарантированно летающего дрона не включая в управляющую схему.
Приведу здесь некоторую информацию, касательно фильтра Маджвика. Может быть, кому-нибудь она будет полезна. Это, кстати, по странному стечению обстоятельств говорит в пользу тезиса автора данного топика, о том, что не всегда стоит полагаться на готовые решения.

Я не очень сильно разбираюсь в математической подоплёке реализации фильтра, однако есть обсуждение фильтра Маджвика, где упоминается пара проблем в авторской реализации. Одна связана с быстрым обратным корнем. И вторая, как утверждается (ближе к концу обсуждения), в ошибочной реализации расчёта градиентов в версии с магнитометром для языка Си.

Выше упоминалось, о не слишком удачном опыте использования данного фильтра на arduino. Если посмотреть на одну из реализаций библиотеки для arduino, то она повторяет оригинальную авторскую версию на Си. Не знаю, может ли это быть одной из причиной. Мои эксперименты, пока, не дали однозначного ответа, насколько данные замечания критичны для работы фильтра.
Спасибо. Вопрос об ошибке в градиенте при использовании магнитометра уже поднимали в комментариях к тому посту, на который я ссылаюсь в начале. Для меня это ещё один аргумент в пользу фильтров с более «геометрическим» и наглядным корректирующим членом, чем у Маджвика.
Если кто-то интересуется работами Маджвика: у него есть три известных публикации по одноименному фильтру (2010, 2011 и 2014 гг.).
Наиболее известна публикация 2010 г. – она представляет собой отчет по НИОКР в Бристольском Университете (связанной с компьютеризированной хирургией, если правильно помню). В ней описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Приведены формулы для расчета коэффициентов фильтрации шума и систематической ошибки (дрейфа) сигналов гироскопа.
В 2011 г. Маджвик сотоварищи опубликовал вторую работу (тезисы выступления на конференции IEEE on Rehabilitation Robotics), в которой описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Кроме того, в ней приведена формула для расчета коэффициента фильтрации шума сигналов гироскопа и экспериментальные данные о влиянии величины этого коэффициента на параметры фильтра.
В 2014 г. он защитил в Бристольском Университете докторскую диссертацию на основе описанных выше разработок и затем потерял интерес к этой теме. Понятно, что диссертация – наиболее капитальная из его публикаций. Кто хочет разобраться – лучше читать именно ее.
1. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io and University of Bristol (UK) vol. 25, 113–118, 30.04.2010
2. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm. 2011 IEEE International Conference on Rehabilitation Robotics, Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 – July 1, 2011
3. Sebastian O.H. Madgwick. AHRS algorithms and calibration solutions to facilitate new applications using low-cost MEMS. PhD Theses, Department of Mechanical Engineering, University of Bristol, Bristol, UK, 2014
Если посмотреть реализацию фильтра Махони в INAV можно заметить то о чём вы рассказывали GPS или датчик воздушной скорости используется для компенсации дрейфа акселерометров в момент больших перегрузок или показания акселерометра вообще не используются:

Реализация фильтра Махони в INAV


/**
 * In Cleanflight accelerometer is aligned in the following way:
 *      X-axis = Forward
 *      Y-axis = Left
 *      Z-axis = Up
 * Our INAV uses different convention
 *      X-axis = North/Forward
 *      Y-axis = East/Right
 *      Z-axis = Up
 */

// the limit (in degrees/second) beyond which we stop integrating
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
// which results in false gyro drift. See
// http://gentlenav.googlecode.com/files/fastRotations.pdf
#define SPIN_RATE_LIMIT     20
#define MAX_ACC_SQ_NEARNESS 25      // 25% or G^2, accepted acceleration of (0.87 - 1.12G)

t_fp_vector imuAccelInBodyFrame;
t_fp_vector imuMeasuredGravityBF;
t_fp_vector imuMeasuredRotationBF;
float smallAngleCosZ = 0;

float magneticDeclination = 0.0f;       // calculated at startup from config
static bool isAccelUpdatedAtLeastOnce = false;

STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
STATIC_UNIT_TESTED float rMat[3][3];

attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800

static imuRuntimeConfig_t *imuRuntimeConfig;
static pidProfile_t *pidProfile;

static float gyroScale;

static bool gpsHeadingInitialized = false;

STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{
    float q1q1 = q1 * q1;
    float q2q2 = q2 * q2;
    float q3q3 = q3 * q3;
    
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;

    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    rMat[0][2] = 2.0f * (q1q3 - -q0q2);

    rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    rMat[1][2] = 2.0f * (q2q3 + -q0q1);

    rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}

void imuConfigure(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile)
{
    imuRuntimeConfig = initialImuRuntimeConfig;
    pidProfile = initialPidProfile;
}

void imuInit(void)
{
    int axis;

    smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
    gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second

    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        imuAccelInBodyFrame.A[axis] = 0;
    }

    imuComputeRotationMatrix();
}

void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
    float x,y,z;

    /* From body frame to earth frame */
    x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
    y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
    z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;

    v->V.X = x;
    v->V.Y = -y;
    v->V.Z = z;
}

void imuTransformVectorEarthToBody(t_fp_vector * v)
{
    float x,y,z;

    v->V.Y = -v->V.Y;
    
    /* From earth frame to body frame */
    x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z;
    y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y + rMat[2][1] * v->V.Z;
    z = rMat[0][2] * v->V.X + rMat[1][2] * v->V.Y + rMat[2][2] * v->V.Z;

    v->V.X = x;
    v->V.Y = y;
    v->V.Z = z;
}

static float invSqrt(float x)
{
    return 1.0f / sqrtf(x);
}

STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
    if (initialRoll > 1800) initialRoll -= 3600;
    if (initialPitch > 1800) initialPitch -= 3600;
    if (initialYaw > 1800) initialYaw -= 3600;

    float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
    float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);

    float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
    float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);

    float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
    float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);

    q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
    q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

    imuComputeRotationMatrix();
}

static bool imuUseFastGains(void)
{
    return !ARMING_FLAG(ARMED) && millis() < 20000;
}

static float imuGetPGainScaleFactor(void)
{
    if (imuUseFastGains()) {
        return 10.0f;
    }
    else {
        return 1.0f;
    }
}

static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                int accWeight, float ax, float ay, float az,
                                bool useMag, float mx, float my, float mz,
                                bool useCOG, float courseOverGround)
{
    static float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    // integral error terms scaled by Ki
    static float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    // integral error terms scaled by Ki
    float recipNorm;
    float ex, ey, ez;
    float qa, qb, qc;

    /* Calculate general spin rate (rad/s) */
    float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);

    /* Step 1: Yaw correction */
    // Use measured magnetic field vector
    if (useMag || useCOG) {
        float kpMag = imuRuntimeConfig->dcm_kp_mag * imuGetPGainScaleFactor();

        recipNorm = mx * mx + my * my + mz * mz;
        if (useMag && recipNorm > 0.01f) {
            // Normalise magnetometer measurement
            recipNorm = invSqrt(recipNorm);
            mx *= recipNorm;
            my *= recipNorm;
            mz *= recipNorm;

            // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
            // This way magnetic field will only affect heading and wont mess roll/pitch angles

            // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
            // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
            float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
            float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
            float bx = sqrtf(hx * hx + hy * hy);

            // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
            float ez_ef = -(hy * bx);

            // Rotate mag error vector back to BF and accumulate
            ex = rMat[2][0] * ez_ef;
            ey = rMat[2][1] * ez_ef;
            ez = rMat[2][2] * ez_ef;
        }
        else if (useCOG) {
            // Use raw heading error (from GPS or whatever else)
            while (courseOverGround >  M_PIf) courseOverGround -= (2.0f * M_PIf);
            while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);

            // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
            // (Rxx; Ryx) - measured (estimated) heading vector (EF)
            // (cos(COG), sin(COG)) - reference heading vector (EF)
            // error is cross product between reference heading and estimated heading (calculated in EF)
            float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0];

            ex = rMat[2][0] * ez_ef;
            ey = rMat[2][1] * ez_ef;
            ez = rMat[2][2] * ez_ef;
        }
        else {
            ex = 0;
            ey = 0;
            ez = 0;
        }

        // Compute and apply integral feedback if enabled
        if(imuRuntimeConfig->dcm_ki_mag > 0.0f) {
            // Stop integrating if spinning beyond the certain limit
            if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
                integralMagX += imuRuntimeConfig->dcm_ki_mag * ex * dt;    // integral error scaled by Ki
                integralMagY += imuRuntimeConfig->dcm_ki_mag * ey * dt;
                integralMagZ += imuRuntimeConfig->dcm_ki_mag * ez * dt;

                gx += integralMagX;
                gy += integralMagY;
                gz += integralMagZ;
            }
        }

        // Calculate kP gain and apply proportional feedback
        gx += kpMag * ex;
        gy += kpMag * ey;
        gz += kpMag * ez;
    }


    /* Step 2: Roll and pitch correction -  use measured acceleration vector */
    if (accWeight > 0) {
        float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor();

        // Just scale by 1G length - That's our vector adjustment. Rather than 
        // using one-over-exact length (which needs a costly square root), we already 
        // know the vector is enough "roughly unit length" and since it is only weighted
        // in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
        recipNorm = 1.0f / GRAVITY_CMSS;

        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        float fAccWeightScaler = accWeight / (float)MAX_ACC_SQ_NEARNESS;

        // Error is sum of cross product between estimated direction and measured direction of gravity
        ex = (ay * rMat[2][2] - az * rMat[2][1]) * fAccWeightScaler;
        ey = (az * rMat[2][0] - ax * rMat[2][2]) * fAccWeightScaler;
        ez = (ax * rMat[2][1] - ay * rMat[2][0]) * fAccWeightScaler;

        // Compute and apply integral feedback if enabled
        if(imuRuntimeConfig->dcm_ki_acc > 0.0f) {
            // Stop integrating if spinning beyond the certain limit
            if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
                integralAccX += imuRuntimeConfig->dcm_ki_acc * ex * dt;    // integral error scaled by Ki
                integralAccY += imuRuntimeConfig->dcm_ki_acc * ey * dt;
                integralAccZ += imuRuntimeConfig->dcm_ki_acc * ez * dt;

                gx += integralAccX;
                gy += integralAccY;
                gz += integralAccZ;
            }
        }

        // Calculate kP gain and apply proportional feedback
        gx += kpAcc * ex;
        gy += kpAcc * ey;
        gz += kpAcc * ez;
    }

    // Integrate rate of change of quaternion
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);

    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;

    // Pre-compute rotation matrix from quaternion
    imuComputeRotationMatrix();
}

STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
    /* Compute pitch/roll angles */
    attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
    attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
    attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination;

    if (attitude.values.yaw < 0)
        attitude.values.yaw += 3600;

    /* Update small angle state */
    if (rMat[2][2] > smallAngleCosZ) {
        ENABLE_STATE(SMALL_ANGLE);
    } else {
        DISABLE_STATE(SMALL_ANGLE);
    }
}

// Idea by MasterZap
static int imuCalculateAccelerometerConfidence(void)
{
    int32_t axis;
    int32_t accMagnitude = 0;

    for (axis = 0; axis < 3; axis++) {
        accMagnitude += (int32_t)accADC[axis] * accADC[axis];
    }

    // Magnitude^2 in percent of G^2
    accMagnitude = accMagnitude * 100 / ((int32_t)acc.acc_1G * acc.acc_1G);

    int32_t nearness = ABS(100 - accMagnitude);

    return (nearness > MAX_ACC_SQ_NEARNESS) ? 0 : MAX_ACC_SQ_NEARNESS - nearness;
}

static bool isMagnetometerHealthy(void)
{
    return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
}

static void imuCalculateEstimatedAttitude(float dT)
{
    float courseOverGround = 0;

    int accWeight = 0;
    bool useMag = false;
    bool useCOG = false;

    accWeight = imuCalculateAccelerometerConfidence();

    if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
        useMag = true;
    }
#if defined(GPS)
    else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
        // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
        if (gpsHeadingInitialized) {
            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
            useCOG = true;
        }
        else {
            // Re-initialize quaternion from known Roll, Pitch and GPS heading
            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
            gpsHeadingInitialized = true;
        }
    }
#endif

    imuMahonyAHRSupdate(dT,     imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
                        accWeight, imuMeasuredGravityBF.A[X], imuMeasuredGravityBF.A[Y], imuMeasuredGravityBF.A[Z],
                        useMag, magADC[X], magADC[Y], magADC[Z],
                        useCOG, courseOverGround);

    imuUpdateEulerAngles();
}

/* Calculate rotation rate in rad/s in body frame */
static void imuUpdateMeasuredRotationRate(void)
{
    int axis;

    for (axis = 0; axis < 3; axis++) {
        imuMeasuredRotationBF.A[axis] = gyroADC[axis] * gyroScale;
    }
}

/* Calculate measured acceleration in body frame cm/s/s */
static void imuUpdateMeasuredAcceleration(void)
{
    int axis;

    /* Convert acceleration to cm/s/s */
    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
        imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
    }

#ifdef GPS
    /** Centrifugal force compensation on a fixed-wing aircraft
      * a_c_body = omega x vel_tangential_body
      * assumption: tangential velocity only along body x-axis
      * assumption: GPS velocity equal to body x-axis velocity
      */
    if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
        imuMeasuredGravityBF.A[Y] -= gpsSol.groundSpeed * imuMeasuredRotationBF.A[Z];
        imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
    }
#endif
}

#ifdef HIL
void imuHILUpdate(void)
{
    /* Set attitude */
    attitude.values.roll = hilToFC.rollAngle;
    attitude.values.pitch = hilToFC.pitchAngle;
    attitude.values.yaw = hilToFC.yawAngle;

    /* Compute rotation quaternion for future use */
    imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);

    /* Fake accADC readings */
    accADC[X] = hilToFC.bodyAccel[X] * (acc.acc_1G / GRAVITY_CMSS);
    accADC[Y] = hilToFC.bodyAccel[Y] * (acc.acc_1G / GRAVITY_CMSS);
    accADC[Z] = hilToFC.bodyAccel[Z] * (acc.acc_1G / GRAVITY_CMSS);
}
#endif

void imuUpdateAccelerometer(void)
{
#ifdef HIL
    if (sensors(SENSOR_ACC) && !hilActive) {
        updateAccelerationReadings();
        isAccelUpdatedAtLeastOnce = true;
    }
#else
    if (sensors(SENSOR_ACC)) {
        updateAccelerationReadings();
        isAccelUpdatedAtLeastOnce = true;
    }
#endif
}

void imuUpdateGyroAndAttitude(void)
{
    /* Calculate dT */
    static uint32_t previousIMUUpdateTime;
    uint32_t currentTime = micros();
    float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
    previousIMUUpdateTime = currentTime;

    /* Update gyroscope */
    gyroUpdate();

    if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
#ifdef HIL
        if (!hilActive) {
            imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
            imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
        }
        else {
            imuHILUpdate();
            imuUpdateMeasuredAcceleration();
        }
#else
            imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
            imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
#endif
    } else {
        accADC[X] = 0;
        accADC[Y] = 0;
        accADC[Z] = 0;
    }
}

bool isImuReady(void)
{
    return sensors(SENSOR_ACC) && isGyroCalibrationComplete();
}

bool isImuHeadingValid(void)
{
    return (sensors(SENSOR_MAG) && persistentFlag(FLAG_MAG_CALIBRATION_DONE)) || (STATE(FIXED_WING) && gpsHeadingInitialized);
}

float calculateCosTiltAngle(void)
{
    return rMat[2][2];
}

float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
{
    if (throttleTiltCompensationStrength) {
        float tiltCompFactor = 1.0f / constrainf(rMat[2][2], 0.6f, 1.0f);  // max tilt about 50 deg
        return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
    }
    else {
        return 1.0f;
    }
}





Вопрос по фильтру Калмона может ли данный фильтр без дополнительных данных GPS или воздушной скорости самостоятельно решать когда использовать данные ускорения для востановления дрейфа гиросков а когда нет только при помощи IMU тройки? В своих тестах на коленке я замечал, что резкое линейное ускорение (от броска БЛА) в фильтре Маждвика приводит к уплыванию показаний тонгажа и крена, на BNO 055 с фильтром Калмона такой проблемы нет но зная, что в INAV для компенсации используются дополнительные датчики непонятно как это может быть реализовано без них.
Only those users with full accounts are able to leave comments. Log in, please.