Hi,

Also, if I have trouble understanding your algorithm, would I be able to ask you a question?

Surely, I will answered at the weekend.

Only short, I'm in a hurry. I just remove the plane 4, the modified code is:

%% 3-Faces-Pyramid
%
%   nx(i) * x(j,i) + ny(i) * y(j,i) + nz(i) * z(j,i) - d(i) = 0
% with i = 1 .. 3
%
%   ||n(i)|| == 1
% 
%  and 
%
%   n(i)' * Apex -d(i) == 0 
%
%
% @version 1.0
% @author Michael Loesler - derletztekick.com
 
clear all
close all
format long g
clc
 
plane1 = [
 -8.7740 10.2929 -0.2645
 -8.7546 10.2701 -0.3177
 -8.8042 10.2864 -0.2647
 -8.7602 10.2767 -0.2908
 -8.7154 10.3041 -0.2639
 -8.7347 10.2841 -0.2905
 -8.7470 10.2985 -0.2641
 -8.8216 10.3067 -0.2113
 -8.8498 10.2977 -0.2114
 -8.7888 10.3102 -0.2109
 -8.8108 10.2940 -0.2379
 -8.7844 10.3051 -0.2378
 -8.7524 10.3050 -0.2374
 -8.7637 10.3184 -0.2109
 -8.8621 10.3120 -0.1847
 -8.8280 10.3142 -0.1843
 -8.7998 10.3231 -0.1842
 -8.7734 10.3298 -0.1841
 -8.7055 10.3322 -0.2103
 -8.7251 10.3155 -0.2372
 -8.6989 10.3245 -0.2371
 -8.7321 10.3238 -0.2106
 -8.6751 10.3405 -0.2103
 -8.7436 10.3375 -0.1838
 -8.7142 10.3427 -0.1837
 -8.6877 10.3555 -0.1837
 -8.6584 10.3609 -0.1835
];
 
plane2 = [
 -8.9011 10.3140 -0.4292
 -8.8979 10.3103 -0.4021
 -8.8503 10.2982 -0.4009
 -8.8934 10.3051 -0.3204
 -8.8922 10.3037 -0.3744
 -8.8957 10.3077 -0.3475
 -8.8903 10.3014 -0.2660
 -8.8403 10.2866 -0.3192
 -8.8439 10.2908 -0.3733
 -8.8447 10.2917 -0.3463
 -8.7987 10.2798 -0.3454
 -8.7972 10.2781 -0.3185
 -8.8420 10.2886 -0.2651
 -8.8414 10.2879 -0.2920
 -8.7972 10.2781 -0.2914
 -8.8914 10.3028 -0.2118
 -8.8913 10.3026 -0.2390
 -8.8911 10.3024 -0.1848
 -8.8431 10.2899 -0.2381
];
 
plane3 = [
 -8.7668 10.3218 -0.4267
 -8.8744 10.3263 -0.4560
 -8.7807 10.3382 -0.4543
 -8.8118 10.3373 -0.4551
 -8.8147 10.2984 -0.4003
 -8.8596 10.3091 -0.4282
 -8.8263 10.3121 -0.4275
 -8.7545 10.3073 -0.3992
 -8.7821 10.3024 -0.3996
 -8.7932 10.3154 -0.4269
 -8.7576 10.2737 -0.3445
 -8.8032 10.2850 -0.3726
 -8.7698 10.2880 -0.3719
 -8.6894 10.3576 -0.4529
 -8.7478 10.3424 -0.4536
 -8.7209 10.3506 -0.4534
 -8.7354 10.3278 -0.4262
 -8.7101 10.3377 -0.4259
 -8.7437 10.2946 -0.3717
];
 
 
% A-priori Coordinates of Apex
xapex = -8.7;
yapex = 10.3;
zapex = -0.3;
 
% Plane Parameters of face 1...4
nx1 = -0.211;
ny1 =  0.875;
nz1 = -0.436;
d1  = 10.971;
 
nx2 =  0.280;
ny2 =  0.959;
nz2 =  0.029;
d2  =  7.389;
 
nx3 = -0.152;
ny3 =  0.831;
nz3 =  0.536;
d3  =  9.678;
 
 
% Vector of unknows
X0 = [xapex, yapex, zapex,...
      nx1, ny1, nz1, d1,...
      nx2, ny2, nz2, d2,...
      nx3, ny3, nz3, d3]';
 
Qll = eye(3*(length(plane1)+length(plane2)+length(plane3)));
 
plane10 = plane1;
plane20 = plane2;
plane30 = plane3;
 
