nonlinear_motor_model.m

clc
close all
clearvars

% load('dados.mat');
load('resposta_degrau.mat');

% N = size(dados,1);

% ni = 5500;
% nf = 9500;

ni = 1;
nf = size(dados,1);

N = nf-ni+1;
n = (0:N-1)';

% Periodo do sinal medido
T = 1690e-6;
fs = 1/T;
t = n*T;

% Degrau de entrada
x = dados(ni:nf,1);

% Resposta ao degrau do motor DC
y = dados(ni:nf,2);

% Parâmetros desejados
% [DC, ganho, tau, atraso]
theta0 = zeros(4,1);

theta0(1) = y(2);
theta0(2) = max(y);

for i=1:N
    if y(i) >= 0.5 * (theta0(2) - theta0(1)) + theta0(1)
        theta0(4) = i/fs;
        break
    end
end

for i=1:N
    if y(i) >= 0.95 * (theta0(2) - theta0(1)) + theta0(1)
        theta0(3) = (i/fs-theta0(4))/3;
        if theta0(3) < 2/fs
            theta0(3) = 1/fs;
        end
        break
    end
end

theta = theta0;

% Modelo da resposta ao impulso do motor DC
f = @(t,theta) theta(1) + theta(2)*(1 - exp(-(t-theta(4))/theta(3))).*heaviside(t-theta(4));
J = @(t,theta) [ones(length(t),1), ...
               (1 - exp(-(t-theta(4))/theta(3))).*heaviside(t-theta(4)), ...
               theta(2) * (theta(4)-t) .* heaviside(t-theta(4)).* ...
               exp( -(t-theta(4))/theta(3) ) / theta(3)^2, ...
               -theta(2) * heaviside(t-theta(4)).* ...
               exp( -(t-theta(4))/theta(3) ) / theta(3)];

max_iters = 100;
error = nan(max_iters,1);
epsilon = 1e-4;
for i=1:max_iters
    
    Jm = J(t,theta);
    [Q,R] = qr(Jm'*Jm,0);
    
    y_hat = f(t,theta);
    
    r = y - y_hat;
    dtheta = R\(Q'*Jm'*r);
    
    theta = theta + dtheta;
    
    error(i) = norm(r,2);
    if error(i) < epsilon
        i
        break
    end
    
    if (error(i) > norm(y,2)) || isnan(error(i))
        theta(1) = theta0(1) + (rand()-1)*theta0(1);
        theta(2) = theta0(2) + (rand()-1)*theta0(2);
        theta(3) = theta0(3) + (rand()-1)*theta0(3);
        theta(4) = theta0(4) + (rand()-1)*theta0(4);
        'error big'
    end
    
    if theta(3) < 0
        theta(3) = theta0(3);
    end
    
    if theta(4) < 0
        theta(4) = theta0(4);
    end
    
end

theta

figure
plot(t,y_hat,t,y,t,x)
xlabel('Tempo [s]')
ylabel('Tensão [V]')

save('parametros_motor.mat','theta')