function exemplo_11_4
%%% Programa: exemplo_11_4.m
%%% Autor: Geovany A. Borges
%%% Objectivo: Estimação de posição de missil equipado com central inercial e altimetro.
%%% Notas:
%%%

close all;
echo off;

FlagImprimir = 1;

FontSize = 10;
%%% Iniciar parâmetros de simulação:
TargetState = struct('X',zeros(3,1),'Y',zeros(3,1),'Theta',0,'PX',zeros(3),'PY',zeros(3),'QX',zeros(3),'QY',zeros(3),'X0',500,'Y0',0,'Theta0',pi/2,'RectangleVertex',zeros(2,4));
TargetStateEstimate_Kalman = struct('X',zeros(3,1),'Y',zeros(3,1),'Theta',0,'PX',zeros(3),'PY',zeros(3),'QX',zeros(3),'QY',zeros(3),'X0',500,'Y0',0,'Theta0',pi/2);
TargetStateEstimate_Integrador = struct('X',zeros(3,1),'Y',zeros(3,1),'Theta',0,'X0',500,'Y0',0,'Theta0',pi/2);

InertialState  = struct('AccX',0,'AccY',0,'AccXVariance',0.05,'AccYVariance',0.05);
AltimeterState  = struct('Ya',0,'YaVariance',1.0);

%%% Tempo:
TMAX = 90; dt = 0.5; t = [0:dt:TMAX]';

%%% Dados a serem armazenados:
Data = struct( 't',zeros(1,length(t)),...
			      'XEst',zeros(3,length(t)),...
   				'YEst',zeros(3,length(t)),...
			      'PXEst',zeros(3,3,length(t)),...
   				'PYEst',zeros(3,3,length(t)),...
			      'X',zeros(3,length(t)),...
			      'Y',zeros(3,length(t)),...
   				'Theta',zeros(1,length(t)));
            
Data_Kalman = Data;
Data_Integrator = Data;
            
%%% Imprimir ambiente:
[TargetState,hTarget] = target(TargetState,0,dt);
ytext = 2300; xtext = 0; ntext = 1; dytext = 130;
hText_t = text(xtext,ytext,'Tempo: 0 s','FontWeight','bold','FontSize',FontSize);
set(gca,'DataAspectRatio',[1 1 1]);
axis([-200.0 3000 -200.0 2500.0]); hold on; 
xlabel('X [m]','FontSize',FontSize);
ylabel('Y [m]','FontSize',FontSize);
set(gca,'Box','on','FontSize',FontSize);

if FlagImprimir
   imprimirtudo('figexemplo_11_4_Init',1,[1],5,6);
end

