14 de julio de 2026Escrito por Francisco Castel

Implementación Práctica de un Filtro de Kalman Lineal en ESP32 para Fusión de Sensores

ESP32Sistemas EmbebidosFiltro de KalmanRobótica

Introducción a la Fusión de Sensores en Mecatrónica

En el desarrollo de robots móviles, drones y sistemas de transporte personal (como los vehículos de auto-balanceo), estimar la orientación (pitch, roll y yaw) de forma precisa es crítico. La herramienta por excelencia para esto es la Unidad de Medición Inercial (IMU), que típicamente combina un acelerómetro de 3 ejes y un giroscopio de 3 ejes.

Sin embargo, ambos sensores presentan limitaciones físicas individuales:

El Acelerómetro: Mide la aceleración de la gravedad para calcular el ángulo estático. No obstante, es extremadamente susceptible al ruido de alta frecuencia provocado por vibraciones mecánicas y movimientos bruscos.
El Giroscopio: Mide la velocidad angular. Integrando este valor en el tiempo se obtiene el ángulo. Sin embargo, debido a imperfecciones térmicas y mecánicas, sufre de deriva (drift), acumulando un error que crece exponencialmente en el tiempo.

Para resolver esto, recurrimos a la fusión de sensores. Mientras que un Filtro Complementario es una solución simple de primer orden, el Filtro de Kalman proporciona una estimación óptima basada en el modelado estadístico del ruido del sistema y de la medición.

El Modelo Matemático del Filtro de Kalman (1D)

Modelaremos el estado de inclinación angular de un solo eje. Definimos nuestro vector de estado como:

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

Donde:

$\theta$ es el ángulo real estimado.
$b$ es el sesgo o bias del giroscopio (el error sistemático que causa la deriva).

1. Ecuación de Transición de Estado (Predicción)

La física del sistema nos dice que el nuevo ángulo es el ángulo anterior más la velocidad angular corregida multiplicada por el paso de tiempo ( $\Delta t$ ):

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

En forma matricial, el modelo de predicción es:

$x_k = A x_{k-1} + B u_k + w_k$

$\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}_k + w_k$

Donde $\dot{\theta}_k$ es la velocidad angular medida por el giroscopio (nuestra entrada de control $u_k$ ), y $w_k$ es el ruido del proceso con matriz de covarianza $Q$ :

$Q = \begin{bmatrix} Q_{\theta} & 0 \\ 0 & Q_b \end{bmatrix}$

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

La única medición directa que tenemos del ángulo proviene del acelerómetro ( $z_k$ ). Por lo tanto, nuestra matriz de observación $H$ es:

$H = \begin{bmatrix} 1 & 0 \end{bmatrix}$

$z_k = H x_k + v_k$

Donde $v_k$ es el ruido de medición del acelerómetro, con varianza $R = \text{cov}(v_k)$ .

Implementación en C++ para ESP32

A continuación, se presenta la implementación de la clase KalmanFilter1D. Esta clase está optimizada para ejecutarse en microcontroladores de 32 bits como el ESP32, minimizando el uso de librerías de álgebra lineal complejas mediante la resolución analítica de las ecuaciones matriciales.

#ifndef KALMAN_H
#define KALMAN_H

class KalmanFilter1D {
private:
    // Parámetros de ruido (Covarianzas)
    float Q_angle;   // Varianza del ruido del proceso para el ángulo
    float Q_bias;    // Varianza del ruido del proceso para el sesgo del giroscopio
    float R_measure; // Varianza del ruido de medición del acelerómetro

    // Variables de estado
    float angle; // Ángulo estimado (Estado x[0])
    float bias;  // Sesgo estimado (Estado x[1])
    float rate;  // Velocidad angular no sesgada

