A = [-284 -230 -176 176 230 284;
140 -63 -266 -266 -63 140;
-0.30 0.30 -0.30 0.30 -0.30 0.30];
AA = abs(A);
B = pinv(A);
m1 = max(B(:,1));
m2 = max(B(:,2));
m3 = max(B(:,3));
Tmp(:,1) = B(:,1)/ m1;
Tmp(:,2) = B(:,2)/ m2;
Tmp(:,3) = B(:,3)/ m3;
Res = round(100 * Tmp)
cr1 = Res(1,1);
cr2 = Res(2,1);
cr3 = Res(3,1);
cr4 = Res(4,1);
cr5 = Res(5,1);
cr6 = Res(6,1);
cp1 = Res(1,2);
cp2 = Res(2,2);
cp3 = Res(3,2);
cp4 = Res(4,2);
cp5 = Res(5,2);
cp6 = Res(6,2);
cy1 = Res(1,3);
cy2 = Res(2,3);
cy3 = Res(3,3);
cy4 = Res(4,3);
cy5 = Res(5,3);
cy6 = Res(6,3);
YawThrottle(1,1) = cy1 + cy2 + cy3 + cy4 +cy5 +cy6;
YawThrottle(1,2) = 0
YawPitch(1,1) = cy1 * AA(2,1) + cy6 * AA(2,6);
YawPitch(1,2) = cy2 * AA(2,2) + cy3 * AA(2,3) + cy4 * AA(2,4) + cy5 * AA(2,5)
YawRoll(1,1) = cy1 * AA(1,1) + cy2 * AA(1,2) + cy3 * AA(1,3);
YawRoll(1,2) = cy4 * AA(1,4) + cy5 * AA(1,5) + cy6 * AA(1,6)
PitchThrottle(1,1) = cp1 + cp2 + cp3 + cp4 + cp5 +cp6;
PitchThrottle(1,2) = 0
PitchYaw(1,1) = cp1 + cp3 + cp5;
PitchYaw(1,2) = cp2 + cp4 + cp6
PitchRoll(1,1) = cp1 * AA(1,1) + cp2 * AA(1,2) + cp3 * AA(1,3);
PitchRoll(1,2) = cp4 * AA(1,4) + cp5 * AA(1,5) + cp5 * AA(1,5)
RollThrottle(1,1) = cr1 + cr2 + cr3 + cr4 +cr5 +cr6;
RollThrottle(1,2) = 0
RollYaw(1,1) = cr1 + cr3 + cr5;
RollYaw(1,2) = cr2 + cr4 + cr6
RollPitch(1,1) = cr1 * AA(2,1) + cr6 * AA(2,6);
RollPitch(1,2) = cr2 * AA(2,2) + cr3 * AA(2,3) + cr4 * AA(2,4) + cr5 * AA(2,5)
rp1 = cr1 * AA(2,1);
rp2 = cr2 * AA(2,2);
rp3 = cr3 * AA(2,3);
rp4 = cr4 * AA(2,4);
rp5 = cr5 * AA(2,5);
rp6 = cr6 * AA(2,6);