Numerical Method code for non linear functions

Question

Respected all, I am trying to find solution of 3 non linear equations using Newton Raphson Method in Matlab.Below is my code and I want this code to converge to a point (4,3,2).

Can some one please see my code.Thanks.

syms x y z
clc
xi=5;
yi=0;
zi=0;

xj=-5;
yj=0;
zj=0;

xk=0;
yk=0;
zk=5;

rij=-6.0537;
rik=3.8563;
rkj=2.1893;

k1=(xi-x)^2+(yi-y)^2+(zi-z)^2+(x-xj)^2+(yj-y)^2+(zj-z)^2-rij^2-2*((((x-xi)^2+(yi-y)^2+(zi-z)^2))*(x-xj)^2+(yj-y)^2+(zj-z)^2)^(1/2);

k2=(xi-x)^2+(yi-y)^2+(zi-z)^2+(xk-x)^2+(yk-y)^2+(zk-z)^2-rik^2-2*((((x-xi)^2+(yi-y)^2+(zi-z)^2))*(xk-x)^2+(yk-y)^2+(zk-z)^2)^(1/2);

k3=(xk-x)^2+(yk-y)^2+(zk-z)^2+(xj-x)^2+(yj-y)^2+(zj-z)^2-rkj^2-2*((((x-xk)^2+(yk-y)^2+(zk-z)^2))*(xj-x)^2+(yj-y)^2+(zj-z)^2)^(1/2);

y1=diff(k1,x);
y2=diff(k1,y);
y3=diff(k1,z);
y4=diff(k2,x);
y5=diff(k2,y);
y6=diff(k2,z);
y7=diff(k3,x);
y8=diff(k3,y);
y9=diff(k3,z);

%% x=4 y=3 z=2
r1=3.5;
r2=2.5;
r3=1.9;

x=r1;
y=r2;
z=r3;
i=1;
x_s=r1;
y_s=r2;
z_s=r3;

while i<=200 

k0=[x y z]';%% Initial guess

rij=9.6953;
rik=3.8563;
rkj=2.1893;

k1=(xi-x)^2+(yi-y)^2+(zi-z)^2+(xj-x)^2+(yj-y)^2+(zj-z)^2-(rij^2)-2*((((xi-x)^2+(yi-y)^2+(zi-z)^2))*(xj-x)^2+(yj-y)^2+(zj-z)^2)^(1/2);

k2=(xi-x)^2+(yi-y)^2+(zi-z)^2+(xk-x)^2+(yk-y)^2+(zk-z)^2-(rik^2)-2*((((xi-x)^2+(yi-y)^2+(zi-z)^2))*(xk-x)^2+(yk-y)^2+(zk-z)^2)^(1/2);

k3=(xk-x)^2+(yk-y)^2+(zk-z)^2+(xj-x)^2+(yj-y)^2+(zj-z)^2-(rkj^2)-2*((((xk-x)^2+(yk-y)^2+(zk-z)^2))*(xj-x)^2+(yj-y)^2+(zj-z)^2)^(1/2);

k=[k1 k2 k3]';


j=[y1 y2 y3;y4 y5 y6;y7 y8 y9];

syms x y z
A3 = subs(j, {x, y, z}, {x_s, y_s, z_s});

inverse_j=inv(A3);

l= k0-(inverse_j)*k;

x=l(1);
y=l(2);
z=l(3);
i=i+1;
k0=[x y z]';
x_s=l(1);
y_s=l(2);
z_s=l(3);

k0

end

Answer 1

The original system of equations you're trying to solve is $$ (\mathbf r_i - \mathbf r)^2 + (\mathbf r_j - \mathbf r)^2 - 2 \sqrt{(\mathbf r_i - \mathbf r)^2 (\mathbf r_j - \mathbf r)^2} = r_{ij}^2\\ (\mathbf r_i - \mathbf r)^2 + (\mathbf r_k - \mathbf r)^2 - 2 \sqrt{(\mathbf r_i - \mathbf r)^2 (\mathbf r_k - \mathbf r)^2} = r_{ik}^2\\ (\mathbf r_k - \mathbf r)^2 + (\mathbf r_j - \mathbf r)^2 - 2 \sqrt{(\mathbf r_k - \mathbf r)^2 (\mathbf r_j - \mathbf r)^2} = r_{kj}^2 $$ which is simply squares of $$ |\mathbf r - \mathbf r_i| - |\mathbf r - \mathbf r_j| = r_{ij}\\ |\mathbf r - \mathbf r_i| - |\mathbf r - \mathbf r_k| = r_{ik}\\ |\mathbf r - \mathbf r_k| - |\mathbf r - \mathbf r_j| = r_{kj}. $$ It is easy to see that this problem has solutions only if $r_{ij} = r_{ik} + r_{kj}$, since the last two equations sum to the first one. Otherwise, there's no solutions. Moreover, when $r_{ij} = r_{ik} + r_{kj}$ there are infinitely many solutions.