    // Matriz de covarianza del error de estimación P
    float P[2][2];

public:
    KalmanFilter1D(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
    // newAngle: ángulo calculado por el acelerómetro (grados o radianes)
    // newRate: velocidad angular del giroscopio (grados/s o radianes/s)
    // dt: delta de tiempo en segundos
    float update(float newAngle, float newRate, float dt) {
        // PASO 1: Predicción
        rate = newRate - bias;
        angle += dt * rate;

        // Actualización de la covarianza del error P = A*P*A' + Q
        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;

        // PASO 2: Actualización (Corrección)
        // Diferencia de medición (Innovación)
        float y = newAngle - angle;

        // Covarianza de la innovación S = H*P*H' + R
        float S = P[0][0] + R_measure;

        // Ganancia de Kalman K = P*H' * inv(S)
        float K[2];
        K[0] = P[0][0] / S;
        K[1] = P[1][0] / S;

        // Actualizar estimación del estado x = x + K*y
        angle += K[0] * y;
        bias  += K[1] * y;

        // Actualizar la covarianza del error P = (I - K*H)*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;
    }
};

#endif

Integración en el Loop de Control del ESP32

Para garantizar un cálculo preciso de $\Delta t$ , utilizaremos el temporizador interno del ESP32 mediante la función micros(). A continuación, se muestra cómo integrar el filtro leyendo una IMU convencional (como la MPU6050 vía el protocolo I2C usando la librería estándar Wire):

#include <Wire.h>
#include "Kalman.h"

// Direcciones de registro de la MPU6050
const int MPU_ADDR = 0x68;
KalmanFilter1D kalmanX;

unsigned long lastTime;
float accX, accY, accZ;
float gyroX, gyroY, gyroZ;

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22); // SDA en GPIO 21, SCL en GPIO 22 del ESP32
    
    // Inicializar MPU6050
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x6B); // Registro PWR_MGMT_1
    Wire.write(0);    // Despertar sensor
    Wire.endTransmission(true);
    
    lastTime = micros();
}

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

    int16_t rawAccX = (Wire.read() << 8) | Wire.read();
    int16_t rawAccY = (Wire.read() << 8) | Wire.read();
    int16_t rawAccZ = (Wire.read() << 8) | Wire.read();
    int16_t rawTemp = (Wire.read() << 8) | Wire.read(); // No utilizado
    int16_t rawGyroX = (Wire.read() << 8) | Wire.read();

    // Conversión física (Asumiendo escalas por defecto: Accel +/- 2g, Gyro +/- 250 deg/s)
    accY = rawAccY / 16384.0;
    accZ = rawAccZ / 16384.0;
    gyroX = rawGyroX / 131.0;

    // Calcular ángulo de inclinación crudo según el acelerómetro
    // atan2 devuelve el ángulo en radianes, lo convertimos a grados
    float rollAccel = atan2(accY, accZ) * 180.0 / PI;

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

    // Aplicar el Filtro de Kalman
    float filteredRoll = kalmanX.update(rollAccel, gyroX, dt);

    // Imprimir datos para telemetría serial
    Serial.print("Medicion_Accel:");
    Serial.print(rollAccel);
    Serial.print(",");
    Serial.print("Filtro_Kalman:");
    Serial.println(filteredRoll);

    delay(10); // Frecuencia de muestreo aprox. 100 Hz
}

Calibración y Sintonización del Filtro

El comportamiento dinámico del filtro de Kalman depende enteramente de la relación entre la matriz de covarianza del ruido de proceso ( $Q$ ) y la varianza del ruido de medición ( $R$ ).

Si aumentas $R$ ( $R_\text{measure}$ ): Le indicas al filtro que el acelerómetro es muy ruidoso. El filtro ignorará las oscilaciones rápidas del acelerómetro y confiará más en la integración del giroscopio. Esto suaviza la señal pero puede causar retraso (delay) temporal en el seguimiento dinámico.
Si aumentas $Q$ ( $Q_\text{angle}$ ): Le indicas al filtro que el modelo dinámico cambia rápidamente de forma imprevista. El filtro reaccionará mucho más rápido a las mediciones del acelerómetro, volviéndose más ruidoso pero con menor retardo.