gps.h

基于多假设的地图匹配算法。程序能够根据车辆在行驶过程中收集到的GPS/DR数据正确得到当前车辆所在的道路位置。

/******************************************************************************

    File Name:    include/GPS.h
    Description:  GPS data definition and its transformation
                  transformation uses 2-axis ellipsoid approximation

******************************************************************************/

/******************************************************************************

    Author:       [e-mail]	
    Upadate:
      2008/09/16  File founded

    Copyright 2008-2009 Robot Lab., Dept.of Automation, Tsinghua University

******************************************************************************/

#ifndef __GPS_H__
#define __GPS_H__

#include <math.h>
#include "../include/Setting.h"
#include "../include/Coordinate.h"

/******************************                   Data structures                           ***************************************************/
/*Raw data structure*/
typedef struct
{
    DATATYPE        longitude;                                          /*Longitude in degrees*/
    DATATYPE        latitude;                                           /*Latitude in degrees*/
}GPS;

/*Use COORD as formatted data structure*/


/******************************                   Synchronition parameters                  ***************************************************/
const TIME GPSPeriod = 1000;                                            /*GPS period time, measured in milliseconds*/


/******************************                   Geographic constants                      ***************************************************/
const DATATYPE DegToRad = Pi/180;                                       /*Transform angle measured by degrees to radians*/
const DATATYPE RadToDeg = 180/Pi;                                       /*Transform angle measured by radians to degrees*/
const DATATYPE EarthRadiusAvg = DATATYPE(6371004);                      /*Average radius of the earth*/
const DATATYPE EarthRadiusEquator = DATATYPE(6378140);                  /*Equator radius of the earth*/
const DATATYPE EarthRadiusPole = DATATYPE(6356755);                     /*Pole radius of the earth*/


/******************************                   Transformation function definitions       ***************************************************/
inline void GPS2Coord(const GPS& gps, const GPS& gpsCenter,
                      COORD& coord);                                    /*Transform GPS data gps to coordinate coord, refer to gpsCenter*/
inline void Coord2GPS(const COORD& coord, const GPS& gpsCenter,
                      GPS& gps);                                        /*Transform coordinate coord to GPS data gps, refer to gpsCenter*/


/******************************                   Transformation function implementations   ***************************************************/
void GPS2Coord(const GPS& gps, const GPS& gpsCenter,
               COORD& coord)
{
    coord.east = (gps.longitude - gpsCenter.longitude) * DegToRad * EarthRadiusEquator * DATATYPE(cos(gps.latitude*DegToRad));
    coord.north = (gps.latitude - gpsCenter.latitude) * DegToRad * EarthRadiusPole;
}

void Coord2GPS(const COORD& coord, const GPS& gpsCenter,
               GPS& gps)
{
    gps.latitude = coord.north/EarthRadiusPole*RadToDeg + gpsCenter.latitude;
    gps.longitude = coord.east/EarthRadiusEquator*RadToDeg/DATATYPE(cos(gps.latitude*DegToRad)) + gpsCenter.longitude;
}

#endif /*__GPS_H__*/