%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PARAMETERIZATION
%Equations of motion
q_dot = [qdot1 qdot2]'
q_dotdot = [qdotdot1 qdotdot2]'
tau = B*q_dotdot + C*q_dot + G

%Find the regressor
param_pi = [m1 Izz1 m2 Izz2]'
c1 = coeffs(tau(1),[m1 Izz1 m2 Izz2])
c2 = coeffs(tau(2),[m2 Izz2])

Y = [c1(4) c1(3) c1(2) c1(1);...
      0     0    c2(2) c2(1)]