I am trying to solve a system of trigonometric equations in Java
, but I don't know where to start. I've used commons-math3
before to solve simple linear sets of equations, but this is above my head. Equations I am trying to solve:
a - e + b cosθ 1 + c sinθ 1 + d*sin(θ 2 +θ 1 )= z
( b sinθ 1 + c cosθ 1 + d*cos(θ 2 -θ 1 ) * sinθ 0 = x
( b sinθ 1 + c cosθ 1 + d*cos(θ 2 -θ 1 ) * sinθ 0 = y
, where a,b,c,d and e are constants. In practical terms, given x, y, and z, I need to solve for θ 0 , θ 1 , θ 2 .
You need to use the root-finding algorithm .
It is usually studied in calculus as the Newton's method or Newton Raphson method.
You will have to use a multi-dimensional secant method or Muller's method . Numerical recipes has something on it.
You can use the least-squares-in-java project for this. Here's the code that will solve your problem:
import org.junit.Assert;
import org.junit.Test;
import org.orangepalantir.leastsquares.Function;
public class NonLinearTrigonometricSolver {
// Solves the following non-linear set of equations:
// a - e + bcosθ1 + csinθ1 + d * sin(θ1 + θ2) ) = z
// ( bsinθ1 + ccosθ1 + d * cos(θ1 + θ2) ) * sinθ0 = x
// ( bsinθ1 + ccosθ1 + d * cos(θ1 + θ2) ) * cosθ0 = y
// given x, y, z, solve for θ0, θ1, θ2
static final double a = 125;
static final double b = 143;
static final double c = 50;
static final double d = 142;
static final double e = 96;
static final double x = 0;
static final double y = 192;
static final double z = 172;
@Test
public void testNonLinearTrigonometricSolver() {
double[][] xs = { { -1 }, { 0 }, { 1 } };
double[] zs = { z, x, y };
double r = Math.sqrt(x * x + y * y);
final double sinTheta0 = x / r;
final double cosTheta0 = y / r;
Function f = new Function() {
@Override
public double evaluate(double[] values, double[] parameters) {
double t1 = parameters[0];
double t2 = parameters[1];
if (values[0] == -1) {
return a - e + b * Math.sin(t1) + c * Math.cos(t1) + d * Math.cos(t2 + t1);
} else if (values[0] == 0) {
return (b * Math.sin(t1) + c * Math.cos(t1) + d * Math.cos(t2 + t1)) * sinTheta0;
} else {
return (b * Math.sin(t1) + c * Math.cos(t1) + d * Math.cos(t2 + t1)) * cosTheta0;
}
}
@Override
public int getNParameters() {
return 2;
}
@Override
public int getNInputs() {
return 1;
}
};
NonLinearSolver fit = new NonLinearSolver(f);
fit.setData(xs, zs);
double[] params = { 0, 0 };
fit.setParameters(params);
fit.fitData();
// improving results.
fit.setMinChange(1e-32);
fit.setMinError(1e-32);
fit.setStepSize(0.5);
fit.fitData();
double t1 = fit.getParameters()[0];
double t2 = fit.getParameters()[1];
double arg = y / (b * Math.sin(t1) + c * Math.cos(t1) + d * Math.cos(t2 + t1));
// System.out.println(" " + arg);
double theta0 = Math.acos(arg) * Math.signum(x);
System.out.println(Math.toDegrees(theta0));
System.out.println(Math.toDegrees(fit.getParameters()[0]));
System.out.println(Math.toDegrees(fit.getParameters()[1]));
Assert.assertEquals(0, Math.toDegrees(theta0), 1e-16);
Assert.assertEquals(0, Math.toDegrees(fit.getParameters()[0]), 1e-16);
Assert.assertEquals(0, Math.toDegrees(fit.getParameters()[1]), 1e-16);
}
}
The technical post webpages of this site follow the CC BY-SA 4.0 protocol. If you need to reprint, please indicate the site URL or the original address.Any question please contact:yoyou2525@163.com.