13 de julio de 2026Escrito por Francisco Castel

Fusión de Sensores con Filtro de Kalman: Implementación Práctica en ESP32 para Estimación de Actitud

Sistemas EmbebidosTeoría de ControlESP32Robótica

Introducción al Problema de Estimación de Actitud

En la ingeniería mecatrónica, conocer con precisión la orientación (actitud) de un sistema en el espacio es crítico. Ya sea para estabilizar un dron, controlar un péndulo invertido o guiar un vehículo autónomo, dependemos de las Unidades de Medición Inercial (IMU).

Sin embargo, los sensores físicos presentan limitaciones inherentes:

El Acelerómetro: Es altamente preciso a largo plazo, pero muy ruidoso a corto plazo debido a las vibraciones mecánicas del sistema.
El Giroscopio: Es extremadamente preciso a corto plazo, pero al integrar la velocidad angular para obtener el ángulo, acumula un error con el tiempo conocido como deriva (drift).

Para resolver esto, recurrimos a la fusión de sensores. Aunque un Filtro Complementario es sencillo de implementar, carece de la optimización estocástica que ofrece el Filtro de Kalman, el cual minimiza la covarianza del error de estimación bajo ruido gaussiano.

La Matemática del Filtro de Kalman Lineal de 2 Variables

Queremos estimar el ángulo real ( $\theta$ ) y el sesgo (bias) del giroscopio ( $b$ ). Definimos nuestro vector de estado como:

$x_k = \begin{bmatrix} \theta \\ b \end{bmatrix}$

1. Modelo de Transición de Estados (Predicción)

El giroscopio mide la velocidad angular de entrada $\dot{\theta}_y$ en el tiempo $k$ . La relación física entre el ángulo actual y el anterior, considerando el sesgo del giroscopio, viene dada por:

$\theta_k = \theta_{k-1} + \Delta t \cdot (\dot{\theta}_{y,k} - b_{k-1})$ $b_k = b_{k-1}$

En forma matricial, el modelo predictivo de estado $x_k = F x_{k-1} + B u_k$ es:

$\begin{bmatrix} \theta \\ b \end{bmatrix}_k = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \theta \\ b \end{bmatrix}_{k-1} + \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \dot{\theta}_{y,k}$

Donde:

$F$ (Matriz de transición de estado): $F = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix}$
$B$ (Matriz de entrada): $B = \begin{bmatrix} \Delta t \\ 0 \end{bmatrix}$
$u_k$ (Entrada de control): la lectura cruda del giroscopio $\dot{\theta}_{y,k}$ .

2. Modelo de Medición (Actualización)

Nuestra única observación directa del ángulo absoluto proviene del acelerómetro ( $z_k = \theta_{acc}$ ). La matriz de observación $H$ relaciona el estado con la medición:

$z_k = H x_k + v_k$ $\theta_{acc} = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} \theta \\ b \end{bmatrix} + v_k$

Donde $H = \begin{bmatrix} 1 & 0 \end{bmatrix}$ y $v_k$ es el ruido de medición del acelerómetro.

Algoritmo de Cómputo del Filtro

El filtro opera en un bucle cerrado de dos etapas esenciales:

Paso de Predicción:

Predicción del Estado:

$\hat{x}_{k\mid k-1} = F \hat{x}_{k-1\mid k-1} + B u_k$

Predicción de la Covarianza del Error ( $P$ ):

$P_{k\mid k-1} = F P_{k-1\mid k-1} F^T + Q$

Donde $Q$ es la matriz de covarianza del ruido del proceso (incertidumbre en el modelo del acelerómetro y el sesgo).

Paso de Actualización:

Innovación de la Medición:

$\tilde{y}_k = z_k - H \hat{x}_{k\mid k-1}$

Covarianza de la Innovación ( $S$ ):

$S_k = H P_{k\mid k-1} H^T + R$

Donde $R$ es la covarianza del ruido de medición (ruido del acelerómetro). 3. Ganancia de Kalman óptima ( $K$ ):

$K_k = P_{k\mid k-1} H^T S_k^{-1}$

Actualización del Estado Estimado:

$\hat{x}_{k\mid k} = \hat{x}_{k\mid k-1} + K_k \tilde{y}_k$

Actualización de la Covarianza del Error:

$P_{k\mid k} = (I - K_k H) P_{k\mid k-1}$

Implementación en C++ para ESP32

A continuación se presenta una implementación robusta orientada a objetos en C++, diseñada para ejecutarse eficientemente en la arquitectura de 32 bits del microcontrolador ESP32 de Espressif Systems.

#include <Wire.h>

class KalmanFilter2D {
private:
    // Parámetros de ruido (Sintonizables)
    float Q_angle;   // Covarianza del ruido de proceso para el ángulo
    float Q_bias;    // Covarianza del ruido de proceso para el sesgo
    float R_measure; // Covarianza del ruido de medición