maxIteration = 200;
dx = inf;
v = zeros(3*(length(plane1)+length(plane2)+length(plane3)),1);
while (abs(max(dx)) > sqrt(eps) && maxIteration >= 0)
    maxIteration = maxIteration - 1;
 
    % Jacobi
    A = zeros(length(plane1)+length(plane2)+length(plane3), length(X0));
    w = zeros(size(A,1), 1);
    % Restriction
    R = zeros(6, length(X0));
    r = zeros(6, 1);
    % Condition
    B = zeros(size(A,1), size(A,1)*3);
 
 
    A(1:length(plane1), 4:7)                                                                                             = [plane1 -ones(length(plane1), 1)];
    A(length(plane1)+1:length(plane1)+length(plane2), 8:11)                                                              = [plane2 -ones(length(plane2), 1)];
    A(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 12:15)                               = [plane3 -ones(length(plane3), 1)];
 
    w(1:length(plane1), 1)                                                                                           = plane1 * X0(4:6)   - X0(7);
    w(length(plane1)+1:length(plane1)+length(plane2), 1)                                                             = plane2 * X0(8:10)  - X0(11);
    w(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 1)                               = plane3 * X0(12:14) - X0(15);
 
    B(1:length(plane1), 1:3*length(plane1)) = kron(eye(length(plane1)), X0(4:6)');
    B(length(plane1)+1:length(plane1)+length(plane2), 3*length(plane1)+1:3*(length(plane1)+length(plane2))) = kron(eye(length(plane2)), X0(8:10)');
    B(length(plane1)+length(plane2)+1:length(plane1)+length(plane2)+length(plane3), 3*(length(plane1)+length(plane2))+1:3*(length(plane1)+length(plane2)+length(plane3))) = kron(eye(length(plane3)), X0(12:14)');
 
    R(1, 4:6)   = 2.*X0(4:6);
    R(2, 8:10)  = 2.*X0(8:10);
    R(3, 12:14) = 2.*X0(12:14);
 
    R(4, [1:3,  4:7])  = [X0(4:6)'   X0(1:3)' -1];
    R(5, [1:3, 8:11])  = [X0(8:10)'  X0(1:3)' -1];
    R(6, [1:3,12:15])  = [X0(12:14)' X0(1:3)' -1];
 
    r(1,1) = X0(4:6)'  *X0(4:6)   - 1;
    r(2,1) = X0(8:10)' *X0(8:10)  - 1;
    r(3,1) = X0(12:14)'*X0(12:14) - 1;
 
    r(4,1) = X0(4:6)'  *X0(1:3) - X0(7);
    r(5,1) = X0(8:10)' *X0(1:3) - X0(11);
    r(6,1) = X0(12:14)'*X0(1:3) - X0(15);
 
    w = -B*v + w;
 
    N = [   B*Qll*B'                          A           zeros(size(A,1),size(R,1))
               A'                      zeros(size(A,2))             R'
         zeros(size(R,1), size(A,1))      R           zeros(size(R,1),size(R,1))     ];
 
    invN = -inv(N);
    kxk = invN * [w; zeros(size(X0,1),1); r];
 
    k1 = kxk(1:1:size(A,1));
    dx = kxk(size(k1,1)+1:1:size(X0,1)+size(k1,1));
    k2 = kxk(size(k1,1)+size(dx,1)+1:1:size(kxk,1));
 
    X0 = X0+dx;
    v = Qll*B'*k1;
 
    for i=1:length(plane1)
        plane1(i, :) = plane10(i, :) + v(3*(i-1)+1:3*(i-1)+3)';
    end
 
    for i=1:length(plane2)
        plane2(i, :) = plane20(i, :) + v(3*length(plane1)+3*(i-1)+1 : 3*length(plane1)+3*(i-1)+3)';
    end
 
    for i=1:length(plane3)
        plane3(i, :) = plane30(i, :) + v(3*(length(plane1)+length(plane2))+3*(i-1)+1 : 3*(length(plane1)+length(plane2))+3*(i-1)+3)';
    end
end
 
omega = v'*inv(Qll)*v + 2*k1'*(B*v+A*dx+w) + 2*k2'*(R*dx+r);
Qxx = invN(size(A,1)+1:1:size(A,1)+size(X0,1), size(A,1)+1:1:size(A,1)+size(X0,1));
sigma = omega / (size(A,1)+size(R,1)-size(A,2));
Qxx = sigma .* Qxx;  
 
disp([X0 sqrt(diag(Qxx))]);
 
plot3(plane1(:,1), plane1(:,2), plane1(:,3),'.b',...
      plane2(:,1), plane2(:,2), plane2(:,3),'.r',...
      plane3(:,1), plane3(:,2), plane3(:,3),'.g',...
      X0(1), X0(2), X0(3), '*c');

regards
Micha

--
applied-geodesy.org - OpenSource Least-Squares Adjustment Software for Geodetic Sciences