# Coding non-linear least squares (GPS iteration) with steps and solutions showing?

I am trying to code non-linear least squares approximation with its solutions shown in tables like far below, i assume there has to be a 5 by 4 Jacobian matrix (as i deal with 4 unknowns and 5 equations) and i know there is a built in least squares function in Mathematica

But i wonder how i could start this code, below i have shown relevant codes as a starting point but i struggle to code the transpose of the Jacobian matrix. This is for GPS satellite equations trying to iterate and find solutions for the location of the receiver, which is (1,6.2,1,0) for (x,y,z,g). If there are any unclear aspects please don't hesitate to question via comments Thank you so much

(*Jacobian matrix*) (*5*4 matrix*)
J[x_, y_, z_,
g_] = {{D[f1[x, y, z, g], x], D[f1[x, y, z, g], y],
D[f1[x, y, z, g], z],
D[f1[x, y, z, g], g]}, {D[f2[x, y, z, g], x],
D[f2[x, y, z, g], y], D[f2[x, y, z, g], z],
D[f2[x, y, z, g], g]}, {D[f3[x, y, z, g], x],
D[f3[x, y, z, g], y], D[f3[x, y, z, g], z],
D[f3[x, y, z, g], g]}, {D[f4[x, y, z, g], x],
D[f4[x, y, z, g], y], D[f4[x, y, z, g], z],
D[f4[x, y, z, g], g]}, {D[f5[x, y, z, g], x],
D[f5[x, y, z, g], y], D[f5[x, y, z, g], z],
D[f5[x, y, z, g], g]}};


the functions i want to iterate are here,

(*Constants*)
x1 = 1; x2 = -1; x3 = -3; x4 = 2;x5 = 4; y1 = 8.2; y2 = 8.2; y3 = 8.2; y4 = \
8.2; y5=8.2; z1 = 1; z2 = -3; z3 = 3; z4 = -2; z5 = -5;
d1 = Sqrt[4];d2 = Sqrt[24];d3 = Sqrt[24];d4 = Sqrt[14];d5 = Sqrt[47];
f1[x_, y_, z_, g_] =
Sqrt[(x - x1)^2 + (y - y1)^2 + (z - z1)^2] - 0.47 g - d1;
f2[x_, y_, z_, g_] =
Sqrt[(x - x2)^2 + (y - y2)^2 + (z - z2)^2] - 0.47 g - d2;
f3[x_, y_, z_, g_] =
Sqrt[(x - x3)^2 + (y - y3)^2 + (z - z3)^2] - 0.47 g - d3;
f4[x_, y_, z_, g_] =
Sqrt[(x - x4)^2 + (y - y4)^2 + (z - z4)^2] - 0.47 g - d4;
f5[x_, y_, z_, g_] =
Sqrt[(x - x5)^2 + (y - y5)^2 + (z - z5)^2] - 0.47 g - d5;


random guesses i want to utilise for (x,y,z,g)

(*Random first guess*)
rn = {3, 5, 3, 0.02};


example of iteration table (this was taken from the newton raphson method with 4 equations only)

Step 1 Solution = {0.0178276,5.76787,0.0304523,-2.32268}
Step 2 Solution = {0.855362,6.41068,0.940703,-0.792372}
Step 3 Solution = {0.993904,6.20146,0.997558,-0.0180172}
Step 4 Solution = {0.999996,6.19999,0.999998,-2.0167*10^-6}
Step 5 Solution = {1.,6.2,1.,-4.38272*10^-11}
Step 6 Solution = {1.,6.2,1.,-1.2335*10^-15}
Step 7 Solution = {1.,6.2,1.,1.33891*10^-15}


@HenrikSchumacher posted already but I'll show how to do a fairly minimal change to get the iteration to work. It amounts to using the pseudoinverse, and using all five functions.

I also changed them to use explicit lists for input, just to tighten the code a bit.

f1[{x_, y_, z_, g_}] =
Sqrt[(x - x1)^2 + (y - y1)^2 + (z - z1)^2] - 0.47 g - d1;
f2[{x_, y_, z_, g_}] =
Sqrt[(x - x2)^2 + (y - y2)^2 + (z - z2)^2] - 0.47 g - d2;
f3[{x_, y_, z_, g_}] =
Sqrt[(x - x3)^2 + (y - y3)^2 + (z - z3)^2] - 0.47 g - d3;
f4[{x_, y_, z_, g_}] =
Sqrt[(x - x4)^2 + (y - y4)^2 + (z - z4)^2] - 0.47 g - d4;
f5[{x_, y_, z_, g_}] =
Sqrt[(x - x5)^2 + (y - y5)^2 + (z - z5)^2] - 0.47 g - d5;