for n=1:length(t),
   %%% Atualizar o alvo:
   [TargetState,hTarget] = target(TargetState,t(n),dt,hTarget);
   
   %%% Predizer a posição do alvo:
   if t(n)==0,
      %%% Condição inicial:
      TargetStateEstimate_Kalman.X =      [TargetStateEstimate_Kalman.X0; 0; 0]; 
      TargetStateEstimate_Kalman.PX =     diag([30 30 5].^2); 
      TargetStateEstimate_Kalman.QX =     diag([0.1 0.01 0.1].^2); 
      TargetStateEstimate_Kalman.Y =      [TargetStateEstimate_Kalman.Y0; 0; 2.5]; 
      TargetStateEstimate_Kalman.PY =     diag([30 30 5].^2); 
      TargetStateEstimate_Kalman.QY =     diag([0.1 0.01 0.1].^2);
      TargetStateEstimate_Kalman.Theta =  TargetStateEstimate_Kalman.Theta0; 
      
      TargetStateEstimate_Integrator.X = TargetStateEstimate_Kalman.X; 
      TargetStateEstimate_Integrator.Y = TargetStateEstimate_Kalman.Y; 
      TargetStateEstimate_Integrator.Theta = TargetStateEstimate_Kalman.Theta; 
   else,
      %%% Filtro de Kalman: Predição
      Fe = [1 dt (dt^2)/2; 0 1 dt; 0 0 1]; Fn = eye(3);
      TargetStateEstimate_Kalman.X     = Fe*TargetStateEstimate_Kalman.X; 
      TargetStateEstimate_Kalman.PX    = Fe*TargetStateEstimate_Kalman.PX*Fe' + Fn*TargetStateEstimate_Kalman.QX*Fn'; 
      TargetStateEstimate_Kalman.Y     = Fe*TargetStateEstimate_Kalman.Y; 
      TargetStateEstimate_Kalman.PY    = Fe*TargetStateEstimate_Kalman.PY*Fe' + Fn*TargetStateEstimate_Kalman.QY*Fn'; 
   end
   
   %%% Coletar medições:
   [InertialState] = Inertial(InertialState,TargetState,t(n));
   [AltimeterState] = Altimeter(AltimeterState,TargetState,t(n));
   
   %%% Corrigir a posição do alvo em função das medições:
   if t(n)>0,
      %%% Filtro de Kalman: Correção
      %%% Equação de medição: AccX = [0 0 1]X + Wx
      %%%                     AccY = [0 0 1]Y + Wy
      X3m = InertialState.AccX; 
      Y3m = InertialState.AccY;
      Y1m = AltimeterState.Ya;
      % Correção independente de X:
      V = X3m - TargetStateEstimate_Kalman.X(3);
      H = [0 0 1]; R = InertialState.AccXVariance;
      K = TargetStateEstimate_Kalman.PX*(H')*inv(H*TargetStateEstimate_Kalman.PX*(H')+R);
      TargetStateEstimate_Kalman.X  = TargetStateEstimate_Kalman.X + K*V(1);         
      TargetStateEstimate_Kalman.PX = (eye(3)-K*H)*TargetStateEstimate_Kalman.PX;         
      % Correção independente de Y: Central inercial
      V = Y3m - TargetStateEstimate_Kalman.Y(3);
      H = [0 0 1]; R = InertialState.AccYVariance;
      K = TargetStateEstimate_Kalman.PY*(H')*inv(H*TargetStateEstimate_Kalman.PY*(H')+R);
      TargetStateEstimate_Kalman.Y  = TargetStateEstimate_Kalman.Y + K*V(1);         
      TargetStateEstimate_Kalman.PY = (eye(3)-K*H)*TargetStateEstimate_Kalman.PY;
      % Correção independente de Y: Altímetro
      V = Y1m - TargetStateEstimate_Kalman.Y(1);
      H = [1 0 0]; R = AltimeterState.YaVariance;
      K = TargetStateEstimate_Kalman.PY*(H')*inv(H*TargetStateEstimate_Kalman.PY*(H')+R);
      TargetStateEstimate_Kalman.Y  = TargetStateEstimate_Kalman.Y + K*V(1);         
      TargetStateEstimate_Kalman.PY = (eye(3)-K*H)*TargetStateEstimate_Kalman.PY;                     
      
      %%% Integrador: 
      TargetStateEstimate_Integrator.X     = Fe*[TargetStateEstimate_Integrator.X(1:2); InertialState.AccX];
      TargetStateEstimate_Integrator.Y     = Fe*[TargetStateEstimate_Integrator.Y(1:2); InertialState.AccY];
   end
   
   %%% Atualizar informações:
   set(hText_t,'String',sprintf('Tempo: %2.1f s',t(n)));
   
   Data_Kalman.t(:,n) 	    = t(n);
   Data_Kalman.XEst(:,n) 	 = TargetStateEstimate_Kalman.X;
   Data_Kalman.YEst(:,n) 	 = TargetStateEstimate_Kalman.Y;
   Data_Kalman.PXEst(:,:,n) = TargetStateEstimate_Kalman.PX;
   Data_Kalman.PYEst(:,:,n) = TargetStateEstimate_Kalman.PY;
   Data_Kalman.X(:,n) 	    = TargetState.X;
   Data_Kalman.Y(:,n) 	    = TargetState.Y;
   Data_Kalman.Theta(:,n)   = TargetState.Theta;
   
   Data_Integrator.t(:,n) 	    = t(n);
   Data_Integrator.XEst(:,n) 	 = TargetStateEstimate_Integrator.X;
   Data_Integrator.YEst(:,n) 	 = TargetStateEstimate_Integrator.Y;
   Data_Integrator.X(:,n) 	    = TargetState.X;
   Data_Integrator.Y(:,n) 	    = TargetState.Y;
   
   if t(n)==0,
      disp('Esperando ordem para o lançamento do míssel! Pressione alguma tecla para continuar.');
      pause;
   end
   
   drawnow
