Table of Contents

........................................................................................................................................ 1
Read observations (X,Y,Z,x,y,z) ............................................................................................ 1
General LSA ...................................................................................................................... 1
Global Test (alpha = 0.05) .................................................................................................... 4

% clc;
% clear all;

Read observations (X,Y,Z,x,y,z)

xyz = dlmread('data2.txt','',0,1);
X = xyz(:,1);
Y = xyz(:,2);
Z = xyz(:,3);
x = xyz(:,4);
y = xyz(:,5);
z = xyz(:,6);

General LSA
np = size(xyz,1);
n = np*6;
r = np*3 - 7;
n0 = n-r;
u = 7;
c = r+u;

% Original observations
l = [];
for i = 1:np
l = [l; X(i); Y(i); Z(i); x(i); y(i); z(i)];
end

% Initial Approximations
sigma0 = 0.07;
W = eye(n);

lambda = 1;
w = deg2rad(1);
phi = deg2rad(1);
K = deg2rad(1);
tx = 0.5;
ty = 0.5;
tz = 0;

l0 = l;
x0 = [lambda; w; phi; K; tx; ty; tz];
Delta = 1;

1
niter = 0;
fprintf('\n\nInitial Parameter Approximations:\nlambda = %f;\nomega =
%f;\nphi = %f;\nkappa = %f;\nt_x = %f;\nt_y = %f;\nt_z = %f\n',x0);
fprintf('\n\nGLS Counting:\nnp = %d; n = %d; n0 = %d; r = %d; u = %d;
c = %d\n',np,n,n0,r,u,c);

while (abs(Delta) >= 1e-8)

% Rotation matrices and their partial derivatives
Mw = [1 0 0;
0 cos(w) sin(w);
0 -sin(w) cos(w)];
dMw = [0 0 0;
0 -sin(w) cos(w);
0 -cos(w) -sin(w)];

Mphi = [cos(phi) 0 -sin(phi);

0 1 0;
sin(phi) 0 cos(phi)];
dMphi = [-sin(phi) 0 -cos(phi);
0 0 0;
cos(phi) 0 -sin(phi)];

Mk = [cos(K) sin(K) 0;
-sin(K) cos(K) 0;
0 0 1];
dMk = [-sin(K) cos(K) 0;
-cos(K) -sin(K) 0;
0 0 0];

M = Mk*Mphi*Mw;

% Create A, B, f matrices
F = [];
A = zeros(c,n);
B = zeros(c,u);
for i = 1:np
xyz = [x(i); y(i); z(i)];
XYZ = [X(i); Y(i); Z(i)];
F = [F; xyz - lambda*M*XYZ - [tx; ty; tz]];

dF_dlam = -M*XYZ;
dF_dw = -lambda*Mk*Mphi*dMw*XYZ;
dF_dphi = -lambda*Mk*dMphi*Mw*XYZ;
dF_dK = -lambda*dMk*Mphi*Mw*XYZ;
dF_dtx = [-1; 0; 0];
dF_dty = [0; -1; 0];
dF_dtz = [0; 0; -1];
B(3*i-2:3*i,:) = [dF_dlam dF_dw dF_dphi dF_dK dF_dtx dF_dty
dF_dtz ];

dF_dx = [1; 0; 0];

dF_dy = [0; 1; 0];
dF_dz = [0; 0; 1];
dF_dX = -lambda*M(:,1);

2
 dF_dY = -lambda*M(:,2);
dF_dZ = -lambda*M(:,3);
A(3*i-2:3*i,6*i-5:6*i) = [dF_dX dF_dY dF_dZ dF_dx dF_dy
dF_dz];
end

f = -F-A*(l-l0);

% GLS Adjustment
Q = inv(W);
Qe = A*Q*A';
We = inv(Qe);
Delta = inv(B'*We*B)*B'*We*f;
K = We*(f-B*Delta);
v = Q*A'*K;

if (niter == 0)
fprintf('\n\n"A" Matrix (Block Diagonal Element) for 1st
iteration:\n(repetitive block values for 1st iteration)\n');
fprintf(' %9.4f %9.4f %9.4f %2d %2d %2d
\n',A(1:3,1:6)');
fprintf('\n\nPart of "B" Matrix for 1st iteration:\n(actual
size = 54x7)\n');
fprintf(' %9.4f %9.4f %9.4f %9.4f %2d %2d %2d
\n',B(1:12,1:7)');
end

% Updation of observations and parameters

x0 = x0+Delta;
l0 = l+v;
niter = niter+1;

fprintf('\n\n\nIteration No. %d\n', niter);

fprintf('Delta: \n');
fprintf('%10.9f\n',Delta);

lambda = x0(1);
w = x0(2); phi = x0(3); K = x0(4);
tx = x0(5); ty = x0(6); tz = x0(7);
X = l0(1:6:6*np-5);
Y = l0(2:6:6*np-4);
Z = l0(3:6:6*np-3);
x = l0(4:6:6*np-2);
y = l0(5:6:6*np-1);
z = l0(6:6:6*np);
end
w = rad2deg(w);
phi = rad2deg(phi);
K = rad2deg(K);

fprintf('\n\nTotal GLS iterations = %d\n',niter);

fprintf('Final parameter values:\nlambda = %f;\nomega = %f;
\nphi = %f;\nkappa = %f;\nt_x = %f;\nt_y = %f;\nt_z = %f
\n',lambda,w,phi,K,tx,ty,tz);

3
 fprintf('\n\nFinal residuals:\n-----|v_X|-----|v_Y|------|v_Z|-----|
v_x|------|v_y|------|v_z|-----\n');
fprintf(' %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f\n',v);
fprintf('\n\nAdjusted observations:\n-----|X|--------|Y|--------|
Z|--------|x|--------|y|--------|z|-----\n');
fprintf(' %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f\n',l0);

Global Test (alpha = 0.05)

Find test statistic

ts = v'*W*v/(sigma0^2);

% compute and show confidence intervals & regions

alpha = 0.05;
cv1 = icdf('chi2',alpha/2,r);
cv2 = icdf('chi2',1-(alpha/2),r);

% Check if hypothesis is accepted or rejected

if(ts>cv1 && ts<cv2)
fprintf('\n\nHypothesis is accepted!!\n');
fprintf('\n\n----|cv1|-------|ts|-------|cv2|----\n');
fprintf(' %7.4f %7.4f %7.4f\n',cv1,ts,cv2);
variance = sigma0;
fprintf('\n\nVariance (prior) = %f\n',variance);
else
fprintf('Hypothesis is rejected!!');
fprintf('\n\nVariance (a-posteriori) = %f\n',variance);
variance = (v'*W*v)/r;
end

Initial Parameter Approximations:

lambda = 1.000000;
omega = 0.017453;
phi = 0.017453;
kappa = 0.017453;
t_x = 0.500000;
t_y = 0.500000;
t_z = 0.000000

GLS Counting:
np = 18; n = 108; n0 = 61; r = 47; u = 7; c = 54

"A" Matrix (Block Diagonal Element) for 1st iteration:

(repetitive block values for 1st iteration)
-0.9997 -0.0178 0.0171 1 0 0
0.0174 -0.9997 -0.0178 0 1 0
-0.0175 0.0174 -0.9997 0 0 1

4
Part of "B" Matrix for 1st iteration:
(actual size = 54x7)
0.0744 0.0013 -0.0254 0.0471 -1 0 0
0.0471 0.0241 0.0004 -0.0744 0 -1 0
0.0254 -0.0484 0.0736 0.0000 0 0 -1
-2.0310 0.0032 -0.0152 0.1654 -1 0 0
0.1654 0.0507 0.0003 2.0310 0 -1 0
0.0153 -0.1299 -2.0336 0.0000 0 0 -1
-3.9841 0.0014 0.0801 0.1584 -1 0 0
0.1584 -0.0106 -0.0014 3.9841 0 -1 0
-0.0801 -0.0888 -3.9862 0.0000 0 0 -1
0.0190 -0.0373 0.0079 -2.1291 -1 0 0
-2.1291 -0.0082 -0.0001 -0.0190 0 -1 0
-0.0079 2.1282 0.0561 0.0000 0 0 -1

Iteration No. 1
Delta:
0.093083346
-0.010437922
-0.003051171
0.027424916
0.029275661
0.009985307
0.531660202

Iteration No. 2
Delta:
0.002771810
0.000846104
0.000062004
-0.002174452
-0.005377880
-0.004054771
-0.002377882

Iteration No. 3
Delta:
-0.000005346
-0.000026374
-0.000005236
0.000039551
-0.000057043
0.000118681
-0.000036520

Iteration No. 4

5
Delta:
0.000002880
0.000001193
0.000000220
-0.000001818
-0.000001962
-0.000010897
-0.000000722

Iteration No. 5
Delta:
-0.000000005
-0.000000035
-0.000000007
0.000000034
-0.000000065
0.000000124
-0.000000057

Total GLS iterations = 5

Final parameter values:
lambda = 1.095853;
omega = 0.448985;
phi = 0.828446;
kappa = 2.448909;
t_x = 0.523839;
t_y = 0.506038;
t_z = 0.529245

Final residuals:
-----|v_X|-----|v_Y|------|v_Z|-----|v_x|------|v_y|------|v_z|-----
0.0560 0.0163 0.0277 -0.0514 -0.0128 -0.0259
0.0466 0.0925 0.0202 -0.0458 -0.0826 -0.0184
0.0055 0.0072 -0.0020 -0.0053 -0.0064 0.0018
-0.0090 -0.0631 -0.0634 0.0099 0.0576 0.0575
-0.0433 0.0769 -0.0474 0.0359 -0.0714 0.0443
-0.0260 0.0247 -0.0589 0.0220 -0.0231 0.0543
0.0149 -0.0219 0.0390 -0.0123 0.0203 -0.0359
0.0029 -0.0404 0.0272 -0.0007 0.0367 -0.0252
0.0914 0.0070 -0.1095 -0.0850 -0.0020 0.0988
0.0229 -0.0159 -0.0090 -0.0204 0.0155 0.0078
0.0066 -0.0590 0.0279 -0.0033 0.0538 -0.0259
-0.0597 -0.1331 0.0272 0.0600 0.1188 -0.0250
0.0570 0.0467 0.0531 -0.0531 -0.0407 -0.0489
-0.0409 0.0641 0.0655 0.0357 -0.0605 -0.0587
-0.0750 -0.0151 -0.0468 0.0684 0.0112 0.0436
-0.0348 -0.0481 -0.0711 0.0327 0.0430 0.0650
0.0325 0.0645 0.0879 -0.0310 -0.0582 -0.0801
-0.0476 -0.0031 0.0326 0.0439 0.0008 -0.0291

6
Adjusted observations:
-----|X|--------|Y|--------|Z|--------|x|--------|y|--------|z|-----
-0.0180 -0.0317 0.0027 0.5026 0.4722 0.5321
2.0796 -0.0365 -0.0328 2.7992 0.3684 0.5266
3.9925 -0.0818 0.0070 4.8907 0.2296 0.6008
-0.0650 2.0649 -0.0174 0.5499 2.7696 0.4915
2.0117 1.9129 -0.0624 2.8169 2.5056 0.4763
3.9950 2.0217 -0.0519 4.9930 2.5319 0.5183
-0.1041 4.0261 -0.0510 0.5997 4.9183 0.4371
2.0469 4.0226 -0.0478 2.9543 4.8137 0.4748
3.9894 4.0990 -0.0935 5.0850 4.8060 0.4548
0.0219 0.0441 1.9610 0.5196 0.5715 2.6778
1.9426 -0.0020 2.0499 2.6187 0.4318 2.8061
3.9203 -0.0191 1.9772 4.7840 0.3198 2.7580
0.0180 1.9357 2.0391 0.6029 2.6433 2.7471
2.0061 1.9711 1.9925 2.7817 2.5885 2.7273
4.0660 2.0609 1.9682 5.0414 2.5902 2.7326
0.0052 4.0189 1.9779 0.6877 4.9240 2.6620
1.9305 3.9935 2.0529 2.7930 4.8068 2.7749
3.9884 3.9349 2.0026 5.0439 4.6458 2.7529

Hypothesis is accepted!!

----|cv1|-------|ts|-------|cv2|----
29.9562 52.9809 67.8206

Variance (prior) = 0.070000

Published with MATLAB R2015a