format long;
%D = [-sin(phi) cos(phi) 1
%    -sin(phi) -cos(phi) 1
%    sin(theta) -cos(theta) 1
%    sin(theta) cos(theta) 1]

%v_x positivo para esquerda
%v_y positivo para direita
%wR positivo para cima
D = [sin(phi) cos(phi) 1 %motor 0
    -sin(theta) cos(theta) 1 %motor 1
    sin(phi) -cos(phi) 1 %motor 2
    -sin(theta) -cos(theta) 1] %motor 3
    
B = pinv(D)
%para valores phi=30 e theta=45: M = eye(4)-D*B
M = eye(4)-D*B
M_std = [ 0.200000000000000  -0.244948974278318  -0.200000000000000   0.244948974278318
         -0.244948974278318   0.300000000000000   0.244948974278318  -0.300000000000000
         -0.200000000000000   0.244948974278318   0.200000000000000  -0.244948974278318
          0.244948974278318  -0.300000000000000  -0.244948974278318   0.300000000000000]
%u = [0.2 -0.2 0.245 -0.245]

C_a = [-sin(phi) -sin(phi) sin(theta) sin(theta)
    %cos(phi) -cos(phi) -sin(theta) sin(theta)
    cos(phi) -cos(phi) -cos(theta) cos(theta)
    1/alfa 1/alfa 1/alfa 1/alfa]

C_ab = [-sin(phi) -sin(phi) sin(theta)
cos(phi) -cos(phi) -cos(theta)
1 1 1]

f = C_ab\C_a(:,4);

f(4)=-1

g = null(C_a)