13 de julio de 2026Escrito por Francisco Castel

Implementación de un Filtro de Kalman Lineal en STM32 (ARM Cortex-M4) para Fusión de Sensores IMU vía SPI

STM32Filtro de KalmanSPI-DMAFusión de Sensores

Introducción y Planteamiento del Problema

En la robótica móvil, sistemas aeroespaciales (UAVs) y robótica de manipulación, la estimación precisa de la actitud (ángulos de Roll, Pitch, Yaw) es crítica. Los sensores inerciales (IMUs) de bajo costo presentan limitaciones físicas intrínsecas:

Acelerómetros: Excelente precisión estática (baja frecuencia), pero altamente susceptibles al ruido de vibración mecánica de alta frecuencia (ruido blanco).
Giroscopios: Excelente respuesta dinámica a alta frecuencia, pero sufren de drift (deriva de polarización debido a la integración numérica del sesgo residual, $b_g$ ),

Para resolver esto, se emplea un Filtro de Kalman Lineal (LKF) de un solo eje para fusionar la tasa angular del giroscopio y el ángulo calculado por el acelerómetro. En este artículo técnico, diseñaremos el filtro desde el modelo matemático espacio-temporal, configuraremos un periférico SPI bajo DMA en un STM32F4 (Cortex-M4), e implementaremos el algoritmo optimizado para punto flotante (FPU).

1. Fundamentos Teóricos del Filtro de Kalman

Definamos el estado del sistema como un vector $x_k \in \mathbb{R}^2$ :

$x_k = \begin{bmatrix} \theta_k \\ b_k \end{bmatrix}$

Donde:

$\theta_k$ es el ángulo real en el instante $k$ .
$b_k$ es el sesgo (bias) del giroscopio.

Ecuación de Transición de Estados

La tasa angular medida por el giroscopio es $\omega_m = \dot{\theta} + b + w_g$ , donde $w_g$ es el ruido del giroscopio. El modelo de tiempo discreto con período de muestreo $\Delta t$ es:

$\theta_k = \theta_{k-1} + \Delta t (\omega_k - b_{k-1}) + w_{\theta}$ $b_k = b_{k-1} + w_b$

Reescribiendo en forma matricial ( $x_k = A x_{k-1} + B u_k + w_k$ ):

$\begin{bmatrix} \theta_k \\ b_k \end{bmatrix} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \theta_{k-1} \\ b_{k-1} \end{bmatrix} + \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \omega_k + w_k$

Donde la matriz de covarianza del ruido del proceso $Q$ se define como:

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

Ecuación de Medida

El acelerómetro mide directamente el ángulo aproximado $\theta_{acc}$ mediante trigonometría (ej. $\text{atan2}(a_y, a_z)$ ). La matriz de observación $H$ relaciona la medición $z_k$ con el estado:

$z_k = H x_k + v_k \implies z_k = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} \theta_k \\ b_k \end{bmatrix} + v_k$

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

Algoritmo de Predicción y Corrección

Predicción (Paso del tiempo):

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

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

Corrección (Paso de actualización):

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

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

\hat{x}_{k\mid k} = \hat{x}_{k\mid k-1} + K_k (z_k - H \hat{x}_{k\mid k-1})

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

2. Arquitectura de Hardware y Adquisición SPI con DMA

Para garantizar determinismo en sistemas embebidos de tiempo real, la lectura de la IMU (ej. MPU9250 o LSM6DSOX) debe ocurrir sin bloquear la CPU. El microcontrolador STM32 configurará el periférico SPI con DMA (Direct Memory Access) en modo circular o disparado por interrupción de temporizador.

Configuración de Registros Clave (HAL / Low-Level)

FPU activa: Asegura que el hardware de punto flotante de 32 bits resuelva las ecuaciones matriciales en pocos ciclos de reloj. Se habilita en system_stm32f4xx.c: SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));
SPI en Modo Full-Duplex: Baudrate preescalado para cumplir con los límites de la IMU (habitualmente < 10 MHz para lecturas de registros).
DMA Stream: Enlazado al buffer de recepción de SPI para transferir los 14 bytes típicos de datos crudos (Acc X/Y/Z, Temp, Gyro X/Y/Z) de forma autónoma.

3. Implementación de Código C++ Orientado a Objetos

A continuación, se presenta la clase del Filtro de Kalman optimizada para microcontroladores ARM Cortex-M4 con punto flotante de precisión simple (float).

`KalmanFilter.hpp`

