Table of Contents

........................................................................................................................................ 1
Perfect observation and distances ........................................................................................... 1
a. Perturb observation with random normal error ...................................................................... 1
b. LSA (x4hat, y4hat) .......................................................................................................... 1
Statistical Tests ................................................................................................................... 2
Counting number of inliers ................................................................................................... 3

clc;
clear all;

Perfect observation and distances

x1 = 20; y1 = 150;
x2 = 10; y2 = 110;
x3 = 60; y3 = 100;
x4_perf = 100;
y4_perf = 170;

d1_perf = sqrt((x4_perf-x1)^2 + (y4_perf-y1)^2);

d2_perf = sqrt((x4_perf-x2)^2 + (y4_perf-y2)^2);
d3_perf = sqrt((x4_perf-x3)^2 + (y4_perf-y3)^2);

fprintf('\n\nPerfect Coordinates and Distances:\nx1

= %d; y1 = %d\nx2 = %d; y2 = %d\nx3 = %d; y3 =
%d\nx4 = %d; y4 = %d\n\nd1 = %f\nd2 = %f\nd3 = %f
\n',x1,y1,x2,y2,x3,y3,x4_perf,y4_perf,d1_perf,d2_perf,d3_perf);

a. Perturb observation with random normal er-

ror
rng(1994);
variance = 0.2;
err = randn(10000,3)*variance;
conf = 0.90;
chi_p2 = icdf('chi2',conf,2);

b. LSA (x4hat, y4hat)

n = 3; n0 = 2; r = 1;

x4_init = 102;
y4_init = 168;
fprintf('\n\nInitial Parameter Approximations:\nx4 = %d; y4 = %d
\n',x4_init,y4_init);

1
 % LSA for perturbed observations
for i = 1:10000
x4_0 = x4_init;
y4_0 = y4_init;
Delta = 1;

% Add error to distance observations

d1_obs = d1_perf + err(i,1);
d2_obs = d2_perf + err(i,2);
d3_obs = d3_perf + err(i,3);

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

D1 = sqrt((x4_0-x1)^2 + (y4_0-y1)^2);
D2 = sqrt((x4_0-x2)^2 + (y4_0-y2)^2);
D3 = sqrt((x4_0-x3)^2 + (y4_0-y3)^2);
F1 = d1_obs - D1;
F2 = d2_obs - D2;
F3 = d3_obs - D3;

% Partial derivatives of 3 condition equations w.r.t. x4 and

y4
dF1_dx4 = -(x4_0-x1)/D1;
dF1_dy4 = -(y4_0-y1)/D1;
dF2_dx4 = -(x4_0-x2)/D2;
dF2_dy4 = -(y4_0-y2)/D2;
dF3_dx4 = -(x4_0-x3)/D3;
dF3_dy4 = -(y4_0-y3)/D3;

B = [dF1_dx4 dF1_dy4;
dF2_dx4 dF2_dy4;
dF3_dx4 dF3_dy4];

W = eye(n);

f = -[F1; F2; F3];

Delta = inv(B'*W*B)*B'*W*f;
x4_0 = x4_0 + Delta(1);
y4_0 = y4_0 + Delta(2);
end

v_d1d2d3(i,:) = (f - B*Delta)';
x4_final(i,1) = x4_0;
y4_final(i,1) = y4_0;
end

Statistical Tests
Q_del = inv(B'*W*B);
sig_xy = variance^2*Q_del;
[V D] = eig(sig_xy);
a = sqrt(D(1,1)*chi_p2);
b = sqrt(D(2,2)*chi_p2);

2
 theta = atan2(V(1,2),V(1,1));
scatter(x4_final,y4_final,'.b');
hold on;
draw_ell(x4_perf, y4_perf, a, b, theta);

Counting number of inliers

n_inliers_perf = 0;
n_inliers_err = 0;
for i = 1:10000
w_perf = V*[x4_final(i)-x4_perf; y4_final(i)-y4_perf];
ellipse_val = w_perf(1)^2/a^2 + w_perf(2)^2/b^2;
if(ellipse_val < 1)
n_inliers_perf = n_inliers_perf + 1;
end
w_err = V*[x4_perf-x4_final(i); y4_perf-y4_final(i)];
ellipse_val = w_err(1)^2/a^2 + w_err(2)^2/b^2;
if(ellipse_val < 1)
n_inliers_err = n_inliers_err + 1;
end
end

fprintf('\n\n\nComputed parameters (first 10 out of 10,000):\n-----|

x4|----------|y4|-----\n');
fprintf(' %10.6f %10.6f \n',x4_final(1:10),y4_final(1:10));

fprintf('\n\nNumber of inliers for ellipse centered at TRUE value =

%d',n_inliers_perf);
fprintf('\nNumber of ellipse (centered at COMPUTED value) having true
value as inlier = %d\n',n_inliers_err);

Perfect Coordinates and Distances:

x1 = 20; y1 = 150
x2 = 10; y2 = 110
x3 = 60; y3 = 100
x4 = 100; y4 = 170

d1 = 82.462113
d2 = 108.166538
d3 = 80.622577

Initial Parameter Approximations:

x4 = 102; y4 = 168

Computed parameters (first 10 out of 10,000):

-----|x4|----------|y4|-----
99.820148 99.597083
100.175961 99.704694

3
 100.160016 100.203680
100.473112 99.889821
100.187451 99.776051
170.444772 170.510378
169.989655 170.277318
169.919833 169.670539
169.541650 170.354047
170.238683 169.778931

Number of inliers for ellipse centered at TRUE value = 9023

Number of ellipse (centered at COMPUTED value) having true value as
inlier = 9023

Published with MATLAB R2015a