-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathleastSquareMethod.m
113 lines (96 loc) · 3.73 KB
/
leastSquareMethod.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
function estimated_position = leastSquareMethod(anchorPoints, distancesArray, zk) %#ok<INUSD>
for i = 1:length(anchorPoints)
eval(sprintf('Source%d = anchorPoints(%d).position;', i, i));
eval(sprintf('x%d = Source%d(1);', i, i));
eval(sprintf('y%d = Source%d(2);', i, i));
eval(sprintf('z%d = Source%d(3);', i, i));
end
for i = 1:length(anchorPoints)
eval(sprintf('r%d = distancesArray(%d);', i, i));
eval(sprintf('c%d = [x%d y%d z%d];', i, i,i,i));
end
x0 = [0 0 0]; % punto di partenza
options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt', 'Display', 'off');
if length(anchorPoints) == 5
% A = 2* [...
% x2-x1, y2-y1 ,z2-z1; ...
% x3-x1, y3-y1 ,z3-z1; ...
% x4-x1, y4-y1 ,z4-z1; ...
% x5-x1, y5-y1 ,z5-z1 ...
% ];
A = [...
x2-x1, y2-y1; ...
x3-x1, y3-y1; ...
x4-x1, y4-y1; ...
x5-x1, y5-y1; ...
];
% b = [...
% r1^2-r2^2-(x1^2+y1^2+z1^2) + (x2^2+y2^2+z2^2);...
% r1^2-r3^2-(x1^2+y1^2+z1^2) + (x3^2+y3^2+z3^2);...
% r1^2-r4^2-(x1^2+y1^2+z1^2) + (x4^2+y4^2+z4^2);...
% r1^2-r5^2-(x1^2+y1^2+z1^2) + (x5^2+y5^2+z5^2)...
% ];
b = 0.5*[...
r1^2-r2^2-(x1^2+y1^2+z1^2) + (x2^2+y2^2+z2^2) - 2*zk*(z2 - z1);...
r1^2-r3^2-(x1^2+y1^2+z1^2) + (x3^2+y3^2+z3^2) - 2*zk*(z3 - z1);...
r1^2-r4^2-(x1^2+y1^2+z1^2) + (x4^2+y4^2+z4^2)- 2*zk*(z4 - z1);...
r1^2-r5^2-(x1^2+y1^2+z1^2) + (x5^2+y5^2+z5^2)- 2*zk*(z4 - z1);...
];
estimated_position = pinv(A)*b;
estimated_position = [estimated_position; zk];
%[estimated_position, ~] = lsqnonlin(@(x)sphere_intersect(x, c1, r1, c2, r2, c3, r3, c4, r4, c5, r5, 5), x0, [], [], options);
elseif length(anchorPoints) == 4
% A = 2* [...
% x2-x1, y2-y1 ,z2-z1; ...
% x3-x1, y3-y1 ,z3-z1; ...
% x4-x1, y4-y1 ,z4-z1; ...
% ];
%
%
% b = [...
% r1^2-r2^2-(x1^2+y1^2+z1^2) + (x2^2+y2^2+z2^2);...
% r1^2-r3^2-(x1^2+y1^2+z1^2) + (x3^2+y3^2+z3^2);...
% r1^2-r4^2-(x1^2+y1^2+z1^2) + (x4^2+y4^2+z4^2);...
% ];
A = [...
x2-x1, y2-y1; ...
x3-x1, y3-y1; ...
x4-x1, y4-y1; ...
];
b = 0.5*[...
r1^2-r2^2-(x1^2+y1^2+z1^2) + (x2^2+y2^2+z2^2) - 2*zk*(z2 - z1);...
r1^2-r3^2-(x1^2+y1^2+z1^2) + (x3^2+y3^2+z3^2) - 2*zk*(z3 - z1);...
r1^2-r4^2-(x1^2+y1^2+z1^2) + (x4^2+y4^2+z4^2)- 2*zk*(z4 - z1);...
];
estimated_position = pinv(A)*b;
estimated_position = [estimated_position; zk];
%[estimated_position, ~] = lsqnonlin(@(x)sphere_intersect(x, c1, r1, c2, r2, c3, r3, c4, r4, 0, 0, 4), x0, [], [], options);
elseif length(anchorPoints) == 3
A =[...
x2-x1, y2-y1; ...
x3-x1, y3-y1; ...
];
b = (1/2)*[...
r1^2-r2^2-(x1^2+y1^2+z1^2) + (x2^2+y2^2+z2^2) - 2*zk*(z2 - z1);...
r1^2-r3^2-(x1^2+y1^2+z1^2) + (x3^2+y3^2+z3^2) - 2*zk*(z3 - z1);...
];
estimated_position = pinv(A)*b;
estimated_position = [estimated_position; zk];
% A = 2* [...
% x2-x1, y2-y1; ...
% x3-x1, y3-y1; ...
%
% ];
%
% b = [...
% r1^2-r2^2-(x1^2+y1^2) + (x2^2+y2^2);...
% r1^2-r3^2-(x1^2+y1^2) + (x3^2+y3^2);...
% ];
%
% estimated_position = pinv(A)*b;
% estimated_position = [estimated_position; zk];
% [estimated_position, ~] = lsqnonlin(@(x)sphere_intersect(x, c1, r1, c2, r2, c3, r3, 0, 0, 0, 0, 3), x0, [], [], options);
else
estimated_position = NaN;
end
end