Newton-Raphson Iteration Solver in 2 and 3 Dimensions
by Matthew Jessick · 05/22/2008 (7:14 am) · 2 comments
Download Code File
Example
As an example of how to use the Newton-Raphson solver, the simple example test driver provided solves a simple trajectory problem: how to aim a computer controlled catapult with a 2 dimensional trajectory (horizontal and vertical). The catapult projectile is acted on by constant gravity and air drag that is linear with velocity.
(While a more true-to-the-real-world model of air drag would be quadratic in velocity, this would complicate the problem to the point where we can't use the simple analytic solution to the trajectory that we will use here for the linear drag case. Note that the Newton-Raphson iteration code here CAN be used to solve the more complicated quadratic drag case also - the iterating function must be upgraded to perform numerical integration of the trajectory since an analytic solution is not available.)
The example problem could be extended to three dimensions (aiming with elevation and windage angles), and you could also add wind or even constant rocket thrust to the simple dynamics pretty easily.
References
The download includes Open Office and PDF files describing the algorithm in mathematical equations.
"Numerical Recipes in C: The Art of Scientific Computing," Chapter 9 "Root Finding and Nonlinear Sets of Equations" (Section 9.6 "Newton-Raphson Method for Nonlinear Systems of Equations"), Cambridge University Press, c.1992, ISBN 0-521-43108-5
Find a version online at: www.nr.com/oldverswitcher.html
Wikipedia: "Newton-Raphson" en.wikipedia.org/wiki/Newton%27s_method
There are many online demonstrations of one dimensional Newton's Method solvers in operation.
Newton-Raphson in multiple dimensions
{X} = {control variables vector}^T
{F} = {constraint values vector}^T
Jacobian matrix J (partial derivatives of the
constraints with respect to the control variables)
The Jacobian matrix elements are:
Jij = dFi/dXj
To calculate the Jacobian without derivative information, use finite difference: Jij = dFi/dXj = (dFi(X+dXj) - dFi(X)) / dXj
whre dXj is a "small" step in Xj, and dFi is a constraint error
Note that you only need to calculate dF(X) once, so for a 2x2 problem,
you need to generate one nominal trajectory (results in F0(X) and F1(X)) and two perturbed trajectories: F0(X0+dX0, X1) and F1(X0, X1 + dX1)
step calculation
{dX} = - [J]^(-1) {dF} new iteration step vector
To calculate the step you just multiply the Jacobian matrix's inverse by the vector of current constraint errors. Again, this is similar to dividing the function value F by the F' derivative in one dimensional Newton's Method.
This requires the inverse of the Jacobian matrix. This is the multi-dimensional version of Newton's Method where:
the step = -F / F'
Luckily, for "non-singular" Jacobian matrices, you can calculate the inverse analytically (see code file VectorNR.cpp for the invert() methods for 2x2 and 3x3 matrices.) Singular cases are similar to where the F' becomes zero in Newton's Method.
Put another way, for a scalar 1x1 problem, the Jacobian is dF/dX, the inverse is dX/dF, so the new step to take is roughly dX = - (dX/dF) * dF
The attached zip file includes a PDF file with the algorithm described in matrices and slight mathematical language for the example problem.
Iteration Algorithm
1. Evaluate the constraint values (run the trajectory) for the current control values (calculate F(X))
2. Look at the constraint values. Are they all close enough to the target constraint values? If so, stop. The algorithm has "converged" to a solution and we are done. (The code calls this status CONVERGENCE_CONSTRAINTS)
3. If we are still too far away from the target, then perform a Newton-Raphson step. Once we calculate the step to take, dX, look at it. Has it become so small that it isn't even worth bothering with going on any more? If the step is so small that we could go on forever with not much progress, we might as well just stop. The code calls this state CONVERGENCE_CONTROLS. The solution amy or may not be useful. In many cases it will be close enough.
4. If the step isn't too small then take it. Update the control variables as X = X + dX
5. Go back to the top (Step #1) and run another nominal trajectory (calculate F(X)), unless we have tried too many times. If we have tried our maximum number of iterations already, then again we stop. The code calls this state CONVERGENCE_FAILED.
Potential iteration problems and solutions
If failure happens a lot for a particular problem, you might call the method with an alpha value smaller than the default 1.0. This means that when you take a step, you take X = X + dX * alpha (that is, a smaller step than originally calculated). This can keep you out of trouble in some cases where the Jacobian is near singular and a single bad step at the beginning sends you far out into left field where it then gets truely lost.
Another trick in difficult cases is to start with a better initial guess for X. (I used to make my living calculating these better initial guesses. ;) )
Finally, sometimes the function F(X) is shaped such that it just isn't possible to solve the problem using this method. See Numerical Recipes reference for examples. For trajectory problems (which are generally relatively simple as these things go), this usually means you have made some awful mistake in how you set the problem up. Perhaps you haven't given the correct control variables to make it possible to reduce all the constraint errors to zero. For a catapult example, perhaps you didn't realize how important it is to allow the algorithm to change both the time-of-flight and the elevation angle and so you used the one dimensional Newton's Method changing the angle only. If you look carefully at what is happening in this case, you might notice that the catapult stone's trajectory is stopping at the chosen final time far over the target ground or under it. This would be a "Clue." ;)
If you suspect something like this is happening and thoughtful analysis fails, you might try a bunch of random solutions. If none of them is coming close, you might be able to figure out why by the patterns you see.
Another common mistake is some problem in calculating the end point of the trajectory or the constraint values from the end point. This kind of thing usually happens the first time you shoot a catapult across the international date line. Really.
One case where you will get CONVERGENCE_FAILED no matter what you try is when you aim the catapult at something farther away than your maximum range. In this case, shoot at some other target! ;)
Example
As an example of how to use the Newton-Raphson solver, the simple example test driver provided solves a simple trajectory problem: how to aim a computer controlled catapult with a 2 dimensional trajectory (horizontal and vertical). The catapult projectile is acted on by constant gravity and air drag that is linear with velocity.
(While a more true-to-the-real-world model of air drag would be quadratic in velocity, this would complicate the problem to the point where we can't use the simple analytic solution to the trajectory that we will use here for the linear drag case. Note that the Newton-Raphson iteration code here CAN be used to solve the more complicated quadratic drag case also - the iterating function must be upgraded to perform numerical integration of the trajectory since an analytic solution is not available.)
The example problem could be extended to three dimensions (aiming with elevation and windage angles), and you could also add wind or even constant rocket thrust to the simple dynamics pretty easily.
References
The download includes Open Office and PDF files describing the algorithm in mathematical equations.
"Numerical Recipes in C: The Art of Scientific Computing," Chapter 9 "Root Finding and Nonlinear Sets of Equations" (Section 9.6 "Newton-Raphson Method for Nonlinear Systems of Equations"), Cambridge University Press, c.1992, ISBN 0-521-43108-5
Find a version online at: www.nr.com/oldverswitcher.html
Wikipedia: "Newton-Raphson" en.wikipedia.org/wiki/Newton%27s_method
There are many online demonstrations of one dimensional Newton's Method solvers in operation.
Newton-Raphson in multiple dimensions
{X} = {control variables vector}^T
{F} = {constraint values vector}^T
Jacobian matrix J (partial derivatives of the
constraints with respect to the control variables)
The Jacobian matrix elements are:
Jij = dFi/dXj
To calculate the Jacobian without derivative information, use finite difference: Jij = dFi/dXj = (dFi(X+dXj) - dFi(X)) / dXj
whre dXj is a "small" step in Xj, and dFi is a constraint error
Note that you only need to calculate dF(X) once, so for a 2x2 problem,
you need to generate one nominal trajectory (results in F0(X) and F1(X)) and two perturbed trajectories: F0(X0+dX0, X1) and F1(X0, X1 + dX1)
step calculation
{dX} = - [J]^(-1) {dF} new iteration step vector
To calculate the step you just multiply the Jacobian matrix's inverse by the vector of current constraint errors. Again, this is similar to dividing the function value F by the F' derivative in one dimensional Newton's Method.
This requires the inverse of the Jacobian matrix. This is the multi-dimensional version of Newton's Method where:
the step = -F / F'
Luckily, for "non-singular" Jacobian matrices, you can calculate the inverse analytically (see code file VectorNR.cpp for the invert() methods for 2x2 and 3x3 matrices.) Singular cases are similar to where the F' becomes zero in Newton's Method.
Put another way, for a scalar 1x1 problem, the Jacobian is dF/dX, the inverse is dX/dF, so the new step to take is roughly dX = - (dX/dF) * dF
The attached zip file includes a PDF file with the algorithm described in matrices and slight mathematical language for the example problem.
Iteration Algorithm
1. Evaluate the constraint values (run the trajectory) for the current control values (calculate F(X))
2. Look at the constraint values. Are they all close enough to the target constraint values? If so, stop. The algorithm has "converged" to a solution and we are done. (The code calls this status CONVERGENCE_CONSTRAINTS)
3. If we are still too far away from the target, then perform a Newton-Raphson step. Once we calculate the step to take, dX, look at it. Has it become so small that it isn't even worth bothering with going on any more? If the step is so small that we could go on forever with not much progress, we might as well just stop. The code calls this state CONVERGENCE_CONTROLS. The solution amy or may not be useful. In many cases it will be close enough.
4. If the step isn't too small then take it. Update the control variables as X = X + dX
5. Go back to the top (Step #1) and run another nominal trajectory (calculate F(X)), unless we have tried too many times. If we have tried our maximum number of iterations already, then again we stop. The code calls this state CONVERGENCE_FAILED.
Potential iteration problems and solutions
If failure happens a lot for a particular problem, you might call the method with an alpha value smaller than the default 1.0. This means that when you take a step, you take X = X + dX * alpha (that is, a smaller step than originally calculated). This can keep you out of trouble in some cases where the Jacobian is near singular and a single bad step at the beginning sends you far out into left field where it then gets truely lost.
Another trick in difficult cases is to start with a better initial guess for X. (I used to make my living calculating these better initial guesses. ;) )
Finally, sometimes the function F(X) is shaped such that it just isn't possible to solve the problem using this method. See Numerical Recipes reference for examples. For trajectory problems (which are generally relatively simple as these things go), this usually means you have made some awful mistake in how you set the problem up. Perhaps you haven't given the correct control variables to make it possible to reduce all the constraint errors to zero. For a catapult example, perhaps you didn't realize how important it is to allow the algorithm to change both the time-of-flight and the elevation angle and so you used the one dimensional Newton's Method changing the angle only. If you look carefully at what is happening in this case, you might notice that the catapult stone's trajectory is stopping at the chosen final time far over the target ground or under it. This would be a "Clue." ;)
If you suspect something like this is happening and thoughtful analysis fails, you might try a bunch of random solutions. If none of them is coming close, you might be able to figure out why by the patterns you see.
Another common mistake is some problem in calculating the end point of the trajectory or the constraint values from the end point. This kind of thing usually happens the first time you shoot a catapult across the international date line. Really.
One case where you will get CONVERGENCE_FAILED no matter what you try is when you aim the catapult at something farther away than your maximum range. In this case, shoot at some other target! ;)
Torque Owner Ronald J Nelson
Code Hammer Games