%%% Programa: exemplo_11_4.m
%%% Autor: Geovany A. Borges
%%% Objectivo: Detecção de falha em motor CC.
%%% Notas:
%%%

clear all;
close all;
echo off;

FlagImprimir = 0;

FontSize = 10;

%%% Tempo:
TMAX = 10; dt = 0.001; t = [0:dt:TMAX]'; N = length(t);

%%% Simulação do modelo contínuo do motor com falha ocorrendo em t = TMAX/2. 
%%% A falha é dada pela redução de jm a um quarto de seu valor nominal.
rho = 3;
la = 21e-3;  % indutância do rotor
ra = 627e-3; % resistência do rotor
jm = 663e-6; % inércia do rotor
b = 3.1e-3;  % coeficiente de atrito
kt = 31e-3; % indutância do rotor
ke = 1/(la*kt);
ic = 1.0;
A = [-b/jm kt*ic/jm; -kt*ic/la -ra/la];
B = [0; 1/la];
va = 10*ones(N,1);
wm = zeros(N,1);
im = zeros(N,1);
dm = zeros(N,1);
X = zeros(2,N);
XEst = zeros(2,N);
PXEst = zeros(2,2,N);
PXEst(:,:,1) = diag([2 2].^2);
F = (eye(2)+dt*A);
G = dt*B;
H = [1 0];
Q = diag([1^2 1^2]/100);
Rw = 0.5^2;
Ri = 0.01^2;
for n=2:N,
   %%% Simular a falha
   if n<=N/2,
      j = jm;
   else
      j = jm / 4;
   end
   A(1,1) = -b/j;
   A(1,2) = kt*ic/j;
   %%% Simular usando Euler de primeira ordem em 20 subintervalos
   Xpast = X(:,n-1);
   ndiv = 20;
   for nn=1:ndiv,
      X(:,n) = (eye(2)+dt/20*A)*Xpast + dt/ndiv*B*va(n-1);
      Xpast = X(:,n);
   end
   %%% Gerar a entrada:
   if rem(n,round(N/10))==2,
      if va(n-1)==10
         va(n) = 20;
      else
         va(n) = 10;
      end
   else,
      va(n) = va(n-1);
   end
%   va(n) = 15 + 5*sin(2*pi*1*t(n));
   
   %%% Sensor de velocidade:
   wm(n) = X(1,n) + sqrt(Rw)*0.8*randn(1); % Usa matrizes de incerteza menores que as usadas pelo filtro
   im(n) = X(2,n) + sqrt(Ri)*0.8*randn(1); % Usa matrizes de incerteza menores que as usadas pelo filtro

   %%% Estimador Estocástico (Filtro de Kalman)
   % Predição:
   XEst(:,n) = F*XEst(:,n-1)+G*va(n-1);
   PXEst(:,:,n) = F*PXEst(:,:,n-1)*F' + Q;
   % Inovação:
   V = wm(n) - XEst(1,n);
   % Correção:
   K = PXEst(:,:,n)*H'*inv(Rw+H*PXEst(:,:,n)*H');
   XEst(:,n) = XEst(:,n) + K * V;
   PXEst(:,:,n) = (eye(2)-K*H)*PXEst(:,:,n);
   % Métrica de Mahalanobis de estimação da corrente (variável não observada):
   dm(n) = (im(n) - XEst(2,n))'*inv(Ri+[0 1]*PXEst(:,:,n)*[0 1]')*(im(n) - XEst(2,n));
end

for dimvar=1:2
      if dimvar==1, str = 'Velocidade real (vermelho) e velocidade estimada (azul)'; ylabelstr = '\omega, \omega_{e} [rad/s]'; end
      if dimvar==2, str = 'Corrente real (vermelho) e corrente estimada (azul)'; ylabelstr = 'i, i_{e} [A]'; end
      figure(dimvar); 
      hold on; set(gca,'FontSize',FontSize);
      plot(t,zeros(size(t)),'k:'); 
      plot(t,X(dimvar,:),'Color','r','LineStyle','-','Marker','.','MarkerSize',3); 
      plot(t,XEst(dimvar,:),'Color','b','LineStyle','-','Marker','.','MarkerSize',3); 
      ylabel(ylabelstr,'FontSize',FontSize);
      title(sprintf('%s ',str),'FontSize',FontSize);
      ax = axis; axis([ax(1:2) -max(abs(ax(3:4))) max(abs(ax(3:4)))]);
end

var = zeros(N,1);
for dimvar=1:2
      if dimvar==1, str = 'Resíduo de velocidade com intervalo de confiança 3-\sigma'; ylabelstr = '\omega - \omega_{e} [rad/s]'; end
      if dimvar==2, str = 'Resíduo de corrente com intervalo de confiança 3-\sigma'; ylabelstr = 'i - i_{e} [A]'; end
      figure(2+dimvar); 
      hold on; set(gca,'FontSize',FontSize);
      for n=1:N, var(n) = PXEst(dimvar,dimvar,n); end
      plot(t,zeros(size(t)),'k:');
      plot(t,X(dimvar,:)-XEst(dimvar,:),'Color','b','LineStyle','-'); 
      plot(t,+3*sqrt(var),'Color','b','LineStyle','-.'); 
      plot(t,-3*sqrt(var),'Color','b','LineStyle','-.'); 
      ylabel(ylabelstr,'FontSize',FontSize);
      title(sprintf('%s ',str),'FontSize',FontSize);
      ax = axis; axis([ax(1:2) -max(abs(ax(3:4))) max(abs(ax(3:4)))]);
end

figure(5);
plot(t(2:N),log10(dm(2:N)),'LineStyle','-','Color','b','MarkerSize',3); hold on;
plot(t(2:N),log10(5.99)*ones(N-1,1),'LineStyle','-','Color','r','MarkerSize',3); 
ylabel('log_{10}(d_{m})','FontSize',FontSize);
xlabel('t [s]','FontSize',FontSize);
title('Distância de Mahalanobis calculada sobre a estimativa de corrente (log)');

figure(6);
plot(t,va,'LineStyle','-','Marker','.','MarkerSize',3); 
ylabel('v_{a} [V]','FontSize',FontSize);
xlabel('t [V]','FontSize',FontSize);
title('Entrada');



if FlagImprimir
   imprimirtudo('figexemplo_11_5',5);
end