jac[{x_, y_, z_, g_}] :=
Grad[{f1[{x, y, z, g}], f2[{x, y, z, g}], f3[{x, y, z, g}],
f4[{x, y, z, g}], f5[{x, y, z, g}]}, {x, y, z, g}]

(*Random first guess*)
rn = {3, 5, 3, 0.02};
Do[(*Itterate*)
rn += -PseudoInverse[
jac[{x, y, z, g}] /. Thread[{x, y, z, g} -> rn]].{f1[rn],
f2[rn], f3[rn], f4[rn], f5[rn]};
Print["Step ", i, " Solution = ", rn];, {i, 1, 8}];

(* Step 1 Solution = {0.433037,5.46576,0.302518,-1.21689}

Step 2 Solution = {1.12809,5.88922,1.06164,0.244063}

Step 3 Solution = {1.13841,6.01264,1.06859,0.369835}

Step 4 Solution = {1.13727,6.01374,1.06771,0.369582}

Step 5 Solution = {1.13726,6.01375,1.0677,0.369555}

Step 6 Solution = {1.13726,6.01376,1.0677,0.369555}

Step 7 Solution = {1.13726,6.01376,1.0677,0.369555}

Step 8 Solution = {1.13726,6.01376,1.0677,0.369555} *)

• This works to the solution but you forgot to add the constants in as part of your code haha (no big deals) but this is awesome stuff im so grateful people like you can help me in this program otherwise i might had to do all by hand!! But thank you so much for your help @DanielLichtblau, same question why do you think Multivariate Newton's method worked better than the least squares method well gauss-newton method? Again thank you so much – Tomedy Apr 29 '18 at 18:19
• The least-squares iteration is working correctly. The 5x4 system is not consistent (there is no solution), and this method is giving result that minimizes discrepancy. – Daniel Lichtblau Apr 29 '18 at 18:29
• ah so because system has no solution it gives us results that approximate the best – Tomedy Apr 30 '18 at 3:03
• That's exactly what it does. – Daniel Lichtblau Apr 30 '18 at 13:55

I thought I would show a RANSAC approach to this overdetermined case. I moved the y coordinates of the satellites so they would not be collinear. I also adjusted the fifth distance so that there would be approximte consistency in the overdetermined system.

Clear[x, y, z]

x = {1, -1, -3, 2, 4};
y = {-33/10, 82/10, 44/10, 82/10, -61/10};
z = {1, -3, 3, -2, -5};

poly[j_] := (d[j] +
47/100*g)^2 - ((xx - x[[j]])^2 + (yy - y[[j]])^2 + (zz -
z[[j]])^2)
polys = Table[poly[j], {j, 5}];
dd = Array[d, 5];
dvalues = Sqrt[{4, 24, 24, 14, 20}];
vars = {xx, yy, zz, g};


We solve the first four equations, using the first four distances.

