clc; clear all; close all; format short g
global tau1 tau2 tau3
 ti=0; h=0.001; tf = 5; t=ti:h:tf; %vector tiempo
    ci=[0; 0; 0; 0;0; 0]';
    opciones=odeset('RelTol',1e-3,'InitialStep',1e-3,'MaxStep',1e-3);
    %solución numérica de la dinámica del robot cartesiano de 3 gdl
    [t,x]=ode45('cap7_idecartesiano3gdl',t,ci,opciones);
   
  
    d1=x(:,1); d2=x(:,2); d3=x(:,3); dp1=x(:,4); dp2=x(:,5); dp3=x(:,6);
    
    [m n]=size(t); dpp1=zeros(m,1); dpp2=zeros(m,1); dpp3=zeros(m,1);
     u1=zeros(m,1); u2=zeros(m,1); u3=zeros(m,1);
    
    for k=1:m
     xp=cap7_idecartesiano3gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4), x(k,5), x(k,6)]);
     dpp1(k,1)=xp(4,1); dpp2(k,1)=xp(5,1); dpp3(k,1)=xp(6,1);
     u1(k,1) =tau1;    u2(k,1) =tau2; u3(k,1) =tau3;
    end

y=dp1.*u1+dp2.*u2+dp3.*u3;        
 fi11=dp1.*dpp1;  
 fi12=dp2.*dpp2; 
 fi13=dp3.*dpp3; 
 fi14=dp1.*dp1; 
 fi15=dp2.*dp2;  
 fi16=dp3.*dp3;   
 fi17=abs(dp1);   
 fi18=abs(dp2);  
 fi19=abs(dp3); 
 fi110=dp1;
 
 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110]; 
 [r,theta]=mincuad(y,fi);
 theta %vector estimado theta
plot(t,r)%grafica la variación temporal de los componentes del vector estimado detheta