A = [-0.43 -0.36 -0.29 -0.22 0.22 0.29 0.36 0.43;
-0.43 -0.12 0.19 0.50 0.50 0.19 -0.12 -0.43;
-0.30 0.30 -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);
cr7 = Res(7,1);
cr8 = Res(8,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);
cp7 = Res(7,2);
cp8 = Res(8,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);
cy7 = Res(7,3);
cy8 = Res(8,3);
YawThrottle(1,1) = cy1 + cy2 + cy3 + cy4 +cy5 +cy6 + cy7 + cy8;
YawThrottle(1,2) = 0
YawPitch(1,1) = cy1 * AA(2,1) + cy2 * AA(2,2) + cy7 * AA(2,7) + cy8 * AA(2,8);
YawPitch(1,2) = cy3 * AA(2,3) + cy4 * AA(2,4) + cy5 * AA(2,5) + cy6 * AA(2,6)
YawRoll(1,1) = cy1 * AA(1,1) + cy2 * AA(1,2) + cy3 * AA(1,3) + cy4 * AA(1,4);
YawRoll(1,2) = cy5 * AA(1,5) + cy6 * AA(1,6) + cy7 * AA(1,7) + cy8 * AA(1,8)
PitchThrottle(1,1) = cp1 + cp2 + cp3 + cp4 + cp5 +cp6 + cp7 + cp8;
PitchThrottle(1,2) = 0
PitchYaw(1,1) = cp1 + cp3 + cp5 + cp7;
PitchYaw(1,2) = cp2 + cp4 + cp6 + cp8
PitchRoll(1,1) = cp1 * AA(1,1) + cp2 * AA(1,2) + cp3 * AA(1,3) + cp4 * AA(1,4);
PitchRoll(1,2) = cp5 * AA(1,5) + cp6 * AA(1,6) + cp7 * AA(1,7) + cp8 * AA(1,8)
RollThrottle(1,1) = cr1 + cr2 + cr3 + cr4 +cr5 +cr6 + cr7 + cr8;
RollThrottle(1,2) = 0
RollYaw(1,1) = cr1 + cr3 + cr5 + cr7;
RollYaw(1,2) = cr2 + cr4 + cr6 + cr8
RollPitch(1,1) = cr1 * AA(2,1) + cr2 * AA(2,2) + cr7 * AA(2,7) + cr8 * AA(2,8);
RollPitch(1,2) = cr3 * AA(2,3) + cr4 * AA(2,4) + cr5 * AA(2,5) + cr6 * AA(2,6)
rp1 = cr1 * AA(2,1);
rp2 = cr2 * AA(2,2);
rp3 = cr7 * AA(2,7);
rp4 = cr8 * AA(2,8);
rp5 = cr3 * AA(2,3);
rp6 = cr4 * AA(2,4);
rp7 = cr5 * AA(2,5);
rp8 = cr6 * AA(2,6);