clc; clear all; close all;
format short g
global tau1 qpp1 %variables global para almacenar par y aceleración

ti=0; h=0.001; tf = 5; t=ti:h:tf;  %vector tiempo
ci=[0; 0]; %condiciones iniciales
opciones=odeset('RelTol',1e-3,'InitialStep',1e-3,'MaxStep',1e-3);
%solución numérica de la dinámica del péndulo
disp('El empleo de funciones signo puede demorar el proceso de simulación....')
[t,x]=ode45('cap7_pendulo',t,ci,opciones);
 xp=cap7_pendulo(t,x,9);
 [m n]=size(t);
 q1=x(:,1); qp1=x(:,2);
     
Ftau1qp1=zeros(m,1); Fq1=zeros(m,1); sFq1=zeros(m,1); Fqp1qp1=zeros(m,1);
sFqp1=zeros(m,1); Fabs=zeros(m,1); Fsinqp1=zeros(m,1); lambda=25;

for k=2:m %filtros  
     Ftau1qp1(k)=exp(-h*lambda)*Ftau1qp1(k-1)+(1-exp(-h*lambda))*tau1(k-1)*qp1(k-1);
     Fq1(k)=exp(-h*lambda)*Fq1(k-1)+(1-exp(-h*lambda))*q1(k-1);
     sFq1(k)=-lambda*Fq1(k)+lambda*q1(k);
     Fqp1qp1(k)=exp(-h*lambda)*Fqp1qp1(k-1)+(1-exp(-h*lambda))*sFq1(k-1)*sFq1(k-1);
     sFqp1(k)=-lambda*Fqp1qp1(k)+lambda*sFq1(k)*sFq1(k);
     Fabs(k)=exp(-h*lambda)*Fabs(k-1)+(1-exp(-h*lambda))*abs(sFq1(k-1));
     Fsinqp1(k)=exp(-h*lambda)*Fsinqp1(k-1)+(1-exp(-h*lambda))*sin(q1(k-1,1))*sFq1(k-1,1);
end

     fi=[(1/2)*sFqp1, Fqp1qp1, Fabs, Fsinqp1];
    [r,theta] =mincuad(Ftau1qp1,fi);
    theta