clc; clear all; close all; format short g
global tau1 tau2
ti=0; h=0.001; tf = 5; t=ti:h:tf;  %tiempo de simulación
ci=[0; 0; 0; 0]; %condiciones iniciales
opciones=odeset('RelTol',1e-3,'InitialStep', 1e-3, 'MaxStep', 1e-3);
%solución numérica del robot antropomórfico de 2 gdl
disp('El proceso de simulación puede demorar por utilizar funciones discontinuas tipo signo.......')
[t,x]=ode45('cap7_iderobot2gdl',t,ci,opciones);
%vectores de posición $\bfq=\Pmatrix{q_1, q_2}^T$ y velocidad $\qp=\Pmatrix{\dot{q}_1, \dot{q}_2}^T  
q1=x(:,1); q2=x(:,2); qp1=x(:,3); qp2=x(:,4);
    
[m n]=size(t); qpp1=zeros(m,1); qpp2=zeros(m,1); u1=zeros(m,1); u2=zeros(m,1);
    
    for  k=1:m
     xp=cap7_iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
     qpp1(k,1)=xp(3,1);
     qpp2(k,1)=xp(4,1);
     u1(k,1) =tau1;    u2(k,1) =tau2;
    end
   
 tau=[u1; u2];
 fi11=qpp1; 
 fi12=2*cos(q2).*qpp1+cos(q2).*qpp2-2*sin(q2).*qp2.*qp1-sin(q2).*qp2.*qp2;
 fi13=qpp2; 
 fi14=sin(q1); 
 fi15=sin(q1+q2); 
 fi16=qp1; 
 fi17=zeros(m,1);
 fi18=sign(qp1);  
 fi19=zeros(m,1); 
 fi110=(1-abs(sign(qp1)));
 fi111=zeros(m,1);
 fi21=zeros(m,1);  
 fi22=cos(q2).*qpp1+sin(q2).*qp1.*qp1;
 fi23=qpp1+qpp2; 
 fi24=zeros(m,1);  
 fi25=sin(q1+q2); 
 fi26=zeros(m,1);  
 fi27=qp2; fi28=zeros(m,1); 
 fi29=sign(qp2); fi210=zeros(m,1);  
 fi211=(1-abs(sign(qp2)));
 
 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111;    
     fi21, fi22, fi23, fi24, fi25, fi26, fi27, fi28, fi29, fi210, fi211];
 theta=mincuadm(tau,fi,m,11,2);%identificación multivariable
 theta