solnA =
Select[vars /.
vars], (Apply[And, Thread[Im[#] == 0]]) &]

(* Out[275]= {{-1.88852, 3.47759, -1.75037, -20.9874}, {3.22179,
1.04844, -1.58539, 7.50075}} *)


Reepeat, but using last four equations and distances.

solnB =
Select[vars /.
NSolve[Rest[polys] /. Thread[Rest[dd] -> Rest[dvalues]], vars],

(* Out[277]= {{-2.42803, -0.0426444, -4.15969, -28.3924}, {3.21237,
1.0654, -1.57779, 7.46281}} *)


Note that one solution from the first is close to one from the second.

Now do similar, but omitting the third equation and distance.

solnC =
Select[vars /.
NSolve[polys[[{1, 2, 4, 5}]] /.
Thread[dd[[{1, 2, 4, 5}]] -> dvalues[[{1, 2, 4, 5}]]], vars],

(* Out[274]= {{-0.850689, 1.66418, -11.8707, -33.8691}, {3.20663,
1.05984, -1.55364, 7.4755}} *)


Again one solution is an approximate match to the prior matching set.

The point is that if the known values (satellite positions, distances) are in "general position" then a unique solution should emerge. One can get a decent estimate by computing a few solution sets to exactly determined subsystems, finding the single solutions from each that are approximately equal, and averaging them. This average typically will give a really good starting point for an iterative nonlinear least squares solver of the type used in other answers.

I do not really understand what you are looking for. But I try to show how to set up Newton's algorithm in a short and concise way.

I join your functions f1, f2,... into one function F that eats a four-vector and spits out the five vector considting of f1, f2, ...

(*Constants*)
X0 = {1., -1., -3., 2., 4.};
Y0 = {8.2, 8.2, 8.2, 8.2, 8.2};
Z0 = {1., -3., 3., -2., -5.};
d = N[{Sqrt[4], Sqrt[24], Sqrt[24], Sqrt[14], Sqrt[47]}];

ClearAll[F, DF];
F[{x_, y_, z_, g_}] := Evaluate[(X0 - x)^2 + (Y0 - y)^2 + (Z0 - z)^2 - 0.47 g - d];
DF[{x_, y_, z_, g_}] := Evaluate[D[F[{x, y, z, g}], {{x, y, z, g}, 1}]];

initialguess = N@{3, 5, 3, 0.02};
newtonstep = X \[Function] X - LeastSquares[DF[X], F[X]];
NestList[newtonstep, initialguess, 40]


Note how we can get the Jacobian for a mupltivariate function F very conveniently with D. (Btw., the second derivative, should you ever need it, can be obtained with D[F[{x, y, z, g}], {{x, y, z, g}, 2}].) Evaluate ensures that all symbolic computations are performed only once.

Edit

From your comment I read off that you are looking for is the Gauss-Newton method.

Note that in our case here, the matrix $DF(x)^T DF(x)$ is not invertible which causes severe trouble if one tries to use the following:

gaussnewtonstep = X \[Function] X - LinearSolve[Transpose[DF[X]].DF[X], Transpose[DF[X]].F[X]];
NestList[gaussnewtonstep, initialguess, 40]


As you can easily see when you execute the code, this is a mess. But in cases where $DF(x)^T DF(x))^{-1}$ is invertible, one has the identity

$$(DF(x)^T DF(x))^{-1} DF(x)^T = DF(x)^\dagger,$$

where $DF(x)^\dagger$ is the Moore-Penrose pseudoinverse of $DF(x)$.

So what you're looking for is precisely what Daniel Lichtblau posted:

gaussnewtonstep = X \[Function] X - PseudoInverse[DF[X]].F[X]];
NestList[gaussnewtonstep, initialguess, 40]


And thats exactly what newtonstep above does:

a = NestList[gaussnewtonstep, initialguess, 40];
b = NestList[newtonstep, initialguess, 40];
Max[Abs[a - b]]


1.80123*10^-12

The advantage is that this works even if $DF(x)^T DF(x)$ is singular. And even if $DF(x)^T DF(x)$ is invertible one should better not use LinearSolve[Transpose[DF[X]].DF[X], Transpose[DF[X]].F[X]] because it tends to have a bad condition number so that LinearSolve might only be able to produce "solutions" with high error.

Summarized: You've already got what you wanted (and even a bit more).

• Thank you but i was asking for non-linear least squares method which involves the transpose of a jacobian matrix, x =(J^T J)^(-1) J^T R(X) where X is the initial guess and x is something that should be added to X in order to make better estimation to the solution, J^T meaning transpose of pseudo-jacobian matrix of functions mentioned above, R(X) meaning residual matrix at X constructing 5*1 matrix in this case. This probably should be mentioned in the question.Sorry but thank you for your help please feel free to offer me any other revised answers thank you very much @HenrikSchumacher – Tomedy Apr 29 '18 at 17:50
• Also to add one more thing this relates to (A^T)(A)(Vector x)= (A^T)(Vector B), something i saw before. – Tomedy Apr 29 '18 at 17:51
• i think you are right, now i wonder why multivariate newton's method worked better in gaining the solution for the location with greater accuracy even with less equations? Thank you so much if i need help with coding at all is there anyways i can contact you at all privately? if there isn't then i guess what a bummer. You really helped me out big time in the last 2 days – Tomedy Apr 29 '18 at 18:16
• (1) If you can invert the (A^T)(A) then the thing on the rhs is the pseudoinverse. But the pseudoinverse is defined regardless, and one can use it in these iterations. (2) This answer is in fact a non-linear least squares method, done using iterations that require linear least squares. This is the adjustment required tomake Newton-Raphson work in the overdetermined setting. – Daniel Lichtblau Apr 29 '18 at 18:17
• commenting on your (2) is the adjustment made to the Newton-Raphson method in order to fit overdetermined setting the reason why the iterative answers aren't correct as Multivariate Newton's method with 4 equations? – Tomedy Apr 29 '18 at 18:22