ECEFtoNEDFrameConverter.java
/*
* Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.frames.converters;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.WrongSizeException;
import com.irurueta.geometry.InvalidRotationMatrixException;
import com.irurueta.navigation.frames.CoordinateTransformation;
import com.irurueta.navigation.frames.ECEFFrame;
import com.irurueta.navigation.frames.ECIorECEFFrame;
import com.irurueta.navigation.frames.FrameType;
import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
import com.irurueta.navigation.frames.NEDFrame;
import com.irurueta.navigation.geodesic.Constants;
/**
* Converts from ECEF frame to NED frame.
* This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
* Integrated Navigation Systems, Second Edition" and on the companion software available at:
* <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/ECEF_to_NED.m">
* https://github.com/ymjdz/MATLAB-Codes/blob/master/ECEF_to_NED.m
* </a>
*/
public class ECEFtoNEDFrameConverter implements FrameConverter<ECEFFrame, NEDFrame> {
/**
* The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
*/
public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
/**
* Earth eccentricity as defined on the WGS84 ellipsoid.
*/
public static final double EARTH_ECCENTRICITY = Constants.EARTH_ECCENTRICITY;
/**
* Converts source ECEF frame to a new NED frame instance.
*
* @param source source frame to convert from.
* @return a new destination frame instance.
*/
@Override
public NEDFrame convertAndReturnNew(final ECEFFrame source) {
final var result = new NEDFrame();
convert(source, result);
return result;
}
/**
* Converts source ECEF frame to destination NED frame.
*
* @param source source frame to convert from.
* @param destination destination frame instance to convert to.
*/
@Override
public void convert(final ECEFFrame source, final NEDFrame destination) {
convertECEFtoNED(source, destination);
}
/**
* Gets source frame type.
*
* @return source frame type.
*/
@Override
public FrameType getSourceType() {
return FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME;
}
/**
* Gets destination frame type.
*
* @return destination frame type.
*/
@Override
public FrameType getDestinationType() {
return FrameType.LOCAL_NAVIGATION_FRAME;
}
/**
* Converts source ECEF frame to a new NED frame instance.
*
* @param source source frame to convert from.
* @return a new destination frame instance.
*/
public static NEDFrame convertECEFtoNEDAndReturnNew(final ECEFFrame source) {
final var result = new NEDFrame();
convertECEFtoNED(source, result);
return result;
}
/**
* Converts source ECEF frame to destination NED frame.
*
* @param source source frame to convert from.
* @param destination destination frame instance to convert to.
*/
@SuppressWarnings("DuplicatedCode")
public static void convertECEFtoNED(final ECEFFrame source, final NEDFrame destination) {
final var x = source.getX();
final var y = source.getY();
final var z = source.getZ();
// Convert position using Borkowski closed-form exact solution from (2.113).
final var longitude = Math.atan2(y, x);
// From (C.29) and (C.30)
final var ecc2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
final var k1 = Math.sqrt(1.0 - ecc2) * Math.abs(z);
final var k2 = ecc2 * EARTH_EQUATORIAL_RADIUS_WGS84;
final var x2 = x * x;
final var y2 = y * y;
final var beta = Math.sqrt(x2 + y2);
final var e = (k1 - k2) / beta;
final var f = (k1 + k2) / beta;
// From (C.31)
final var p = 4.0 / 3.0 * (e * f + 1.0);
// From (C.32)
final var e2 = e * e;
final var f2 = f * f;
final var q = 2.0 * (e2 - f2);
// From (C.33)
final var p3 = p * p * p;
final var q2 = q * q;
final var d = p3 + q2;
// From (C.34)
final var sqrtD = Math.sqrt(d);
final var exp = 1.0 / 3.0;
final var v = Math.pow(sqrtD - q, exp) - Math.pow(sqrtD + q, exp);
// From (C.35)
final var g = 0.5 * (Math.sqrt(e2 + v) + e);
// From (C.36)
final var g2 = g * g;
final var t = Math.sqrt(g2 + (f - v * g) / (2.0 * g - e)) - g;
// From (C.37)
final var t2 = t * t;
final var latitude = Math.signum(z) * Math.atan((1.0 - t2) / (2.0 * t * Math.sqrt(1.0 - ecc2)));
// From (C.38)
final var height = (beta - EARTH_EQUATORIAL_RADIUS_WGS84 * t) * Math.cos(latitude)
+ (z - Math.signum(z) * EARTH_EQUATORIAL_RADIUS_WGS84 * Math.sqrt(1.0 - ecc2))
* Math.sin(latitude);
try {
// Calculate ECEF to NED coordinate transformation matrix
final var cen = CoordinateTransformation.ecefToNedMatrix(latitude, longitude);
// Transform velocity using (2.73)
final var vx = source.getVx();
final var vy = source.getVy();
final var vz = source.getVz();
final var vEbe = new Matrix(ECIorECEFFrame.NUM_VELOCITY_COORDINATES, 1);
vEbe.setElementAtIndex(0, vx);
vEbe.setElementAtIndex(1, vy);
vEbe.setElementAtIndex(2, vz);
final var vEbn = cen.multiplyAndReturnNew(vEbe);
final var vn = vEbn.getElementAtIndex(0);
final var ve = vEbn.getElementAtIndex(1);
final var vd = vEbn.getElementAtIndex(2);
// Transform attitude using (2.15)
final var cbe = source.getCoordinateTransformation().getMatrix();
cen.multiply(cbe); // cen is now cbn
final var c = new CoordinateTransformation(cen, FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
// Set result
destination.setLatitude(latitude);
destination.setLongitude(longitude);
destination.setHeight(height);
destination.setVn(vn);
destination.setVe(ve);
destination.setVd(vd);
destination.setCoordinateTransformation(c);
} catch (final WrongSizeException | InvalidRotationMatrixException
| InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
}