Práctica 6: Sensores Inteligentes

Motor de DC - Filtro de Kalman

• Determinación del Punto de Operación: Calcular analíticamente el punto de equilibrio del sistema no lineal ante una entrada nominal, resolviendo las ecuaciones algebraicas resultantes.
• Linealización del Modelo: Obtener una aproximación lineal del sistema mediante el cálculo de los Jacobianos alrededor del punto de operación, validando su rango de operación efectivo mediante simulaciones comparativas.
• Extensión al Espacio de Estados: Formular el modelo aumentado considerando la cinemática y la dinámica del motor para incluir la posición y aceleración como variables de interés.
• Diseño del Estimador Estocástico: Sintonizar e implementar un Filtro de Kalman discreto, definiendo las matrices de covarianza de proceso Q y medición R para minimizar el error cuadrático medio de la estimación.
• Validación por Simulación: Evaluar el desempeño del estimador comparando sus resultados con la derivación numérica y el modelo no lineal ante secuencias de entrada variables y presencia de ruido gaussiano.

Planteamiento del problema: Esta práctica se basa en el análisis y extensión de los resultados obtenidos en la investigación "Data-driven modeling of DC motor dynamics: a benchmark experiment" de Maldonado-Uriostigue et al. Dicho trabajo se centra en un banco experimental de instrumentación y control para un motor de corriente directa Mod. TY36A/EV, el cual opera a y posee una velocidad máxima de . Este banco experimental, se muestra en la Figura 1.

Para el modelado, se utilizaron datos de entrada-salida adquiridos en una región acotada de operación, donde la entrada es el Voltaje de Armadura () y la salida es la Velocidad Angular ). A continuación se presentan las gráficas de los datos experimentales utilizados:

% Cargar datos

filename = 'DATA_MOTOR_N4SID2_4000.csv';

data = readtable(filename);

Warning: Column headers from the file were modified to make them valid MATLAB identifiers before creating variable names for the table. The original column headers are saved in the VariableDescriptions property.
Set 'VariableNamingRule' to 'preserve' to use the original column headers as table variable names.

 

% Parámetros

dt = 0.001; % tiempo de muestreo

 

% Cargar señales

Ts = data.Ts;

Varm = data.Varm;

AngularSpeed = data.("AngularSpeed");

 

% Conversión de los datos

Varmc = 22.18; Wc = 3.27; Wmax = 4000; Varmax = 24;

Varm = (Varmc / 870) * Varm;

mVarm = (Varmc / 870);

RPMc = Varmc * (Wmax / Varmax);

mAngularSpeed = 0.0049 * (RPMc / Wc);

AngularSpeed = 0.0049 * AngularSpeed * (RPMc / Wc);

AngularSpeed = AngularSpeed * (2 * pi / 60); % rad/s

 

% Graficar entradas suavizadas

% Gráfica 1: Voltaje de Armadura

fig1 = figure('Color', 'w');

plot(Varm, 'k-', 'LineWidth', 2); hold on;

xlabel('Muestras', 'Color', 'k');

ylabel('V_{arm} [V]', 'Color', 'k');

set(gca, 'XColor', 'k', 'YColor', 'k', 'GridColor', 'k', 'GridAlpha', 0.2, 'Color', 'w');

lgd1 = legend('Mediciones');

