Стабилизация данных с четырех гироскопов/акселерометра

Глядя на ваш код, я вижу, что единственное место, где обновляется gyro_1_angle_x, находится здесь:

gyro_1_angle_x = gyro_1_angle_x
    + (gyro_1.gyro.x - mpu_1_gyro_err_x) * elapsedTime;

Это добавляет к gyro_1_angle_x величину, которая не зависит от самого gyro_1_angle_x. Это означает, что при наличии неисправленной систематической ошибки в измерении gyro_1.gyro.x (которая всегда есть, так как вы никогда не сможете достичь идеальной калибровки) ошибки будут накапливаться, приводя к неограниченному дрейфу. Затем вы оцениваете угол крена следующим образом:

roll_1 = 0.96 * gyro_1_angle_x + 0.04 * acc_1_angle_x;

Коэффициент 0.96 незначительно уменьшает величину дрейфа (на 4%), но не меняет качественного поведения: roll_1 также подвержен неограниченному дрейфу.

Предлагаемый подход

Мое предложение будет заключаться в том, чтобы забыть об углах. Использование углов для отслеживания положения вашего устройства-плохая идея: формулы, необходимые для обновления углов, сложны (и поэтому легко ошибиться), дороги в вычислительном отношении и подвержены блокировке кардана. Для отслеживания всего отношения было бы более разумно использовать матрицы вращения или, еще лучше, кватернионы.

Однако в вашем случае вам не нужно отслеживать все отношение. У вас нет средств надежно отслеживать угол рыскания, и вы на самом деле даже не заботитесь об этом угле. Вас интересует только ориентация устройства относительно вертикального направления. Поэтому я предлагаю вам даже не пытаться отслеживать отношение, а вместо этого просто отслеживать вертикальное направление. Это направление задается координатами вектора гравитации в системе отсчета прибора. Тогда у вас есть только один вектор для отслеживания. Никаких углов, никаких матриц, никаких кватернионов - просто простой вектор.

Использование акселерометров

Акселерометры чувствительны к гравитации, поэтому вы можете использовать их показания в качестве оценки вектора гравитации, как в следующем псевдокоде:

at each time step:
    (accel, gyro) = read_MPU();
    gravity = accel;

Это может дать вам противоположность гравитации (направление зенита). Это нормально, если вы знаете, какой вектор вы отслеживаете.

Однако в этом подходе есть одна проблема. Акселерометры чувствительны не только к гравитации, но и к ускорению (отсюда и название). Если ваш объект движется, его ускорение вызовет ошибку в оценке вектора гравитации. Оценка плохая в краткосрочной перспективе, но она должна быть хорошей в долгосрочной перспективе: поскольку ваш объект вряд ли будет поддерживать устойчивое ускорение, среднее время показаний акселерометра должно точно соответствовать вектору гравитации.

Использование гироскопов

Чтобы избавиться от ошибок, вызванных ускорением, вы можете вместо этого использовать показания гироскопа. Предполагая, что вам каким-то образом удастся получить начальную оценку вектора гравитации, вы можете использовать показания гироскопа для итеративного обновления этой оценки:

at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);

здесь вращение-это вектор вращения, описывающий, насколько сильно гравитация повернулась с предыдущего временного шага. Знак минус в его формуле исходит из того факта, что гироскоп дает вам вращение устройства относительно неподвижной системы отсчета, тогда как вы хотите вращение неподвижного вектора относительно системы отсчета устройства. Функция apply_rotation() применит формулу вращения Родригеса, чтобы повернуть вектор гравитации в его новую ориентацию.

Этот подход не страдает от ошибок, вызванных ускорением. Однако он склонен к дрейфу, так как объединение большого количества небольших вращений приводит к накоплению большого количества мелких ошибок.

Объединение данных гироскопа и акселерометра

Подход акселерометра склонен к кратковременным ошибкам, в то время как подход гироскопа склонен к долгосрочному дрейфу. Вы можете попытаться объединить эти два подхода и использовать акселерометры для определения долгосрочного поведения вашей оценки гравитации, в то время как гироскопы определяют ее краткосрочное поведение. Для этого вы должны на каждом временном шаге:

• используйте данные гироскопа для поворота текущей оценки вектора гравитации
• затем осторожно “перемешайте” его в направлении показаний акселерометра.

В псевдокоде это будет выглядеть так:

at each time step:
    (accel, gyro) = read_MPU();
    dt = time_since_previous_time_step;
    rotation = - dt * gyro;
    gravity = apply_rotation(rotation, gravity);
    gravity = gravity + (dt/tau) * (accel - gravity)

Последняя строка имеет эффект незначительного подталкивания оценки гравитации к показаниям акселерометра. Обратите внимание, что эта строка выглядит как реализация фильтра нижних частот первого порядка с постоянной времени тау. Эта постоянная времени определяет границу между “короткими” временными шкалами, где вы доверяете гироскопам больше, чем акселерометрам, и “длинными” временными шкалами, где вы больше доверяете акселерометрам. Значение тау должно быть выбрано экспериментально. Он управляет компромиссом между шумом, исходящим от ускорения, и систематической ошибкой , исходящей от гироскопов.

С помощью этой формулы накопленные ошибки гироскопа всегда будут оставаться связанными, а общая ошибка пропорциональна тау. Поскольку пошаговые вращения , по-видимому, очень малы, и поскольку ошибки вращения не накапливаются неограниченным образом, вы, вероятно, можете избежать использования линеаризации формулы вращения Родригеса (cos θ = 1, sin θ = θ):

gravity = gravity + rotation × gravity

и тогда вам вообще не нужна тригонометрия.

Стоит также отметить, что при таком подходе можно смело инициализировать гравитацию до нуля: она сразу же начнет расти по мере выполнения программы, и ее величина со временем сойдется к соответствующему значению.

Получение подачи и крена

Как только вы знаете вектор гравитации, вы можете легко вычислить тангаж и крен. Я не говорю, что вы должны полностью избегать использования углов: вероятно, вам нужны углы в вашем пользовательском интерфейсе. Я хочу сказать, что вы должны воздерживаться от использования углов для отслеживания ориентации устройства. Вычисление углов в качестве шага постобработки- это прекрасно.

На самом деле вы можете с таким же успехом вычислить эти углы в компьютере, который выполняет окончательную обработку данных. ESP могут просто отправить свою лучшую оценку вектора гравитации и позволить компьютеру сделать тригонометрию.