To break the linear dependence one may add one more equation and remove the redundant one $$ |\mathbf r - \mathbf r_i| - |\mathbf r - \mathbf r_j| = r_{ij}\\ |\mathbf r - \mathbf r_i| - |\mathbf r - \mathbf r_k| = r_{ik}\\ |\mathbf r - \mathbf r_i| - |\mathbf r - \mathbf r_l| = r_{il}. $$

Denoting $\mathbf r_0 = \mathbf r_i, \mathbf r_1 = \mathbf r_j, \mathbf r_2 = \mathbf r_k, \mathbf r_3 = \mathbf r_l$, $$ F_i = |\mathbf r - \mathbf r_0| - |\mathbf r - \mathbf r_i| - r_{0i} $$ The derivative $\frac{\partial \mathbf F}{\partial \mathbf r}$ can be easily computed by hand: $$ \frac{\partial F_i}{\partial \mathbf r} = \frac{\mathbf r - \mathbf r_0}{|\mathbf r - \mathbf r_0|} - \frac{\mathbf r - \mathbf r_i}{|\mathbf r - \mathbf r_i|}. $$ Now the Newton's method looks like $$ \mathbf r^{(s+1)} = \mathbf r^{(s)} - \begin{pmatrix} \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_0^\top}{|\mathbf r - \mathbf r_0|} - \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_1^\top}{|\mathbf r - \mathbf r_1|}\\ \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_0^\top}{|\mathbf r - \mathbf r_0|} - \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_2^\top}{|\mathbf r - \mathbf r_2|}\\ \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_0^\top}{|\mathbf r - \mathbf r_0|} - \frac{\mathbf (\mathbf r^{(s)})^\top - \mathbf r_3^\top}{|\mathbf r - \mathbf r_3|} \end{pmatrix}^{-1} \begin{pmatrix} |\mathbf r^{(s)} - \mathbf r_0| - |\mathbf r^{(s)} - \mathbf r_1| - r_{01}\\ |\mathbf r^{(s)} - \mathbf r_0| - |\mathbf r^{(s)} - \mathbf r_2| - r_{02}\\ |\mathbf r^{(s)} - \mathbf r_0| - |\mathbf r^{(s)} - \mathbf r_3| - r_{03} \end{pmatrix} $$

I think the initial approximation $$ \mathbf r^{(0)} = \frac{\mathbf r_0 + \mathbf r_1 + \mathbf r_2 + \mathbf r_3}{4} $$ may work just well.

Here's a Matlab implementaion

r0 = [5; 0; 0];
r1 = [-5; 0; 0];
r2 = [0; 0; 5];
r3 = [0; 5; 0];

r01 = -3;
r02 = 3;
r03 = -2;

rs = 0.25 * (r0 + r1 + r2 + r3);

for it = 1:20
    F = [ norm(rs - r0) - norm(rs - r1) - r01; ...
          norm(rs - r0) - norm(rs - r2) - r02; ...
          norm(rs - r0) - norm(rs - r3) - r03 ];
    dFdr = [ (rs' - r0') / norm(rs - r0) - (rs' - r1') / norm(rs - r1); ...
             (rs' - r0') / norm(rs - r0) - (rs' - r2') / norm(rs - r2); ...
             (rs' - r0') / norm(rs - r0) - (rs' - r3') / norm(rs - r3) ];
    drs = dFdr \ F;
    rs = rs - drs;
    fprintf('||F|| = %e, ||drs|| = %e\n', norm(F), norm(drs));
    if norm(drs) < 1e-14
        break;
    end
end

rs