gpsUtils.h

Go to the documentation of this file.

#ifndef GPSUTILS_H
#define GPSUTILS_H
 
#include <math.h>
 
class GpsUtils
{
    // WGS-84 geodetic constants
public:
    GpsUtils()
    {
        a = 6378137;        // WGS-84 Earth semimajor axis (m)
        b = 6356752.3142;   //WGS-84 Earth semiminor axis (m)
        f = (a - b) / a;    // Ellipsoid Flatness
        e_sq = f * (2 - f); //Square of Eccentricity
    }
    ~GpsUtils() {}
 
    void GeodeticToEcef(double lat, double lon, double h)
    {
        // Convert to radians in notation consistent with the paper:
        double lambda = DegreesToRadians(lat);
        double phi = DegreesToRadians(lon);
        double s = sin(lambda);
        double N = a / sqrt(1 - e_sq * s * s);
 
        double sin_lambda = sin(lambda);
        double cos_lambda = cos(lambda);
        double cos_phi = cos(phi);
        double sin_phi = sin(phi);
 
        xEcef = (h + N) * cos_lambda * cos_phi;
        yEcef = (h + N) * cos_lambda * sin_phi;
        zEcef = (h + (1 - e_sq) * N) * sin_lambda;
    }
 
    void EcefToEnu(double lat0, double lon0, double h0)
    {
        // Convert to radians in notation consistent with the paper:
        double lambda = DegreesToRadians(lat0);
        double phi = DegreesToRadians(lon0);
        double s = sin(lambda);
        double N = a / sqrt(1 - e_sq * s * s);
 
        double sin_lambda = sin(lambda);
        double cos_lambda = cos(lambda);
        double cos_phi = cos(phi);
        double sin_phi = sin(phi);
 
        double x0 = (h0 + N) * cos_lambda * cos_phi;
        double y0 = (h0 + N) * cos_lambda * sin_phi;
        double z0 = (h0 + (1 - e_sq) * N) * sin_lambda;
 
        double xd, yd, zd;
        xd = xEcef - x0;
        yd = yEcef - y0;
        zd = zEcef - z0;
 
        // This is the matrix multiplication
        xEast = -sin_phi * xd + cos_phi * yd;
        yNorth = -cos_phi * sin_lambda * xd - sin_lambda * sin_phi * yd +
                 cos_lambda * zd;
        zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd +
              sin_lambda * zd;
    }
 
    void GeodeticToEnu(double lat, double lon, double h, double lat0,
                       double lon0, double h0)
    {
        GeodeticToEcef(lat, lon, h);
        EcefToEnu(lat0, lon0, h0);
    }
    bool AreClose(double x0, double x1)
    {
        double d = x1 - x0;
        return (d * d) < 1e-2;
    }
    double DegreesToRadians(double degrees) { return M_PI / 180.0 * degrees; }
    double RadiansToDegrees(double radians) { return 180.0 / M_PI * radians; }
    double xEast; 
    double yNorth;
    double zUp;
 
private:
    double a;    
    double b;    
    double f;    
    double e_sq; 
    double xEcef; 
    double yEcef; 
    double zEcef; 
};
 
#endif