end,

%%% Apresentar resultados
sigmacolor = [1 1 1]*0.90;
for nm=1:2,
   if nm==1, method = 'filtro de Kalman'; Data = Data_Kalman; end;
   if nm==2, method = 'Integrador'; Data = Data_Integrator; end;
   
   for dimvar=1:3
      if dimvar==1, str = 'Posição'; ylabelstr = 'residuo [m]'; end
      if dimvar==2, str = 'Velocidade'; ylabelstr = 'residuo [m/s]'; end
      if dimvar==3, str = 'Aceleração'; ylabelstr = 'residuo [m/s^{2}]'; end
      figure((nm-1)*3+dimvar+1); 
      subplot(211), hold on; set(gca,'FontSize',FontSize);
      sigma = zeros(1,length(t)); for i=1:length(t), sigma(1,i)=sqrt(Data.PXEst(dimvar,dimvar,i)); end
      plot(Data.t,zeros(size(t)),'k:'); 
      plot(Data.t,Data.X(dimvar,:)-Data.XEst(dimvar,:),'LineStyle','-','Marker','.','MarkerSize',5); 
      title(sprintf('Residuos sobre X: %s (%s)',str,method),'FontSize',FontSize);
      ylabel(ylabelstr,'FontSize',FontSize);
      ax = axis; axis([ax(1:2) -max(abs(ax(3:4))) max(abs(ax(3:4)))]);
      subplot(212), hold on; set(gca,'FontSize',FontSize);
      plot(Data.t,zeros(size(t)),'k:'); 
      plot(Data.t,Data.Y(dimvar,:)-Data.YEst(dimvar,:),'LineStyle','-','Marker','.','MarkerSize',5); 
      title(sprintf('Residuos sobre Y: %s (%s)',str,method),'FontSize',FontSize);
      xlabel('t [s]','FontSize',FontSize); ylabel(ylabelstr,'FontSize',FontSize);
      ax = axis; axis([ax(1:2) -max(abs(ax(3:4))) max(abs(ax(3:4)))]);
   end
end

for dimvar=1:3
   for nvar=1:2
      nm=1; figure((nm-1)*3+dimvar+1);subplot(210+nvar); axm1 = axis;
      nm=2; figure((nm-1)*3+dimvar+1);subplot(210+nvar); axm2 = axis;
      ax = [axm1(1:2) min([axm1(3),axm2(3)]) max([axm1(4),axm2(4)])];
      nm=1; figure((nm-1)*3+dimvar+1);subplot(210+nvar); axis(ax); 
      nm=2; figure((nm-1)*3+dimvar+1);subplot(210+nvar); axis(ax);
   end
end

if FlagImprimir
   imprimirtudo('figexemplo_11_4',7,[1 2 2 2 2 2 2],5,6);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Função Inertial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [InertialState] = Inertial(InertialState,TargetState,t)