set(lgd1, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on;

 

 

% Gráfica 2: Velocidad Angular

fig2 = figure('Color', 'w');

plot(AngularSpeed, 'k-', 'LineWidth', 2); hold on;

xlabel('Muestras', 'Color', 'k');

ylabel('Velocidad Angular [rad/s]', 'Color', 'k');

set(gca, 'XColor', 'k', 'YColor', 'k', 'GridColor', 'k', 'GridAlpha', 0.2, 'Color', 'w');

lgd2 = legend('Mediciones');

set(lgd2, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on;

Paso 1. Punto de Equilibrio/Operación.

Para un sistema descrito por la ecuación diferencial , un punto de es de equilibrio si para todo tiempo [2]. Lo anterior cuando la entrada es cero, sin embargo en algunos textos, cuando existe una entrada de equilibrio, al punto de equilibrio para el cual para todo tiempo , es llamado punto de operación.

Por lo que obteniendo el punto de operación de la ecuación (1) para una entrada de . Comenzamos con cambiar las variables y , donde reemplazamos en la ecuación (1) por los valores de equilibrio:

Para finalmente resolver la ecuación (3) para . Para esto, nos apoyaremos de MATLAB para resolver la ecuación cuadrática, la siguiente sección de código muestra el planteamiento completo para obtener el punto de equilibrio, este fragmento de código lo deberás trasncribir en tu archivo .mlx para responder las preguntas de la sección.

• ¿Cuál es el punto de operación del sistema?
• ¿Qué valor de utilizar?. Justifique su respuesta.
• ¿Cuál es el punto de equilibrio para una entrada de equilibrio ?

Paso 2. Linealización.

Para linealizar alrededor del punto de equilibrio , definimos las variables de desviación (pequeñas perturbaciones):

• Cerca del punto de equilibrio: La curva y la recta casi se tocan, por lo que el error es despreciable.
• Lejos del punto de equilibrio: La curva se aleja de la recta tangente. El modelo lineal predice una velocidad basándose en una pendiente constante, mientras que el sistema real cambia su dinámica.

%% 6. Configuración de la Simulación

dt = 0.001; % Tiempo de muestreo [s]

t_step = 1.5; % Duración de cada voltaje [s]

voltajes = [10, 15, 20, 17, 23, 21, 13, 24, 19]; % Secuencia de entrada [V]

 

total_steps = length(voltajes);

t_total = total_steps * t_step;

time = 0:dt:t_total-dt;

 

% Vectores de almacenamiento

w_nl = zeros(size(time)); % Velocidad No Lineal

w_lin = zeros(size(time)); % Velocidad Lineal

V_in = zeros(size(time)); % Entrada de voltaje

 

% Condiciones iniciales = Punto de equilibrio

w_nl(1) = w_eq;

w_lin(1) = w_eq;

 

%% 7. Bucle de Simulación

idx_v = 1;

for i = 1:length(time)-1

% Determinar qué voltaje toca según el tiempo

current_step_index = floor(time(i) / t_step) + 1;

if current_step_index > length(voltajes)

current_step_index = length(voltajes);

end

V_act = voltajes(current_step_index);

V_in(i) = V_act;

% --- DINÁMICA NO LINEAL (SINDy)---

% dw/dt = f(w, V)

dw_nl = c1*V_act + c2*w_nl(i) + c3*V_act^2 + ...

c4*V_act*w_nl(i) + c5*w_nl(i)^2;

w_nl(i+1) = w_nl(i) + dw_nl * dt;

% --- DINÁMICA LINEAL (Aproximación realizada) ---

% dw_lin = A * (delta_w) + B * (delta_V)

% delta_w = w_lin - w_eq

% delta_V = V_act - V_eq

dw_l = A * (w_lin(i) - w_eq) + B * (V_act - Varm);

w_lin(i+1) = w_lin(i) + dw_l * dt;

end

V_in(end) = V_in(end-1); % Ajuste final para gráfica

error = abs(w_nl - w_lin);

%% 8. Gráficas y Análisis

fig = figure('Color', 'w', 'Position', [300, 300, 2600, 2200]);

% Subplot 1: Voltaje de Entrada

subplot(3,1,1);

plot(time, V_in, 'k-', 'LineWidth', 2); hold on;

yline(Varm, 'g--', 'LineWidth', 1.5);

title('Entrada', 'Color', 'k');

xlabel('Tiempo [s]', 'Color', 'k');

ylabel('V_{arm} [V]', 'Color', 'k');

set(gca, 'XColor', 'k', 'YColor', 'k', 'GridColor', 'k', 'GridAlpha', 0.2, 'Color', 'w');

lgd1 = legend('V_{entrada}', 'Entrada de Equilibrio (20V)');

set(lgd1, 'FontSize', 5, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on;

% Subplot 2: Comparación de Velocidades

subplot(3,1,2);

plot(time, w_nl, 'b-', 'LineWidth', 2); hold on;

plot(time, w_lin, 'r--', 'LineWidth', 2);

 

title('Modelo No Lineal vs Modelo Linealizado', 'Color', 'k');

xlabel('Tiempo [s]', 'Color', 'k');

ylabel('Velocidad \omega [rad/s]', 'Color', 'k');

set(gca, 'XColor', 'k', 'YColor', 'k', 'GridColor', 'k', 'GridAlpha', 0.2, 'Color', 'w');

lgd2 = legend('Modelo No Lineal', 'Modelo Lineal');

set(lgd2, 'FontSize', 5, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on;

 

% Subplot 3: Error

subplot(3,1,3);

area(time, error, 'FaceColor', [1 0.8 0.8], 'EdgeColor', 'r');

title('Error Absoluto (|Real - Aprox|)', 'Color', 'k');

xlabel('Tiempo [s]', 'Color', 'k');

ylabel('Error [rad/s]', 'Color', 'k');

set(gca, 'XColor', 'k', 'YColor', 'k', 'GridColor', 'k', 'GridAlpha', 0.2, 'Color', 'w');

grid on;

 

• ¿Cuál es el modelo linealizado en espacio de estados obtenido?. Nota: indica el modelo completo, incluyendo las matrices C y D.
• Basado en el gráfico de error, ¿cuál es el rango máximo de voltaje alrededor de 20V donde el error se mantiene por debajo del 5%?
• ¿Cambia la constante de tiempo (relacionado a la señal azul) cuando estamos en 10V comparado con 24V?
• ¿Cuál es el el modelo linealizado para una entrada de equilibrio ?. Utiliza el punto de equilibrio obtenido en las preguntas de la primera sección.

Paso 3. Filtro de Kalman.

3.1. Expansión del modelo en espacio de estados.

El modelo en espacio de estados obtenido de la linealización, describe solo la velocidad. Sin embargo, para controlar o monitorear diferentes sistemas donde los motores de DC son el núcleo como robots [4], a menudo se requiere saber donde está el efector final (posición ) y que tán rápido cambia su velocidad (aceleración ). Para realizar lo anterior, en el espacio de estados, definimos un vector de estado que contiene todas las variables que necesitamos estimar. Pasamos de un estado escalar a un vector aumentado:

• Cinemática: La derivada de la posición es la velocidad,
• Dinámica: La derivada de la velocidad sigue la ecuación del modelo linealizado en espacio de estados, . Nota: Responder pregunta de la sección anterior para verlo de manera más clara

3.2. Filtro de Kalman vs Enfoque Clásico (Derivadas e Integrales)

• Combina la predicción (con el modelo linealizado) con la medición realizada por un sensor en la planta.
• Para la aceleración, en lugar de derivar la medición del sensor con ruido, usa el modelo de la planta alimentado con la velocidad limpia estimada.
• Para la posición, integra la velocidad estimada, que tiene mucho menos ruido que la medida directa, esto lo hace con el conocimiento de la dinámica de la planta.
• Adicional, es recursivo y se puede implementar "en línea". Muchos algoritmos de análisis de datos procesan todo el conjunto de datos al final (Batch Processing). Una implementación en línea significa que el algoritmo: Recibe un solo dato en el tiempo t,calcula la estimación actual, olvida el dato pasado y espera el dato .Esto es crítico para sistemas embebidos (microcontroladores como Arduino/ESP32) porque requiere muy poca memoria RAM y permite controlar el motor en tiempo real

3.3. Algoritmo del Filtro de Kalman

Paso 1: Predicción

• Estado:
• Incertidumbre (): , aquí la incertidumbre crece (+Q) porque al proyectar a futuro perdemos certeza.

Donde F es la matriz A discretizada, G es la matriz B discretizada, es la matriz de covarianza del error, Q es la matriz de covarianza del ruido del proceso, son los estados estimados, son las mediciones de la entrada.

Paso 2: Error

Comparamos la predicción con la lectura real del sensor :

• 
• Si es pequeño, el modelo va bien. Si es grande, algo cambió o hay mucho ruido.

Donde es el error o innovación y H es la matriz de observación que relaciona la lectura disponible del sensor.

Paso 3: Ganancia de Kalman

• 
• Si el sensor es muy ruidoso (R alto) → K es pequeño (confíar más en el modelo)
• Si el modelo poco preciso (Q alto) → K es grande (confíar más en la medición del sensor)

Paso 4: Corrección

• Estado:
• Incertidumbre:

 

%% 8. Filtro de Kalman

% Parámetros de simulación

c1 = -6.754; c2 = -1.721; c3 = 5.204; c4 = -0.060; c5 = -0.008; % Coeficientes modelo no lineal

V_eq = 20; w_eq = 343.41; % Punto de equilibrio

A_lin = -8.416; B_lin = 180.801; % Modelo linealizado

dt = 0.001; % Tiempo de muestreo [s]

t_step = 2; % Duracion de los escalones [s]

secuencia_voltajes = [20, 22, 19, 21, 18, 23, 20];

T_total = length(secuencia_voltajes) * t_step;

t = 0:dt:T_total-dt;

 

% Generacion de la señal de entrada

V_in = zeros(size(t));

for i = 1:length(t)

idx = floor(t(i)/t_step) + 1;

if idx > length(secuencia_voltajes), idx = length(secuencia_voltajes); end

V_in(i) = secuencia_voltajes(idx);

end

 

% Extendiendo el orden del modelo

% Estados: x = [theta; omega]

A_sys = [0, 1; 0, A_lin];

B_sys = [0; B_lin];

C_sys = [0, 1];

F = eye(2) + A_sys * dt; G = B_sys * dt; H = C_sys;

 

% ----- SECCIÓN A MODIFICAR -----

Q = diag([1e-4, 1]); % Q: Confianza en integración de theta y dinámica de w

R = 10; % R: Ruido del sensor de velocidad (ajustar si se ve muy sucio)

% ----- ----- ----- ----- -----

P = eye(2);

 

x_est = [0; w_eq];

affine_term = -A_lin*w_eq - B_lin*V_eq; % Con este término,

% el filtro sabe que el sistema está "centrado" alrededor de 20V y 343 rad/s.

 

% Almacenamiento

log_theta_real = zeros(size(t)); log_w_real = zeros(size(t));

log_alpha_real = zeros(size(t)); log_theta_est = zeros(size(t));

log_alpha_est = zeros(size(t)); log_y_meas = zeros(size(t));

 

theta_real = 0; w_real = w_eq;

 

% Bucle Principal

for k = 1:length(t)

u = V_in(k);

% --- Dinámica No lineal ---

d_omega_real = c1*u + c2*w_real + c3*u^2 + c4*u*w_real + c5*w_real^2;

theta_real = theta_real + w_real * dt;

w_real = w_real + d_omega_real * dt;

log_theta_real(k) = theta_real;

log_w_real(k) = w_real;

log_alpha_real(k) = d_omega_real;

% --- Sensor de VELOCIDAD ---

% *** LINEA DE RUIDO ***

ruido = 0;%% Cambiar a: 1.5 * randn(); para ver efecto ruidoso

y_meas = w_real + ruido; % Medimos w

log_y_meas(k) = y_meas;

% --- Kalman Filter ---

% Predicción

x_pred = F*x_est + G*u + [0; affine_term*dt];

P_pred = F * P * F' + Q;

% Corrección

y_tilde = y_meas - H * x_pred;

S = H * P_pred * H' + R;

K = P_pred * H' / S;

x_est = x_pred + K * y_tilde;

P = (eye(2) - K * H) * P_pred;

 

% Corrección de simetría numérica

P = (P + P')/2;

log_theta_est(k) = x_est(1);

log_alpha_est(k) = A_lin * x_est(2) + B_lin * u + affine_term;

end

 

% Cálculo de la Derivada Numérica de la Velocidad Medida (Aceleración Aprox)

accel_num = [0, diff(log_y_meas)/dt];

 

% GRÁFICAS

col_azul = [0 0.4470 0.7410];

col_rojo = [0.8500 0.3250 0.0980];

col_verde_claro = [0.5 0.8 0.5];

 

% --- FIGURA 1: Entrada y Salida ---

fig1 = figure('Color', 'w', 'Name', 'Entrada_Salida', 'Position', [50, 500, 700, 600]);

% Subplot 1

subplot(2,1,1);

stairs(t, V_in, 'k-', 'LineWidth', 2);

title('Entrada: Voltaje de Armadura', 'Color', 'k', 'FontWeight', 'bold');

ylabel('V_{arm} [V]', 'Color', 'k');

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

% Subplot 2

subplot(2,1,2);

% 1. Velocidad Medida (Ruidosa)

plot(t, log_y_meas, 'Color', col_rojo, 'LineWidth', 2); hold on;

% 2. Velocidad Real

plot(t, log_w_real, 'Color', col_azul, 'LineWidth', 2, 'LineStyle', '--');

title('Salida: Velocidad Angular \omega', 'Color', 'k', 'FontWeight', 'bold');

ylabel('\omega [rad/s]', 'Color', 'k'); xlabel('Tiempo [s]', 'Color', 'k');

lgd1 = legend('Medición (Sensor+Ruido)', 'Real (Modelo NL)');

set(lgd1, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

ylim([min(log_w_real)-20, max(log_w_real)+20]);

 

 

% --- FIGURA 2: Posición Estimada ---

fig2 = figure('Color', 'w', 'Name', 'Posicion', 'Position', [800, 500, 700, 800]);

% Subplot 1

subplot(3,1,1);

plot(t, log_theta_real, 'Color', col_azul, 'LineWidth', 2); hold on;

plot(t, log_theta_est, '--', 'Color', col_rojo, 'LineWidth', 2);

title('Posición Angular Acumulada \theta (Estimada vs Real)', 'Color', 'k', 'FontWeight', 'bold');

ylabel('\theta [rad]', 'Color', 'k');

lgd2 = legend('Real', 'Estimada (KF)');

set(lgd2, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k', 'Location', 'northwest');

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

% Subplot 2

subplot(3,1,2);

plot(t, sin(log_theta_real), 'Color', col_azul, 'LineWidth', 1.5); hold on;

plot(t, sin(log_theta_est), '--', 'Color', col_rojo, 'LineWidth', 1.5);

title('Rotación Física (sin(\theta))', 'Color', 'k', 'FontWeight', 'bold');

ylabel('sin(\theta)', 'Color', 'k');

ylim([-1.5, 1.5]);

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

% Subplot 3: ZOOM (3.8s a 4.2s)

subplot(3,1,3);

plot(t, sin(log_theta_real), 'Color', col_azul, 'LineWidth', 2); hold on;

plot(t, sin(log_theta_est), '--', 'Color', col_rojo, 'LineWidth', 2);

xlim([3.8, 4.2]);

title('Zoom (t = 3.8s a 4.2s)', 'Color', 'k', 'FontWeight', 'bold');

xlabel('Tiempo [s]', 'Color', 'k'); ylabel('sin(\theta)', 'Color', 'k');

ylim([-1.5, 1.5]);

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

 

 

% --- FIGURA 3: Aceleración (Derivada del Sensor vs Estimada) ---

fig3 = figure('Color', 'w', 'Name', 'Aceleracion', 'Position', [50, 50, 700, 600]);

% Subplot 1:

subplot(2,1,1);

plot(t, log_alpha_real, 'Color', col_azul, 'LineWidth', 2); hold on;

plot(t, accel_num, ':', 'Color', [0 0.6 0], 'LineWidth', 1);

plot(t, log_alpha_est, '--', 'Color', col_rojo, 'LineWidth', 2);

title('Aceleración angular', 'Color', 'k', 'FontWeight', 'bold');

ylabel('\alpha [rad/s^2]', 'Color', 'k');

% Zoom

if max(abs(accel_num)) > 3*max(abs(log_alpha_real))

ylim([min(log_alpha_real)-300, max(log_alpha_real)+300]);

end

lgd3 = legend('Real', 'Derivada Numérica (d\omega_{med}/dt)', 'Estimada (KF)');

set(lgd3, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

% Subplot 2

subplot(2,1,2);

plot(t, log_alpha_real, 'Color', col_azul, 'LineWidth', 2); hold on;

plot(t, log_alpha_est, '--', 'Color', col_rojo, 'LineWidth', 2);

title('Sin Derivada Numérica', 'Color', 'k', 'FontWeight', 'bold');

ylabel('\alpha [rad/s^2]', 'Color', 'k'); xlabel('Tiempo [s]', 'Color', 'k');

lgd3b = legend('Real', 'Estimada (KF)');

set(lgd3b, 'TextColor', 'k', 'Color', 'w', 'EdgeColor', 'k');

grid on; set(gca, 'XColor', 'k', 'YColor', 'k', 'GridAlpha', 0.3, 'Color', 'w');

 

 

Actividad

1. Modificar la linea 243, del código referente al Filtro de Kalman. Cambiar el ruido de 0 a 1.5 * randn() para ver los cambios en las gráficas. Indicar sus observaciones.
2. Mantener el ruido y cambiar el valor de Q de diag([1e-4, 1]) a diag([1e-4, 0.5]) e incrementar el valor de R a 20 y posteriormente a 50. Indicar sus observaciones.
3. ¿Qué efectos hay en el incremento de y modificación de las matrices Q y R?.
4. Investigue aplicaciones del Filtro del Kalman.
5. Investigue el algoritmo del Filtro de Kalman como estimador de parámetros.

Referencias