    float angle; // Ángulo estimado (Estado x_0)
    float bias;  // Sesgo del giroscopio estimado (Estado x_1)
    
    // Matriz de covarianza del error P (2x2)
    float P[2][2];

public:
    KalmanFilter2D(float q_ang = 0.001f, float q_bias = 0.003f, float r_meas = 0.03f) {
        Q_angle = q_ang;
        Q_bias = q_bias;
        R_measure = r_meas;

        angle = 0.0f;
        bias = 0.0f;

        P[0][0] = 0.0f; P[0][1] = 0.0f;
        P[1][0] = 0.0f; P[1][1] = 0.0f;
    }

    // Retorna el ángulo filtrado
    // newRate: lectura del giroscopio (deg/s o rad/s)
    // newAngle: ángulo calculado por el acelerómetro (deg o rad)
    // dt: diferencial de tiempo en segundos
    float update(float newAngle, float newRate, float dt) {
        // 1. Predicción del Estado (a priori)
        float rate = newRate - bias;
        angle += dt * rate;

        // 2. Predicción de la Covarianza del Error P
        P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
        P[0][1] -= dt * P[1][1];
        P[1][0] -= dt * P[1][1];
        P[1][1] += Q_bias * dt;

        // 3. Ganancia de Kalman K
        float S = P[0][0] + R_measure;
        float K[2];
        K[0] = P[0][0] / S;
        K[1] = P[1][0] / S;

        // 4. Actualización del Estado (a posteriori)
        float y = newAngle - angle; // Innovación
        angle += K[0] * y;
        bias  += K[1] * y;

        // 5. Actualización de la Covarianza P
        float P00_temp = P[0][0];
        float P01_temp = P[0][1];

        P[0][0] -= K[0] * P00_temp;
        P[0][1] -= K[0] * P01_temp;
        P[1][0] -= K[1] * P00_temp;
        P[1][1] -= K[1] * P01_temp;

        return angle;
    }
};

// Instancia del filtro
KalmanFilter2D kalmanPitch;

// Direcciones del MPU6050
const int MPU_ADDR = 0x68;
int16_t raw_ax, raw_ay, raw_az;
int16_t raw_gx, raw_gy, raw_gz;

unsigned long lastTime;

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22); // Pines SDA y SCL para ESP32
    
    // Inicializar MPU6050
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x6B); // Registro PWR_MGMT_1
    Wire.write(0);    // Despertar el sensor
    Wire.endTransmission(true);

    lastTime = micros();
}

void loop() {
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x3B); // Registro de inicio de datos del acelerómetro
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_ADDR, 14, true);

    // Leer datos crudos
    raw_ax = Wire.read() << 8 | Wire.read();
    raw_ay = Wire.read() << 8 | Wire.read();
    raw_az = Wire.read() << 8 | Wire.read();
    Wire.read() << 8 | Wire.read(); // Saltear temperatura
    raw_gx = Wire.read() << 8 | Wire.read();
    raw_gy = Wire.read() << 8 | Wire.read();
    raw_gz = Wire.read() << 8 | Wire.read();

    // Calcular Delta T
    unsigned long currentTime = micros();
    float dt = (float)(currentTime - lastTime) / 1000000.0f;
    lastTime = currentTime;

    // Convertir lecturas del acelerómetro a ángulo de Pitch (en grados)
    // pitch = atan2(ay, az) transformada a grados
    float accelPitch = atan2((float)raw_ay, (float)raw_az) * 180.0 / M_PI;

    // Convertir velocidad del giroscopio a grados por segundo
    float gyroRatePitch = (float)raw_gx / 131.0f; // Con escala +/- 250 deg/s

    // Procesar la fusión inercial con el filtro de Kalman
    float filteredPitch = kalmanPitch.update(accelPitch, gyroRatePitch, dt);

    // Graficar para depuración (Teleplot o Serial Plotter)
    Serial.print("Acc:"); Serial.print(accelPitch);
    Serial.print(",");
    Serial.print("Kalman:"); Serial.println(filteredPitch);

    delay(10); // Ciclo de muestreo (~100Hz)
}

Análisis de Resultados y Sintonización (Tuning)

El rendimiento óptimo del Filtro de Kalman depende de la sintonización fina de las covarianzas de ruido:

$R_measure$ (Ruido del Acelerómetro): Si aumentas este valor, le estás diciendo al filtro que el acelerómetro no es de confianza; la estimación se volverá muy suave pero reaccionará más lento a los cambios estructurales.
$Q_angle$ (Ruido del Ángulo en el Proceso): Indica qué tan confiable es nuestro modelo dinámico predictivo. Si se incrementa, el filtro confiará más en las mediciones instantáneas del acelerómetro, reduciendo la suavidad.

En el ESP32, gracias a su coprocesador de punto flotante (FPU), realizar estas operaciones matriciales toma menos de 15 microsegundos, lo que permite frecuencias de muestreo de hasta 1 kHz sin sobrecargar el procesador.