Fusión de Sensores con Filtro de Kalman de 2 Estados: Teoría e Implementación en C++ para Microcontroladores

En el diseño de robots móviles, vehículos aéreos no tripulados (UAVs) y sistemas de control de actitud (como plataformas auto-balanceadas), la estimación precisa de la orientación (pitch/roll) es fundamental.

Los sensores inerciales (IMUs) de bajo costo, como el MPU6050 o el ISM330DLC, presentan limitaciones físicas individuales:

1. El Acelerómetro mide la gravedad para determinar el ángulo, pero es extremadamente sensible al ruido de alta frecuencia provocado por la vibración de los motores.
2. El Giroscopio mide la velocidad angular. Es inmune a las vibraciones rápidas, pero al integrar la señal en el tiempo para obtener el ángulo, acumula un error sistemático conocido como drift (deriva) de baja frecuencia.

Para resolver esto, utilizamos un Filtro de Kalman Lineal de 2 estados. Este algoritmo fusiona de manera matemáticamente óptima ambas señales, aprovechando la estabilidad a largo plazo del acelerómetro y la precisión a corto plazo del giroscopio.

---

1. Fundamento Matemático del Filtro

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

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

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

La ecuación que gobierna la dinámica de nuestro sistema, asumiendo una velocidad angular medida $\dot{\theta}_m$ (entrada de control $u_k$), es:

$\theta_k = \theta_{k-1} + dt \cdot (\dot{\theta}_{m, k} - b_{g, k-1})$ $b_{g, k} = b_{g, k-1}$

En forma matricial, el modelo de transición de estados $x_k = A x_{k-1} + B u_k + w_k$ se define mediante:

$A = \begin{bmatrix} 1 & -dt \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} dt \\ 0 \end{bmatrix}$

Donde $dt$ es el período de muestreo y $w_k$ representa el ruido de proceso, modelado por la matriz de covarianza de ruido de proceso $Q$:

$Q = \begin{bmatrix} Q_{\theta} & 0 \\ 0 & Q_{bias} \end{bmatrix} \cdot dt$

Aquí, $Q_{\theta}$ representa la incertidumbre del ruido en la estimación del ángulo y $Q_{bias}$ la estabilidad del sesgo del giroscopio.

Modelo de Medición (Corrección)

La única variable física que medimos directamente para corregir el estado es el ángulo calculado mediante el acelerómetro (medición $z_k$). Por lo tanto, el modelo de medición $z_k = H x_k + v_k$ se define con:

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

La covarianza del ruido de medición se reduce a un escalar $R$ (varianza del ruido del acelerómetro):

$R = [\sigma^2_{acc}]$

---

2. Implementación de Algoritmo en C++

Para garantizar la portabilidad y el rendimiento en microcontroladores de arquitectura ARM Cortex-M (STM32) o procesadores de doble núcleo como el ESP32, implementamos el filtro de Kalman en una clase de C++ sin dependencias dinámicas de memoria (evitando el uso de std::vector u operadores de asignación pesados).

Dado que el sistema de matrices es de $2 \times 2$, podemos expandir analíticamente las operaciones matriciales para evitar el overhead computacional de una biblioteca de álgebra lineal genérica.

#ifndef KALMAN_H
#define KALMAN_H

class KalmanFilter2D {
private:
    // Parámetros de ruido (Hiperparámetros de sintonía)
    float Q_angle;   // Ruido de proceso para el ángulo
    float Q_bias;    // Ruido de proceso para el sesgo del giroscopio
    float R_measure; // Ruido de medición del acelerómetro

    // Variables de estado
    float angle; // Ángulo estimado (Estado x_0)
    float bias;  // Sesgo estimado del giroscopio (Estado x_1)

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

public:
    KalmanFilter2D(float q_angle = 0.001f, float q_bias = 0.003f, float r_measure = 0.03f) 
        : Q_angle(q_angle), Q_bias(q_bias), R_measure(r_measure) {
        
        // Inicialización de estados y covarianza
        angle = 0.0f;
        bias = 0.0f;
        
        P[0][0] = 0.1f; P[0][1] = 0.0f;
        P[1][0] = 0.0f; P[1][1] = 0.1f;
    }

    /**
     * @brief Actualiza la estimación del filtro de Kalman
     * @param newAngle Ángulo calculado por el acelerómetro (en grados)
     * @param newRate Velocidad angular medida por el giroscopio (en grados/segundo)
     * @param dt Tiempo transcurrido desde la última muestra (en segundos)
     * @return float Ángulo estimado filtrado
     */
    float update(float newAngle, float newRate, float dt) {
        // -----------------------------------------------------------------
        // PASO 1: Predicción (A priori)
        // -----------------------------------------------------------------
        // x = A * x + B * u
        const float rate = newRate - bias;
        angle += dt * rate;

        // P = A * P * A^T + 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: Corrección (A posteriori)
        // -----------------------------------------------------------------
        // Innovación (Diferencia de medición)
        const float y = newAngle - angle;

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

        // Ganancia de Kalman de 2 elementos: K = P * H^T * S^-1
        const float K[2] = {
            P[0][0] / S,
            P[1][0] / S
        };

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

        // Actualización de la covarianza del error: P = (I - K * H) * P
        const float P00_temp = P[0][0];
        const 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;
    }

    void setAngle(float newAngle) { angle = newAngle; }
    float getRate() const { return -bias; }
};

#endif // KALMAN_H

---

3. Integración en el Lazo de Control de un Microcontrolador

Para aplicar el filtro en un firmware real (por ejemplo, utilizando ESP-IDF o STM32 HAL), debemos estructurar una tarea periódica que garantice un tiempo de muestreo ($dt$) constante.

Ejemplo de Tarea de Adquisición y Filtrado (Pseudo-C++/FreeRTOS)

#include "KalmanFilter2D.h"
#include "IMU_Driver.h" // Supongamos una API de lectura del sensor

KalmanFilter2D pitchKalman(0.001f, 0.003f, 0.03f);

void vIMUTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(10); // Lazo de control a 100Hz
    const float dt = 0.010f; // 10ms

    // Inicialización del sensor IMU
    IMU_Init();

    while (true) {
        // Lectura de datos en bruto del sensor por I2C/SPI
        IMU_Data raw_data = IMU_Read();

        // Calcular ángulo del acelerómetro: pitch = atan2(ax, az) * 180 / M_PI
        float accel_pitch = atan2f(raw_data.ax, sqrtf(raw_data.ay * raw_data.ay + raw_data.az * raw_data.az)) * 57.29577951f;
        float gyro_pitch_rate = raw_data.gy; // En grados por segundo

        // Aplicar el Filtro de Kalman
        float filtered_pitch = pitchKalman.update(accel_pitch, gyro_pitch_rate, dt);

        // Envío de la estimación al controlador PID de los actuadores
        Actuator_UpdatePID(filtered_pitch);

        // Espera de tiempo real precisa para mantener el dt constante
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}

---

4. Sintonía del Filtro (Tuning)

El comportamiento dinámico del filtro depende de la relación entre las matrices $Q$ y $R$:

• Si aumentas $R$ (ruido de medición): El filtro asume que el acelerómetro es extremadamente ruidoso. Dependerá más de la integración del giroscopio. Esto suaviza la señal pero introduce retardo de fase si se mantiene un valor exagerado.
• Si aumentas $Q_{\theta}$ (ruido del proceso): El filtro confía menos en su modelo dinámico interno y se adapta más rápido a las lecturas directas del acelerómetro. La respuesta será más rápida pero menos filtrada ante vibraciones mecánicas.