InertialState.AccX = TargetState.X(3) + sqrt(InertialState.AccXVariance)*randn(1);
InertialState.AccY = TargetState.Y(3) + sqrt(InertialState.AccYVariance)*randn(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Função Altimeter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [AltimeterState] = Altimeter(AltimeterState,TargetState,t)
AltimeterState.Ya = TargetState.Y(1) + sqrt(AltimeterState.YaVariance)*randn(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Função Target
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [TargetState,hTarget] = target(TargetState,t,dt,hTarget)

% Determinar as variaveis de estado do alvo para cada t
tb = 80;
if t == 0,
	TargetState.X =     [TargetState.X0; 0; 0]; 
	TargetState.Y =     [TargetState.Y0; 0; 0]; 
	TargetState.Theta = TargetState.Theta0; 
else,
   TargetState.X(3) = -1.0*(1-t)/(tb);
  	TargetState.Y(3) = 0.5;
	TargetState.X     = [1 dt (dt^2)/2; 0 1 dt; 0 0 1]*TargetState.X; 
	TargetState.Y     = [1 dt (dt^2)/2; 0 1 dt; 0 0 1]*TargetState.Y; 
   TargetState.Theta = atan2(TargetState.Y(2),TargetState.X(2)); 
end,
x     = TargetState.X(1);
y     = TargetState.Y(1);
theta = TargetState.Theta;

% Determinar a configuração do alvo para cada t
R = zeros(2,2); % Matrix de rotação do eixo (Xa,Ya)
R(1,:) = [ cos(theta-pi/2), sin(theta-pi/2)];
R(2,:) = [-sin(theta-pi/2), cos(theta-pi/2)];

% Coordenadas dos vertices dos objetos geométricos que formam o alvo:
Hc = 15;
Lc = 3;
Hb = 5;
Lb1 = 2;
Lb2 = Lc;
xbase1 = [-Lb2/2; -Lb2/2; -Lb2/2-Lb1;  -Lb2/2];
ybase1 = [0;  Hb; 0; 0] - Hc/2;
xbase2 = [ Lb2/2;  Lb2/2;  Lb2/2+Lb1;   Lb2/2];
ybase2 = [0;  Hb; 0; 0] - Hc/2;
xcorpo = [Lc/2; -Lc/2; -Lc/2;  Lc/2; Lc/2];
ycorpo = [Hc;  Hc; 0; 0; Hc] - Hc/2;
xbico  = [ Lb2/2;  0; -Lb2/2; Lb2/2];
ybico  = [ Hc;  Hc+Hb; Hc; Hc] - Hc/2;

S = 15; % Fator de escala.

% Determinar as coordenadas de preenchimento:
Nf = 17;
FillCoord = zeros(2,Nf);
for n=1:4,
   FillCoord(:,n) = [x;y] + S*R'*[xbase1(n);ybase1(n)];
end,
for n=5:8,
   FillCoord(:,n) = [x;y] + S*R'*[xbase2(n-4);ybase2(n-4)];
end,
for n=9:13,
   FillCoord(:,n) = [x;y] + S*R'*[xcorpo(n-8);ycorpo(n-8)];
end,
for n=14:17,
   FillCoord(:,n) = [x;y] + S*R'*[xbico(n-13);ybico(n-13)];
end,

%%% Desenhar
hold on;
if nargin<4,
   hTarget = zeros(4,1);
   
   hTarget(1) = fill([FillCoord(1,1:4)],[FillCoord(2,1:4)],[1 1 1]*0);
   hTarget(2) = fill([FillCoord(1,5:8)],[FillCoord(2,5:8)],[1 1 1]*0);
   hTarget(3) = fill([FillCoord(1,9:13)],[FillCoord(2,9:13)],[1 1 1]*0.6);
   hTarget(4) = fill([FillCoord(1,14:17)],[FillCoord(2,14:17)],[1 1 1]*0.3);
   for i=1:4,
      i1 = i; i2 = i + 1; if i2==5, i2 = 1; end
      hTarget(4+i) = plot([TargetState.RectangleVertex(1,i1);TargetState.RectangleVertex(1,i2)],[TargetState.RectangleVertex(2,i1);TargetState.RectangleVertex(2,i2)],'g');
   end
   for n=1:4+i,
      set(hTarget(n),'EraseMode','xor');
	end      
else
   set(hTarget(1),'xdata',[FillCoord(1,1:4)],'ydata',[FillCoord(2,1:4)]);
   set(hTarget(2),'xdata',[FillCoord(1,5:8)],'ydata',[FillCoord(2,5:8)]);
   set(hTarget(3),'xdata',[FillCoord(1,9:13)],'ydata',[FillCoord(2,9:13)]);
   set(hTarget(4),'xdata',[FillCoord(1,14:17)],'ydata',[FillCoord(2,14:17)]);
   for i=1:4,
      i1 = i; i2 = i + 1; if i2==5, i2 = 1; end
      set(hTarget(4+i),'xdata',[TargetState.RectangleVertex(1,i1);TargetState.RectangleVertex(1,i2)],'ydata',[TargetState.RectangleVertex(2,i1);TargetState.RectangleVertex(2,i2)]);
   end
end,

