List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Vector3D getNormSq
public double getNormSq()
From source file:org.orekit.orbits.CartesianOrbit.java
/** {@inheritDoc} */ public void addKeplerContribution(final PositionAngle type, final double gm, final double[] pDot) { final PVCoordinates pv = getPVCoordinates(); // position derivative is velocity final Vector3D velocity = pv.getVelocity(); pDot[0] += velocity.getX();//from w w w . j a va 2s.c o m pDot[1] += velocity.getY(); pDot[2] += velocity.getZ(); // velocity derivative is Newtonian acceleration final Vector3D position = pv.getPosition(); final double r2 = position.getNormSq(); final double coeff = -gm / (r2 * FastMath.sqrt(r2)); pDot[3] += coeff * position.getX(); pDot[4] += coeff * position.getY(); pDot[5] += coeff * position.getZ(); }
From source file:org.orekit.orbits.CircularOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the {@link PVCoordinates} in inertial frame * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if frame is not a {@link * Frame#isPseudoInertial pseudo-inertial frame} */// w ww .j av a 2 s . co m public CircularOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute semi-major axis final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; if (rV2OnMu > 2) { throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName()); } a = r / (2 - rV2OnMu); // compute inclination final Vector3D momentum = pvCoordinates.getMomentum(); i = Vector3D.angle(momentum, Vector3D.PLUS_K); // compute right ascension of ascending node final Vector3D node = Vector3D.crossProduct(Vector3D.PLUS_K, momentum); raan = FastMath.atan2(node.getY(), node.getX()); // 2D-coordinates in the canonical frame final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double xP = pvP.getX(); final double yP = pvP.getY(); final double zP = pvP.getZ(); final double x2 = (xP * cosRaan + yP * sinRaan) / a; final double y2 = ((yP * cosRaan - xP * sinRaan) * cosI + zP * sinI) / a; // compute eccentricity vector final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; final double f = eCE - e2; final double g = FastMath.sqrt(1 - e2) * eSE; final double aOnR = a / r; final double a2OnR2 = aOnR * aOnR; ex = a2OnR2 * (f * x2 + g * y2); ey = a2OnR2 * (f * y2 - g * x2); // compute latitude argument final double beta = 1 / (1 + FastMath.sqrt(1 - ex * ex - ey * ey)); alphaV = eccentricToTrue(FastMath.atan2(y2 + ey + eSE * beta * ex, x2 + ex - eSE * beta * ey)); serializePV = true; }
From source file:org.orekit.orbits.CircularOrbit.java
/** {@inheritDoc} */ protected TimeStampedPVCoordinates initPVCoordinates() { // get equinoctial parameters final double equEx = getEquinoctialEx(); final double equEy = getEquinoctialEy(); final double hx = getHx(); final double hy = getHy(); final double lE = getLE(); // inclination-related intermediate parameters final double hx2 = hx * hx; final double hy2 = hy * hy; final double factH = 1. / (1 + hx2 + hy2); // reference axes defining the orbital plane final double ux = (1 + hx2 - hy2) * factH; final double uy = 2 * hx * hy * factH; final double uz = -2 * hy * factH; final double vx = uy; final double vy = (1 - hx2 + hy2) * factH; final double vz = 2 * hx * factH; // eccentricity-related intermediate parameters final double exey = equEx * equEy; final double ex2 = equEx * equEx; final double ey2 = equEy * equEy; final double e2 = ex2 + ey2; final double eta = 1 + FastMath.sqrt(1 - e2); final double beta = 1. / eta; // eccentric latitude argument final double cLe = FastMath.cos(lE); final double sLe = FastMath.sin(lE); final double exCeyS = equEx * cLe + equEy * sLe; // coordinates of position and velocity in the orbital plane final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - equEx); final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - equEy); final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS); final double xdot = factor * (-sLe + beta * equEy * exCeyS); final double ydot = factor * (cLe - beta * equEx * exCeyS); final Vector3D position = new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz); final double r2 = position.getNormSq(); final Vector3D velocity = new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz); final Vector3D acceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), position); return new TimeStampedPVCoordinates(getDate(), position, velocity, acceleration); }
From source file:org.orekit.orbits.CircularOrbit.java
/** {@inheritDoc} */ protected double[][] computeJacobianMeanWrtCartesian() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final PVCoordinates pvc = getPVCoordinates(); final Vector3D position = pvc.getPosition(); final Vector3D velocity = pvc.getVelocity(); final double x = position.getX(); final double y = position.getY(); final double z = position.getZ(); final double vx = velocity.getX(); final double vy = velocity.getY(); final double vz = velocity.getZ(); final double pv = Vector3D.dotProduct(position, velocity); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double v2 = velocity.getNormSq(); final double mu = getMu(); final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a); final double rOa = r / a; final double aOr = a / r; final double aOr2 = a / r2; final double a2 = a * a; final double ex2 = ex * ex; final double ey2 = ey * ey; final double e2 = ex2 + ey2; final double epsilon = FastMath.sqrt(1 - e2); final double beta = 1 / (1 + epsilon); final double eCosE = 1 - rOa; final double eSinE = pv * oOsqrtMuA; final double cosI = FastMath.cos(i); final double sinI = FastMath.sin(i); final double cosRaan = FastMath.cos(raan); final double sinRaan = FastMath.sin(raan); // da//from w w w.j a v a 2s. com fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0); fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3); // differentials of the normalized momentum final Vector3D danP = new Vector3D(v2, position, -pv, velocity); final Vector3D danV = new Vector3D(r2, velocity, -pv, position); final double recip = 1 / pvc.getMomentum().getNorm(); final double recip2 = recip * recip; final Vector3D dwXP = new Vector3D(recip, new Vector3D(0, vz, -vy), -recip2 * sinRaan * sinI, danP); final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP); final Vector3D dwZP = new Vector3D(recip, new Vector3D(vy, -vx, 0), -recip2 * cosI, danP); final Vector3D dwXV = new Vector3D(recip, new Vector3D(0, -z, y), -recip2 * sinRaan * sinI, danV); final Vector3D dwYV = new Vector3D(recip, new Vector3D(z, 0, -x), recip2 * cosRaan * sinI, danV); final Vector3D dwZV = new Vector3D(recip, new Vector3D(-y, x, 0), -recip2 * cosI, danV); // di fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0); fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3); // dRaan fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0); fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3); // orbital frame: (p, q, w) p along ascending node, w along momentum // the coordinates of the spacecraft in this frame are: (u, v, 0) final double u = x * cosRaan + y * sinRaan; final double cv = -x * sinRaan + y * cosRaan; final double v = cv * cosI + z * sinI; // du final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP, cv * sinRaan / sinI, dwYP, 1, new Vector3D(cosRaan, sinRaan, 0)); final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV, cv * sinRaan / sinI, dwYV); // dv final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP, -u * sinRaan * cosI / sinI - cosRaan * z, dwYP, cv, dwZP, 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI)); final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV, -u * sinRaan * cosI / sinI - cosRaan * z, dwYV, cv, dwZV); final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position, -2 * aOr2 * eSinE * oOsqrtMuA, velocity); final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position, 2 / mu, velocity); final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position, aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity); final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position, eSinE / (mu * epsilon), velocity); final double cof1 = aOr2 * (eCosE - e2); final double cof2 = aOr2 * epsilon * eSinE; final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP); final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV); final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP); final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV); fillHalfRow(1, dexP, jacobian[1], 0); fillHalfRow(1, dexV, jacobian[1], 3); fillHalfRow(1, deyP, jacobian[2], 0); fillHalfRow(1, deyV, jacobian[2], 3); final double cle = u / a + ex - eSinE * beta * ey; final double sle = v / a + ey + eSinE * beta * ex; final double m1 = beta * eCosE; final double m2 = 1 - m1 * eCosE; final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey); final double m4 = -sle + cle * eSinE * beta; final double m5 = cle + sle * eSinE * beta; fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position, (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity, m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP, jacobian[5], 0); fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position, (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity, m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.CircularParametersTest.java
private void fillColumn(PositionAngle type, int i, CircularOrbit orbit, double hP, double[][] jacobian) { // at constant energy (i.e. constant semi major axis), we have dV = -mu dP / (V * r^2) // we use this to compute a velocity step size from the position step size Vector3D p = orbit.getPVCoordinates().getPosition(); Vector3D v = orbit.getPVCoordinates().getVelocity(); double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq()); double h;// w w w.ja v a2s . c o m Vector3D dP = Vector3D.ZERO; Vector3D dV = Vector3D.ZERO; switch (i) { case 0: h = hP; dP = new Vector3D(hP, 0, 0); break; case 1: h = hP; dP = new Vector3D(0, hP, 0); break; case 2: h = hP; dP = new Vector3D(0, 0, hP); break; case 3: h = hV; dV = new Vector3D(hV, 0, 0); break; case 4: h = hV; dV = new Vector3D(0, hV, 0); break; default: h = hV; dV = new Vector3D(0, 0, hV); break; } CircularOrbit oM4h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM3h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM2h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oM1h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP1h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP2h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP3h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); CircularOrbit oP4h = new CircularOrbit( new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) + 32 * (oP3h.getA() - oM3h.getA()) - 168 * (oP2h.getA() - oM2h.getA()) + 672 * (oP1h.getA() - oM1h.getA())) / (840 * h); jacobian[1][i] = (-3 * (oP4h.getCircularEx() - oM4h.getCircularEx()) + 32 * (oP3h.getCircularEx() - oM3h.getCircularEx()) - 168 * (oP2h.getCircularEx() - oM2h.getCircularEx()) + 672 * (oP1h.getCircularEx() - oM1h.getCircularEx())) / (840 * h); jacobian[2][i] = (-3 * (oP4h.getCircularEy() - oM4h.getCircularEy()) + 32 * (oP3h.getCircularEy() - oM3h.getCircularEy()) - 168 * (oP2h.getCircularEy() - oM2h.getCircularEy()) + 672 * (oP1h.getCircularEy() - oM1h.getCircularEy())) / (840 * h); jacobian[3][i] = (-3 * (oP4h.getI() - oM4h.getI()) + 32 * (oP3h.getI() - oM3h.getI()) - 168 * (oP2h.getI() - oM2h.getI()) + 672 * (oP1h.getI() - oM1h.getI())) / (840 * h); jacobian[4][i] = (-3 * (oP4h.getRightAscensionOfAscendingNode() - oM4h.getRightAscensionOfAscendingNode()) + 32 * (oP3h.getRightAscensionOfAscendingNode() - oM3h.getRightAscensionOfAscendingNode()) - 168 * (oP2h.getRightAscensionOfAscendingNode() - oM2h.getRightAscensionOfAscendingNode()) + 672 * (oP1h.getRightAscensionOfAscendingNode() - oM1h.getRightAscensionOfAscendingNode())) / (840 * h); jacobian[5][i] = (-3 * (oP4h.getAlpha(type) - oM4h.getAlpha(type)) + 32 * (oP3h.getAlpha(type) - oM3h.getAlpha(type)) - 168 * (oP2h.getAlpha(type) - oM2h.getAlpha(type)) + 672 * (oP1h.getAlpha(type) - oM1h.getAlpha(type))) / (840 * h); }
From source file:org.orekit.orbits.EquinoctialOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the position, velocity and acceleration * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if eccentricity is equal to 1 or larger or * if frame is not a {@link Frame#isPseudoInertial pseudo-inertial frame} *//* ww w . j a v a 2 s .c o m*/ public EquinoctialOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute semi-major axis final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; if (rV2OnMu > 2) { throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, getClass().getName()); } // compute inclination vector final Vector3D w = pvCoordinates.getMomentum().normalize(); final double d = 1.0 / (1 + w.getZ()); hx = -d * w.getY(); hy = d * w.getX(); // compute true longitude argument final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r; final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r; lv = FastMath.atan2(sLv, cLv); // compute semi-major axis a = r / (2 - rV2OnMu); // compute eccentricity vector final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a); final double eCE = rV2OnMu - 1; final double e2 = eCE * eCE + eSE * eSE; final double f = eCE - e2; final double g = FastMath.sqrt(1 - e2) * eSE; ex = a * (f * cLv + g * sLv) / r; ey = a * (f * sLv - g * cLv) / r; }
From source file:org.orekit.orbits.EquinoctialOrbit.java
/** {@inheritDoc} */ protected TimeStampedPVCoordinates initPVCoordinates() { // get equinoctial parameters final double lE = getLE(); // inclination-related intermediate parameters final double hx2 = hx * hx; final double hy2 = hy * hy; final double factH = 1. / (1 + hx2 + hy2); // reference axes defining the orbital plane final double ux = (1 + hx2 - hy2) * factH; final double uy = 2 * hx * hy * factH; final double uz = -2 * hy * factH; final double vx = uy; final double vy = (1 - hx2 + hy2) * factH; final double vz = 2 * hx * factH; // eccentricity-related intermediate parameters final double exey = ex * ey; final double ex2 = ex * ex; final double ey2 = ey * ey; final double e2 = ex2 + ey2; final double eta = 1 + FastMath.sqrt(1 - e2); final double beta = 1. / eta; // eccentric longitude argument final double cLe = FastMath.cos(lE); final double sLe = FastMath.sin(lE); final double exCeyS = ex * cLe + ey * sLe; // coordinates of position and velocity in the orbital plane final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex); final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey); final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS); final double xdot = factor * (-sLe + beta * ey * exCeyS); final double ydot = factor * (cLe - beta * ex * exCeyS); final Vector3D position = new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz); final double r2 = position.getNormSq(); final Vector3D velocity = new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz); final Vector3D acceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), position); return new TimeStampedPVCoordinates(getDate(), position, velocity, acceleration); }
From source file:org.orekit.orbits.EquinoctialOrbit.java
/** {@inheritDoc} */ protected double[][] computeJacobianMeanWrtCartesian() { final double[][] jacobian = new double[6][6]; // compute various intermediate parameters final Vector3D position = getPVCoordinates().getPosition(); final Vector3D velocity = getPVCoordinates().getVelocity(); final double r2 = position.getNormSq(); final double r = FastMath.sqrt(r2); final double r3 = r * r2; final double mu = getMu(); final double sqrtMuA = FastMath.sqrt(a * mu); final double a2 = a * a; final double e2 = ex * ex + ey * ey; final double oMe2 = 1 - e2; final double epsilon = FastMath.sqrt(oMe2); final double beta = 1 / (1 + epsilon); final double ratio = epsilon * beta; final double hx2 = hx * hx; final double hy2 = hy * hy; final double hxhy = hx * hy; // precomputing equinoctial frame unit vectors (f,g,w) final Vector3D f = new Vector3D(1 - hy2 + hx2, 2 * hxhy, -2 * hy).normalize(); final Vector3D g = new Vector3D(2 * hxhy, 1 + hy2 - hx2, 2 * hx).normalize(); final Vector3D w = Vector3D.crossProduct(position, velocity).normalize(); // coordinates of the spacecraft in the equinoctial frame final double x = Vector3D.dotProduct(position, f); final double y = Vector3D.dotProduct(position, g); final double xDot = Vector3D.dotProduct(velocity, f); final double yDot = Vector3D.dotProduct(velocity, g); // drDot / dEx = dXDot / dEx * f + dYDot / dEx * g final double c1 = a / (sqrtMuA * epsilon); final double c2 = a * sqrtMuA * beta / r3; final double c3 = sqrtMuA / (r3 * epsilon); final Vector3D drDotSdEx = new Vector3D(c1 * xDot * yDot - c2 * ey * x - c3 * x * y, f, -c1 * xDot * xDot - c2 * ey * y + c3 * x * x, g); // drDot / dEy = dXDot / dEy * f + dYDot / dEy * g final Vector3D drDotSdEy = new Vector3D(c1 * yDot * yDot + c2 * ex * x - c3 * y * y, f, -c1 * xDot * yDot + c2 * ex * y + c3 * x * y, g); // da// www .j av a2 s .co m final Vector3D vectorAR = new Vector3D(2 * a2 / r3, position); final Vector3D vectorARDot = new Vector3D(2 * a2 / mu, velocity); fillHalfRow(1, vectorAR, jacobian[0], 0); fillHalfRow(1, vectorARDot, jacobian[0], 3); // dEx final double d1 = -a * ratio / r3; final double d2 = (hy * xDot - hx * yDot) / (sqrtMuA * epsilon); final double d3 = (hx * y - hy * x) / sqrtMuA; final Vector3D vectorExRDot = new Vector3D((2 * x * yDot - xDot * y) / mu, g, -y * yDot / mu, f, -ey * d3 / epsilon, w); fillHalfRow(ex * d1, position, -ey * d2, w, epsilon / sqrtMuA, drDotSdEy, jacobian[1], 0); fillHalfRow(1, vectorExRDot, jacobian[1], 3); // dEy final Vector3D vectorEyRDot = new Vector3D((2 * xDot * y - x * yDot) / mu, f, -x * xDot / mu, g, ex * d3 / epsilon, w); fillHalfRow(ey * d1, position, ex * d2, w, -epsilon / sqrtMuA, drDotSdEx, jacobian[2], 0); fillHalfRow(1, vectorEyRDot, jacobian[2], 3); // dHx final double h = (1 + hx2 + hy2) / (2 * sqrtMuA * epsilon); fillHalfRow(-h * xDot, w, jacobian[3], 0); fillHalfRow(h * x, w, jacobian[3], 3); // dHy fillHalfRow(-h * yDot, w, jacobian[4], 0); fillHalfRow(h * y, w, jacobian[4], 3); // dLambdaM final double l = -ratio / sqrtMuA; fillHalfRow(-1 / sqrtMuA, velocity, d2, w, l * ex, drDotSdEx, l * ey, drDotSdEy, jacobian[5], 0); fillHalfRow(-2 / sqrtMuA, position, ex * beta, vectorEyRDot, -ey * beta, vectorExRDot, d3, w, jacobian[5], 3); return jacobian; }
From source file:org.orekit.orbits.EquinoctialParametersTest.java
private void fillColumn(PositionAngle type, int i, EquinoctialOrbit orbit, double hP, double[][] jacobian) { // at constant energy (i.e. constant semi major axis), we have dV = -mu dP / (V * r^2) // we use this to compute a velocity step size from the position step size Vector3D p = orbit.getPVCoordinates().getPosition(); Vector3D v = orbit.getPVCoordinates().getVelocity(); double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq()); double h;//from w ww . j a va2s . co m Vector3D dP = Vector3D.ZERO; Vector3D dV = Vector3D.ZERO; switch (i) { case 0: h = hP; dP = new Vector3D(hP, 0, 0); break; case 1: h = hP; dP = new Vector3D(0, hP, 0); break; case 2: h = hP; dP = new Vector3D(0, 0, hP); break; case 3: h = hV; dV = new Vector3D(hV, 0, 0); break; case 4: h = hV; dV = new Vector3D(0, hV, 0); break; default: h = hV; dV = new Vector3D(0, 0, hV); break; } EquinoctialOrbit oM4h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oM3h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oM2h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oM1h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oP1h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oP2h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oP3h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); EquinoctialOrbit oP4h = new EquinoctialOrbit( new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)), orbit.getFrame(), orbit.getDate(), orbit.getMu()); jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) + 32 * (oP3h.getA() - oM3h.getA()) - 168 * (oP2h.getA() - oM2h.getA()) + 672 * (oP1h.getA() - oM1h.getA())) / (840 * h); jacobian[1][i] = (-3 * (oP4h.getEquinoctialEx() - oM4h.getEquinoctialEx()) + 32 * (oP3h.getEquinoctialEx() - oM3h.getEquinoctialEx()) - 168 * (oP2h.getEquinoctialEx() - oM2h.getEquinoctialEx()) + 672 * (oP1h.getEquinoctialEx() - oM1h.getEquinoctialEx())) / (840 * h); jacobian[2][i] = (-3 * (oP4h.getEquinoctialEy() - oM4h.getEquinoctialEy()) + 32 * (oP3h.getEquinoctialEy() - oM3h.getEquinoctialEy()) - 168 * (oP2h.getEquinoctialEy() - oM2h.getEquinoctialEy()) + 672 * (oP1h.getEquinoctialEy() - oM1h.getEquinoctialEy())) / (840 * h); jacobian[3][i] = (-3 * (oP4h.getHx() - oM4h.getHx()) + 32 * (oP3h.getHx() - oM3h.getHx()) - 168 * (oP2h.getHx() - oM2h.getHx()) + 672 * (oP1h.getHx() - oM1h.getHx())) / (840 * h); jacobian[4][i] = (-3 * (oP4h.getHy() - oM4h.getHy()) + 32 * (oP3h.getHy() - oM3h.getHy()) - 168 * (oP2h.getHy() - oM2h.getHy()) + 672 * (oP1h.getHy() - oM1h.getHy())) / (840 * h); jacobian[5][i] = (-3 * (oP4h.getL(type) - oM4h.getL(type)) + 32 * (oP3h.getL(type) - oM3h.getL(type)) - 168 * (oP2h.getL(type) - oM2h.getL(type)) + 672 * (oP1h.getL(type) - oM1h.getL(type))) / (840 * h); }
From source file:org.orekit.orbits.KeplerianOrbit.java
/** Constructor from cartesian parameters. * * <p> The acceleration provided in {@code pvCoordinates} is accessible using * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods * use {@code mu} and the position to compute the acceleration, including * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}. * * @param pvCoordinates the PVCoordinates of the satellite * @param frame the frame in which are defined the {@link PVCoordinates} * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame}) * @param mu central attraction coefficient (m/s) * @exception IllegalArgumentException if frame is not a {@link * Frame#isPseudoInertial pseudo-inertial frame} *//*from w w w . j av a 2s . c om*/ public KeplerianOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu) throws IllegalArgumentException { super(pvCoordinates, frame, mu); // compute inclination final Vector3D momentum = pvCoordinates.getMomentum(); final double m2 = momentum.getNormSq(); i = Vector3D.angle(momentum, Vector3D.PLUS_K); // compute right ascension of ascending node raan = Vector3D.crossProduct(Vector3D.PLUS_K, momentum).getAlpha(); // preliminary computations for parameters depending on orbit shape (elliptic or hyperbolic) final Vector3D pvP = pvCoordinates.getPosition(); final Vector3D pvV = pvCoordinates.getVelocity(); final double r = pvP.getNorm(); final double V2 = pvV.getNormSq(); final double rV2OnMu = r * V2 / mu; // compute semi-major axis (will be negative for hyperbolic orbits) a = r / (2 - rV2OnMu); final double muA = mu * a; // compute true anomaly if (a > 0) { // elliptic or circular orbit final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(muA); final double eCE = rV2OnMu - 1; e = FastMath.sqrt(eSE * eSE + eCE * eCE); v = ellipticEccentricToTrue(FastMath.atan2(eSE, eCE)); } else { // hyperbolic orbit final double eSH = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA); final double eCH = rV2OnMu - 1; e = FastMath.sqrt(1 - m2 / muA); v = hyperbolicEccentricToTrue(FastMath.log((eCH + eSH) / (eCH - eSH)) / 2); } // compute perigee argument final Vector3D node = new Vector3D(raan, 0.0); final double px = Vector3D.dotProduct(pvP, node); final double py = Vector3D.dotProduct(pvP, Vector3D.crossProduct(momentum, node)) / FastMath.sqrt(m2); pa = FastMath.atan2(py, px) - v; }