Quando você pega um giroscópio e integra a velocidade angular ao longo do tempo, você obtém atitude (orientação). O problema é que “representar atitude” com ângulos de Euler (roll/pitch/yaw) parece intuitivo, mas dá dor de cabeça em firmware: aparece gimbal lock (singularidades), as equações ficam cheias de senos/cossenos acoplados e pequenas imprecisões numéricas explodem mais fácil. Quaternions entram justamente para representar rotações 3D de forma estável, contínua e eficiente: você atualiza a orientação com multiplicações e normalizações, e só converte para Euler no final, se precisar mostrar em tela ou enviar telemetria.
Um quaternion unitário pode ser escrito como (q = [q_w,; q_x,; q_y,; q_z]), onde (q_w) é a parte escalar e \([q_x,q_y,q_z]\) é a parte vetorial. Para atitude, usamos quaternion unitário, isto é, \(||q||=1\). Isso é crucial: se o módulo “anda”, sua rotação deixa de ser pura e você começa a introduzir distorção numérica. Por isso, em sistemas embarcados, normalizar o quaternion frequentemente não é luxo; é parte do controle de erro.
A ideia central: derivada do quaternion a partir do giroscópio
Se o giroscópio mede \(\boldsymbol{\omega} = [\omega_x,\omega_y,\omega_z]\) em rad/s no referencial do corpo, a cinemática do quaternion é:
\[
\dot{q} = \frac{1}{2}; q \otimes \Omega
\quad \text{onde}\quad
\Omega = [0,;\omega_x,;\omega_y,;\omega_z]
\]
e \(\otimes\) é o produto de quaternions. Discretizando com passo (dt):
\[
q_{k+1} \approx q_k + \dot{q}; dt
\]
e depois você normaliza \(q_{k+1}\).
Isso sozinho já dá um “filtro”: é a integração do giro. Só que o giroscópio tem bias (deslocamento DC) e ruído. A longo prazo, o erro cresce. É aí que entram acelerômetro e magnetômetro como “âncoras” para corrigir drift.
Como o acelerômetro ajuda sem “medir rotação”
O acelerômetro mede “aceleração específica”, que em baixa dinâmica (ou com filtragem) aproxima a direção da gravidade. Ele não te dá yaw (guinada) absoluto, mas dá um ótimo pitch/roll de referência. Já o magnetômetro dá yaw absoluto (com todos os cuidados de calibração/soft-iron/hard-iron).
Madgwick e Mahony fazem a mesma coisa em espírito: usam o erro entre o que o sensor “deveria ver” pela orientação atual e o que ele realmente vê, e retroalimentam isso no giro. A diferença é o jeito matemático dessa retroalimentação.
Madgwick: correção por gradiente (rápida e boa para MCU)
No Madgwick, você ajusta o quaternion para minimizar uma função de erro baseada na diferença entre o vetor gravidade previsto e o gravidade medido (e opcionalmente o campo magnético). A implementação clássica aplica um termo de “descida do gradiente” (\nabla f) e mistura isso na atualização:
\[
\dot{q} = \frac{1}{2}; q \otimes \Omega ;-; \beta ; \frac{\nabla f}{||\nabla f||}
\]
O ganho (\beta) controla quanta “força” você dá para o acelerômetro/magnetômetro corrigirem o drift do giro. Em prática embarcada: (\beta) maior corrige mais rápido, mas pode injetar ruído quando o acelerômetro está sob aceleração linear (robôs, drones, motores vibrando).
Abaixo vai um exemplo IMU-only (gyro + accel), que é a base mais usada em projetos de equilíbrio e estimativa de inclinação. Está em C “puro”, pensado para rodar em Cortex-M/RISC-V sem dependências. (A versão com magnetômetro é uma extensão direta, mas mais longa por causa do modelo do campo magnético.)
#include <math.h>
#include <stdint.h>
typedef struct {
float w, x, y, z;
} quat_t;
static inline float inv_sqrt(float x) {
return 1.0f / sqrtf(x);
}
static inline void quat_normalize(quat_t *q) {
float n = q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z;
if (n > 0.0f) {
float inv = inv_sqrt(n);
q->w *= inv; q->x *= inv; q->y *= inv; q->z *= inv;
}
}
static inline quat_t quat_mul(quat_t a, quat_t b) {
quat_t r;
r.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
r.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
r.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
r.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
return r;
}
/**
* Madgwick IMU update (gyro [rad/s], accel [g] ou m/s^2 normalizado).
* q: orientação do corpo em relação ao referencial inercial (convenção típica).
* beta: ganho da correção (comece em algo como 0.05..0.2 dependendo do ruído/dinâmica).
* dt: período de amostragem em segundos.
*/
void madgwick_update_imu(quat_t *q,
float gx, float gy, float gz,
float ax, float ay, float az,
float beta, float dt)
{
// Se accel for zero (sensor saturado/desligado), integra apenas giro
float a2 = ax*ax + ay*ay + az*az;
if (a2 <= 1e-12f) {
quat_t omega = {0.0f, gx, gy, gz};
quat_t qdot = quat_mul(*q, omega);
qdot.w *= 0.5f; qdot.x *= 0.5f; qdot.y *= 0.5f; qdot.z *= 0.5f;
q->w += qdot.w * dt; q->x += qdot.x * dt;
q->y += qdot.y * dt; q->z += qdot.z * dt;
quat_normalize(q);
return;
}
// Normaliza aceleração (direção da gravidade medida)
float inva = inv_sqrt(a2);
ax *= inva; ay *= inva; az *= inva;
// Atalhos
float q1 = q->w, q2 = q->x, q3 = q->y, q4 = q->z;
// Função de erro (IMU): gravidade prevista vs medida
// A gravidade prevista (no corpo) pode ser derivada do quaternion.
// Aqui usamos a forma popular do algoritmo (gradiente já “embutido”).
float _2q1 = 2.0f*q1;
float _2q2 = 2.0f*q2;
float _2q3 = 2.0f*q3;
float _2q4 = 2.0f*q4;
float _4q1 = 4.0f*q1;
float _4q2 = 4.0f*q2;
float _4q3 = 4.0f*q3;
float _8q2 = 8.0f*q2;
float _8q3 = 8.0f*q3;
float q1q1 = q1*q1;
float q2q2 = q2*q2;
float q3q3 = q3*q3;
float q4q4 = q4*q4;
// Gradiente (s1..s4)
float s1 = _4q1*q3q3 + _2q3*ax + _4q1*q2q2 - _2q2*ay;
float s2 = _4q2*q4q4 - _2q4*ax + 4.0f*q1q1*q2 - _2q1*ay - _4q2 + _8q2*q2q2 + _8q2*q3q3 + _4q2*az;
float s3 = 4.0f*q1q1*q3 + _2q1*ax + _4q3*q4q4 - _2q4*ay - _4q3 + _8q3*q2q2 + _8q3*q3q3 + _4q3*az;
float s4 = 4.0f*q2q2*q4 - _2q2*ax + 4.0f*q3q3*q4 - _2q3*ay;
// Normaliza o gradiente para estabilidade numérica
float s2n = s1*s1 + s2*s2 + s3*s3 + s4*s4;
if (s2n > 1e-12f) {
float invs = inv_sqrt(s2n);
s1 *= invs; s2 *= invs; s3 *= invs; s4 *= invs;
}
// qdot = 0.5*q⊗omega - beta*s
quat_t omega = {0.0f, gx, gy, gz};
quat_t qdot = quat_mul(*q, omega);
qdot.w *= 0.5f; qdot.x *= 0.5f; qdot.y *= 0.5f; qdot.z *= 0.5f;
qdot.w -= beta * s1;
qdot.x -= beta * s2;
qdot.y -= beta * s3;
qdot.z -= beta * s4;
// Integra
q->w += qdot.w * dt; q->x += qdot.x * dt;
q->y += qdot.y * dt; q->z += qdot.z * dt;
// Mantém unitário
quat_normalize(q);
}
Esse núcleo já resolve a parte mais importante: você sai de uma IMU crua para uma orientação estável, com drift bem menor que integração pura do giro. Em produto real, o que mais derruba qualidade não é “o algoritmo em si”, e sim: unidade errada (graus/s em vez de rad/s), (dt) inconsistente, aceleração linear contaminando o acelerômetro, e falta de calibração do bias do giro.
Mahony: realimentação PI no erro (controle clássico e intuitivo)
O Mahony parece, conceitualmente, um controle de atitude: você calcula um vetor de erro (\mathbf{e}) como produto vetorial entre a gravidade medida e a gravidade prevista, e usa isso para corrigir o giroscópio. A forma típica faz:
\[
\boldsymbol{\omega}_{corr} = \boldsymbol{\omega} + K_p \mathbf{e} + K_i \int \mathbf{e},dt
\]
Depois você integra o quaternion usando (\boldsymbol{\omega}_{corr}). A sacada é que o termo integral atua como estimativa do bias do giroscópio. Em firmware, isso costuma ficar muito robusto quando o dispositivo tem períodos frequentes de “quase-estático” (por exemplo, robô que para, equipamento fixo, wearable em repouso).
Exemplo IMU-only em C:
#include <math.h>
#include <stdint.h>
typedef struct { float w, x, y, z; } quat_t;
static inline float inv_sqrt(float x) { return 1.0f / sqrtf(x); }
static inline void quat_normalize(quat_t *q) {
float n = q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z;
if (n > 0.0f) {
float inv = inv_sqrt(n);
q->w *= inv; q->x *= inv; q->y *= inv; q->z *= inv;
}
}
static inline quat_t quat_mul(quat_t a, quat_t b) {
quat_t r;
r.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
r.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
r.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;
r.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;
return r;
}
/**
* Mahony IMU update.
* kp: ganho proporcional (correção rápida).
* ki: ganho integral (estimativa de bias).
* integral_fb: estado do integrador (persistente).
*/
void mahony_update_imu(quat_t *q,
float gx, float gy, float gz,
float ax, float ay, float az,
float kp, float ki, float dt,
float integral_fb[3])
{
float a2 = ax*ax + ay*ay + az*az;
if (a2 <= 1e-12f) {
// Sem accel válido: integra giro sem correção
quat_t omega = {0.0f, gx, gy, gz};
quat_t qdot = quat_mul(*q, omega);
qdot.w *= 0.5f; qdot.x *= 0.5f; qdot.y *= 0.5f; qdot.z *= 0.5f;
q->w += qdot.w * dt; q->x += qdot.x * dt;
q->y += qdot.y * dt; q->z += qdot.z * dt;
quat_normalize(q);
return;
}
// Normaliza accel (direção gravidade medida)
float inva = inv_sqrt(a2);
ax *= inva; ay *= inva; az *= inva;
// Gravidade prevista no corpo a partir do quaternion (convenção comum)
// v = [2(qx qz - qw qy), 2(qw qx + qy qz), qw^2 - qx^2 - qy^2 + qz^2]
float qw = q->w, qx = q->x, qy = q->y, qz = q->z;
float vx = 2.0f*(qx*qz - qw*qy);
float vy = 2.0f*(qw*qx + qy*qz);
float vz = qw*qw - qx*qx - qy*qy + qz*qz;
// Erro = (gravidade medida) x (gravidade prevista)
// e = a × v
float ex = (ay*vz - az*vy);
float ey = (az*vx - ax*vz);
float ez = (ax*vy - ay*vx);
// Integra erro para estimar bias (termo I)
if (ki > 0.0f) {
integral_fb[0] += ki * ex * dt;
integral_fb[1] += ki * ey * dt;
integral_fb[2] += ki * ez * dt;
} else {
integral_fb[0] = integral_fb[1] = integral_fb[2] = 0.0f;
}
// Corrige giro
gx += kp * ex + integral_fb[0];
gy += kp * ey + integral_fb[1];
gz += kp * ez + integral_fb[2];
// Integra quaternion com giro corrigido
quat_t omega = {0.0f, gx, gy, gz};
quat_t qdot = quat_mul(*q, omega);
qdot.w *= 0.5f; qdot.x *= 0.5f; qdot.y *= 0.5f; qdot.z *= 0.5f;
q->w += qdot.w * dt; q->x += qdot.x * dt;
q->y += qdot.y * dt; q->z += qdot.z * dt;
quat_normalize(q);
}
Na prática, você escolhe (K_p) para “colar” o pitch/roll rapidamente no acelerômetro quando estiver estável, e (K_i) pequeno para não explodir em dinâmica. Um detalhe de firmware que faz diferença: se o seu sistema passa longos períodos sob aceleração linear (por exemplo, um robô andando), vale reduzir (K_p) adaptativamente quando (|\mathbf{a}|) se afasta muito de 1g, porque isso indica que o acelerômetro não está “vendo só gravidade”.
Convertendo quaternion para Euler (só para exibir / log)
Mesmo que você use quaternion internamente, muitas interfaces querem Euler. A conversão típica (uma convenção comum) é:
#include <math.h>
void quat_to_euler_rad(const quat_t *q, float *roll, float *pitch, float *yaw)
{
float qw = q->w, qx = q->x, qy = q->y, qz = q->z;
// roll (x-axis rotation)
float sinr_cosp = 2.0f * (qw*qx + qy*qz);
float cosr_cosp = 1.0f - 2.0f * (qx*qx + qy*qy);
*roll = atan2f(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
float sinp = 2.0f * (qw*qy - qz*qx);
if (fabsf(sinp) >= 1.0f) {
*pitch = copysignf((float)M_PI / 2.0f, sinp); // saturação
} else {
*pitch = asinf(sinp);
}
// yaw (z-axis rotation)
float siny_cosp = 2.0f * (qw*qz + qx*qy);
float cosy_cosp = 1.0f - 2.0f * (qy*qy + qz*qz);
*yaw = atan2f(siny_cosp, cosy_cosp);
}
Note que Euler pode ter singularidade em pitch perto de ±90°, mas isso não “estraga” o filtro se você só usa Euler como saída final.
Madgwick vs Mahony: quando escolher qual (na vida real)
Se você quer um filtro muito direto, rápido, com comportamento excelente em MCU e fácil de portar, Madgwick é normalmente uma escolha “plugável”, principalmente na versão IMU-only. Mahony costuma brilhar quando você quer uma interpretação mais “controle PI”, com ajuste intuitivo de (K_p/K_i) e boa correção de bias em cenários com trechos estáveis. Em ambos, a diferença entre “funciona” e “fica profissional” quase sempre está menos no algoritmo e mais na engenharia: calibração do giroscópio, sincronização de amostragem, filtragem do acelerômetro, e consistência de (dt).
