test_carl.m

clear all
global m;
global B;
global k;

global mpi;
global ml;
ml = 0;

global mp;
mp = 2;

d = 2/31;
d2 = 5/31;

scale = 0.3264;

%punkt-plan korrespondanser %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Ange antal plan punkt korrespondanser mpi
mpi = 27;
%Skriv in nomaler till planen n
n = [...
        -1/sqrt(2) 0 1/sqrt(2); ...
        -1/sqrt(2) 0 1/sqrt(2); ...
        -1/sqrt(2) 0 1/sqrt(2); ...
        0 -1/sqrt(2) 1/sqrt(2); ...
        0 -1/sqrt(2) 1/sqrt(2); ...
        0 -1/sqrt(2) 1/sqrt(2); ...
        1/sqrt(2) 0 1/sqrt(2); ...
        1/sqrt(2) 0 1/sqrt(2); ...
        1/sqrt(2) 0 1/sqrt(2); ...
        0 1/sqrt(2) 1/sqrt(2); ...
        0 1/sqrt(2) 1/sqrt(2); ...
        0 1/sqrt(2) 1/sqrt(2); ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        -1 0 0; ...
        -1 0 0; ...
        -1 0 0; ...
        1 0 0; ...
        1 0 0; ...
        1 0 0; ...
        0 -1 0; ...
        0 -1 0; ...
        0 -1 0; ...
        0 1 0; ...
        0 1 0; ...
        0 1 0; ...
    ]';
        
%punkter på planen ypi
ypi = [...
        0 0 d2; ...
        0 0 d2; ...
        0 0 d2; ...
        1+d -1-d d2; ...
        1+d -1-d d2; ...
        1+d -1-d d2; ...
        3+2*d 0 d2; ...
        3+2*d 0 d2; ...
        3+2*d 0 d2; ...
        1+d 2+d d2; ...
        1+d 2+d d2; ...
        1+d 2+d d2; ...
        1+d 1 1+d; ...
        1+d 1 1+d; ...
        1+d 1 1+d; ...
        1+d 0 0; ...
        1+d 0 0; ...
        1+d 0 0; ...
        2+d 0 0; ...
        2+d 0 0; ...
        2+d 0 0; ...
        1+d 0 0; ...
        1+d 0 0; ...
        1+d 0 0; ...
        1+d 1 0; ...
        1+d 1 0; ...
        1+d 1 0 ...
    ]';
%mätpunkter    
xpi = scale*[...
35.7523 14.9130 2.3913 ; ...
35.9119 16.8331 2.4082 ; ...
33.9933 15.7678 4.2237 ; ...
32.9470 18.7614 3.8901 ; ...
32.6966 20.3653 2.2209 ; ...
30.9807 20.0255 2.6940 ; ...
29.1302 17.4000 3.8614 ; ...
27.2992 16.3562 2.0987 ; ...
29.0671 15.3619 3.9770 ; ...
30.6120 13.8958 4.1509 ; ...
32.4087 13.7291 4.1239 ; ...
32.3165 12.0553 2.3290 ; ...
32.5733 15.3796 4.5553 ; ...
30.8647 17.2839 4.5728 ; ...
32.8226 16.9062 4.5509 ; ...
33.3890 18.3514 3.3161 ; ...
33.4758 19.5356 2.0612 ; ...
33.0059 12.2761 2.1305 ; ...
30.3224 18.8118 3.1844 ; ...
30.3899 19.8053 1.8780 ; ...
29.9569 13.4604 2.3450 ; ...
35.1031 17.5877 2.1335 ; ...
34.3660 17.6231 2.4881 ; ...
29.3501 17.9471 2.4718 ; ...
33.9035 14.5054 3.5535 ; ...
34.9201 14.4384 2.2512 ; ...
29.1911 14.8704 3.5860  ...
]';


%punkt-linje korrespondanser %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Ange antal linje punkt korrespondanser ml
ml = 12
%Skriv in riktningskoefficienter till linjerna v
v = [...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1; ...
        0 0 1]';
%Punter på linjerna yl
yl = [...
        1+d 0 0; ...
        1+d 0 0; ...
        1+d 0 0; ...
        2+d 0 0; ...
        2+d 0 0; ...
        2+d 0 0; ...
        2+d 1 0; ...
        2+d 1 0; ...
        2+d 1 0; ...
        1+d 1 0; ...
        1+d 1 0; ...
        1+d 1 0 ...
]';
%Uppmätta punkter på linjerna
xl = scale*[...
33.3659 17.6175 4.0228 ; ...
33.3439 17.6286 3.2425 ; ...
33.3383 17.6318 2.1417 ; ...
30.3122 17.8735 4.0483 ; ...
30.2985 17.8840 3.1541 ; ...
30.3023 17.8896 1.9503 ; ...
30.0770 14.7661 3.9396 ; ...
30.0549 14.7578 2.5215 ; ...
30.0418 14.7579 1.8449 ; ...
33.1576 14.5466 3.7905 ; ...
33.1607 14.5596 2.9585 ; ...
33.1468 14.5772 1.7961  ...
]';
 