#ifndef KALMAN_FILTER_HPP
#define KALMAN_FILTER_HPP

class KalmanFilter {
public:
    KalmanFilter(float q_angle, float q_bias, float r_measure) 
        : Q_angle(q_angle), Q_bias(q_bias), R_measure(r_measure) {
        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;
    }

    /**
     * @brief Actualiza la estimación del estado utilizando la medición y la tasa de cambio.
     * @param newAngle Medida del ángulo derivada del acelerómetro (z_k)
     * @param newRate Tasa angular medida por el giroscopio (u_k)
     * @param dt Período de muestreo (segundos)
     * @return Ángulo óptimo estimado
     */
    float update(float newAngle, float newRate, float dt) {
        // --- PASO 1: Predicción ---
        float rate = newRate - bias;
        angle += dt * rate;

        // Actualización de la covarianza del error de predicción 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: Correcció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;

        // Innovación (Diferencia de medición)
        float y = newAngle - angle;

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

        // Actualización de la covarianza del error de estimación 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;
    }

private:
    // Covarianzas de ruido del proceso
    float Q_angle;
    float Q_bias;
    // Covarianza del ruido de medida
    float R_measure;

    // Variables de estado
    float angle; // Ángulo estimado
    float bias;  // Sesgo estimado
    float P[2][2]; // Matriz de covarianza de error
};

#endif // KALMAN_FILTER_HPP

`main.cpp` e Integración de Interrupciones de DMA

En un entorno embebido estructurado, la ejecución se dispara típicamente desde el callback de finalización de DMA (HAL_SPI_RxCpltCallback).

#include "main.h"
#include "KalmanFilter.hpp"
#include <cmath>

extern SPI_HandleTypeDef hspi1;

#define IMU_RX_BUFFER_SIZE 14
uint8_t imu_rx_buf[IMU_RX_BUFFER_SIZE];
volatile bool imu_data_ready = false;

// Inicializamos el filtro con varianzas empíricas de ruido
KalmanFilter kalmanX(0.001f, 0.003f, 0.03f);

float angle_est_x = 0.0f;
const float dt = 0.001f; // Frecuencia de muestreo a 1kHz (Timer de 1ms)

void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
    if (hspi->Instance == SPI1) {
        imu_data_ready = true;
    }
}

void Process_Sensor_Data(void) {
    if (!imu_data_ready) return;
    imu_data_ready = false;

    // Decodificación de datos de 16 bits (complemento a 2) del buffer DMA
    int16_t acc_y_raw = (imu_rx_buf[2] << 8) | imu_rx_buf[3];
    int16_t acc_z_raw = (imu_rx_buf[4] << 8) | imu_rx_buf[5];
    int16_t gyro_x_raw = (imu_rx_buf[8] << 8) | imu_rx_buf[9];

    // Conversión a unidades físicas (ej. MPU9250 con escala +/-2g y +/-250 deg/s)
    float ay = (float)acc_y_raw / 16384.0f;
    float az = (float)acc_z_raw / 16384.0f;
    float gyro_x_dps = (float)gyro_x_raw / 131.0f;

    // Calcular ángulo de inclinación basado puramente en acelerómetro
    float angle_acc = atan2f(ay, az) * 57.2957795f; // Rad a Deg

    // Ejecución del paso del filtro de Kalman
    angle_est_x = kalmanX.update(angle_acc, gyro_x_dps, dt);
}

4. Buenas Prácticas de Ingeniería Mecatrónica

Alineación de memoria (Memory Alignment): Asegúrate de mapear los búferes de DMA en regiones de memoria direccionables por el bus de DMA (no todos los microcontroladores permiten DMA desde la memoria RAM acoplada al núcleo (CCM-RAM)).
Tuning del Filtro: El ajuste de $Q_{angle}$ $Q_{an g l e}$ , $Q_{bias}$ $Q_{bia s}$ y $R_{measure}$ $R_{m e a s u r e}$ es empírico.
- Si incrementas $R_{measure}$ , confiarás menos en el acelerómetro y la respuesta será suave pero con mayor lentitud a cambios de estado permanentes.
- Si incrementas $Q_{angle}$ , el filtro reaccionará rápido pero filtrará menos ruido dinámico.
Cálculos trigonométricos optimizados: Se utiliza atan2f (versión flotante con el sufijo f) para indicarle explícitamente al compilador GCC que invoque la FPU de precisión simple en hardware, evitando el casteo pesado a double (64 bits), el cual se emula por software en microcontroladores Cortex-M4F.