%punkt-punkt korrespondanser %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Ange antal punkt punkt korrespondanser ml
mp = 10
%modell Punter
yp = [...
        0 0 d2; ...
        1+d -1-d d2; ...
        2+d -1-d d2; ...
        3+2*d 0 d2; ...
        3+2*d 1 d2; ...
        2+d 2+d d2; ...
        1+d 2+d d2; ...
        0 1 d2; ...
        1+d-d2 0 1+d; ...
        2+d+d2 1 1+d; ...
]';

%Uppmätta punkter
xp = scale*[... 
36.5888 17.4763 1.7289 ; ...
33.5409 20.8242 1.6484 ; ...
30.4845 21.0278 1.6225 ; ...
27.0145 18.0106 1.6042 ; ...
26.8720 15.0103 1.7909 ; ...
29.8630 11.6156 1.6632 ; ...
32.9191 11.4212 1.6810 ; ...
36.3578 14.3407 1.7290 ; ...
33.8774 17.6196 4.4838 ; ...
29.5339 14.9245 4.4780 ; ...
]'

% Beräkna matriser %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = zeros(3,3);
if mp > 0
   N = N + mp*eye(3);
end

if ml > 0
   V = cell(1,ml);
   for i = 1:ml;
      V{i} = eye(3) - v(:,i)*v(:,i)';
      N = N + V{i}'*V{i};
   end
end

if mpi > 0
   for i = 1:mpi
      N = N + n(:,i)*n(:,i)';
   end
end

if rank(N) < 3;
    disp('N ej inverterbar');
end

% Beräkna målfunktion (punkt-plan delen) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mpi > 0
   A = cell(1,mpi);
   for i = 1:mpi
      a = n(:,i);
      b = xpi(:,i);
      A{i} = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
	      a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
	      
	      a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
	      a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
	      
	      a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
	      -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		
	      a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
	      a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
   end
   
   
   A1 = cell(1,mpi);
   for i = 1:mpi
      A1{i} = zeros(4,4);
      slask = zeros(4,4);
      for j = 1:mp
	 a = (n(:,i)'*N^-1)' ;
	 b = xp(:,j);
	 slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		  
		  a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		  a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		  
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		  -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		  
		  a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		  a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	 A1{i} = A1{i} + slask;
      end
   end
   
   k1 = cell(1,mpi);
   for i = 1:mpi
      slask = 0;
      for j = 1:mp
	 slask = slask + n(:,i)'*N^-1*yp(:,j);
      end
      k1{i} = slask;
   end
   
   A2 = cell(1,mpi);
   for i = 1:mpi 
      A2{i} = zeros(4,4);
      slask = zeros(4,4);
      for j = 1:ml
	 a = (n(:,i)'*N^-1*V{j}'*V{j})' ;
	 b = xl(:,j);
	 slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		  
		  a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		  a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		  
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		  -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		  
		  a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		  a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	 A2{i} = A2{i} + slask;
      end
   end
   
   k2 = cell(1,mpi);
   for i = 1:mpi
      slask = 0;
      for j = 1:ml
	 slask = slask + n(:,i)'*N^-1*V{j}'*V{j}*yl(:,j);
      end
      k2{i} = slask;
   end
   
   A3 = cell(1,mpi);
   for i = 1:mpi
      slask = zeros(4,4); 
      A3{i} = zeros(4,4);
      for j = 1:mpi
	 a = (n(:,i)'*N^-1*n(:,j)*n(:,j'))' ;
	 b = xpi(:,j);
	 slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		  
		  a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		  a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		  
		  a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		  -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		  
		  a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		  a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	 A3{i} = A3{i} + slask;
      end
   end

   k3 = cell(1,mpi);
   for i = 1:mpi
      slask = 0;
      for j = 1:mpi
	 slask = slask + n(:,i)'*N^-1*n(:,j)*n(:,j)'*ypi(:,j);
      end
      k3{i} = slask;
   end
   
   B = cell(1,mpi);
   k = cell(1,mpi);
   for i = 1:mpi
      B{i} = A{i}-A1{i}-A2{i}-A3{i};
      k{i} = k1{i} + k2{i} + k3{i} - n(:,i)'*ypi(:,i);
   end
end

   
% Beräkna målfunktion (punkt-linje delen) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ml > 0;
   A = cell(1,ml);
   for ii = 1:3
      for i = 1:ml
	 vik = V{i}(ii,:);
	 a = vik';
	 b = xl(:,i);
	 A{i} = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		 a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		 
		 a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		 a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		 
		 a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		 -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		 
		 a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		 a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
      end
      
      
      A1 = cell(1,ml);
      for i = 1:ml
	 vik = V{i}(ii,:);
	 A1{i} = zeros(4,4);
	 slask = zeros(4,4);
	 for j = 1:mp
	    a = (vik*N^-1)' ;
	    b = xp(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A1{i} = A1{i} + slask;
	 end
      end
      
      k1 = cell(1,ml);
      for i = 1:ml
	 vik = V{i}(ii,:);
	 slask = 0;
	 for j = 1:mp
	    slask = slask + vik*N^-1*yp(:,j);
	 end
	 k1{i} = slask;
      end
      
      A2 = cell(1,ml);
      for i = 1:ml 
	 A2{i} = zeros(4,4);
	 slask = zeros(4,4);
	 vik = V{i}(ii,:);
	 for j = 1:ml
	    a = (vik*N^-1*V{j}'*V{j})' ;
	    b = xl(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A2{i} = A2{i} + slask;
	 end
      end
      
      k2 = cell(1,ml);
      for i = 1:ml
	 slask = 0;
	 vik = V{i}(ii,:);
	 for j = 1:ml
	    slask = slask + vik*N^-1*V{j}'*V{j}*yl(:,j);
	 end
	 k2{i} = slask;
      end
      
      A3 = cell(1,ml);
      for i = 1:ml
	 vik = V{i}(ii,:);
	 slask = zeros(4,4); 
	 A3{i} = zeros(4,4);
	 for j = 1:mpi
	    a = (vik*N^-1*n(:,j)*n(:,j'))' ;
	    b = xpi(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A3{i} = A3{i} + slask;
	 end
      end
      
      k3 = cell(1,mpi);
      for i = 1:ml
	 slask = 0;
	 vik = V{i}(ii,:);
	 for j = 1:mpi
	    slask = slask + vik*N^-1*n(:,j)*n(:,j)'*ypi(:,j);
	 end
	 k3{i} = slask;
      end
      
      for i = 1:ml
	 vik = V{i}(ii,:);
	 B{end+1} = A{i}-A1{i}-A2{i}-A3{i};
	 k{end+1} = k1{i} + k2{i} + k3{i} - vik*yl(:,i);
      end
   end
end


% Beräkna målfunktion (punkt-punkt delen) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mp >0
   A = cell(1,mp);
   I = eye(3);
   for ii = 1:3
      for i = 1:mp
	 ek = I(ii,:);
	 a = ek';
	 b = xp(:,i);
	 A{i} = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		 a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		 
		 a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		 a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		 
		 a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		 -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		 
		 a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		 a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
      end
      
      
      A1 = cell(1,mp);
      for i = 1:mp
	 ek = I(ii,:);
	 A1{i} = zeros(4,4);
	 slask = zeros(4,4);
	 for j = 1:mp
	    a = (ek*N^-1)' ;
	    b = xp(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A1{i} = A1{i} + slask;
	 end
      end
      
      k1 = cell(1,mp);
      for i = 1:mp
	 ek = I(ii,:);
	 slask = 0;
	 for j = 1:mp
	    slask = slask + ek*N^-1*yp(:,j);
	 end
	 k1{i} = slask;
      end
      
      A2 = cell(1,mp);
      for i = 1:mp 
	 A2{i} = zeros(4,4);
	 slask = zeros(4,4);
	 ek = I(ii,:);
	 for j = 1:ml
	    a = (ek*N^-1*V{j}'*V{j})' ;
	    b = xl(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A2{i} = A2{i} + slask;
	 end
      end
      
      k2 = cell(1,mp);
      for i = 1:mp
	 slask = 0;
	 ek = I(ii,:);
	 for j = 1:ml
	    slask = slask + ek*N^-1*V{j}'*V{j}*yl(:,j);
	 end
	 k2{i} = slask;
      end
      
      A3 = cell(1,mp);
      for i = 1:mp
	 ek = I(ii,:);
	 slask = zeros(4,4); 
	 A3{i} = zeros(4,4);
	 for j = 1:mpi
	    a = (ek*N^-1*n(:,j)*n(:,j'))' ;
	    b = xpi(:,j);
	    slask = [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)  a(2)*b(1)+a(1)*b(2) ...
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)-a(2)*b(3); ...
		     
		     a(2)*b(1)+a(1)*b(2)  -a(1)*b(1)+a(2)*b(2)-a(3)*b(3) ...
		     a(3)*b(2)+a(2)*b(3)  -a(3)*b(1)+a(1)*b(3); ...
		     
		     a(3)*b(1)+a(1)*b(3)  a(3)*b(2)+a(2)*b(3) ...
		     -a(1)*b(1)-a(2)*b(2)+a(3)*b(3)  a(2)*b(1)-a(1)*b(2); ...
		     
		     a(3)*b(2)-a(2)*b(3)  -a(3)*b(1)+a(1)*b(3) ...
		     a(2)*b(1)-a(1)*b(2)  a(1)*b(1)+a(2)*b(2)+a(3)*b(3)];
	    A3{i} = A3{i} + slask;
	 end
      end
      
      k3 = cell(1,mp);
      for i = 1:mp
	 slask = 0;
	 ek = I(ii,:);
	 for j = 1:mpi
	    slask = slask + ek*N^-1*n(:,j)*n(:,j)'*ypi(:,j);
	 end
	 k3{i} = slask;
      end
      
      for i = 1:mp
	 ek = I(ii,:);
	 B{end+1} = A{i}-A1{i}-A2{i}-A3{i};
	 k{end+1} = k1{i} + k2{i} + k3{i} - ek*yp(:,i);
      end
   end
end


m = 3*ml+3*mp+mpi