mhgpsdr.cpp

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

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

    File Name:   MH/MHGPSDR.cpp
    Description: Multi-hypothesis map matching
                 Calculate weights with GPS, bearing and odometry distance 

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

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

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

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

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

#include "../map/SubMap.h"
#include "../MH/MHGPSDR.h"

/******************************                   Implementation of GPS-DR MH matching      ***************************************************/
/*Constructor*/
MHGPSDR::MHGPSDR()
{
    return;
}

/*Destructor*/
MHGPSDR::~MHGPSDR()
{
    return;
}

/*Weigh hypotheses (after new hypothesis generated)*/
DATATYPE MHGPSDR::Weigh(const HypoNode& hypoOld, const HypoNode& hypoNew)
{
    COORD coordOnLink;
    DATATYPE finalBear;

    hypoNew.link->PointOnLink(hypoNew.direction, hypoNew.disOnLink, coordOnLink, finalBear);

    DATATYPE weight = DATATYPE(0);

    /*GPS weight*/
    if(gpsAvail==1)
    {
        weight += - coordOnLink.DisSquareTo(coord)/(2*GPSDev*GPSDev);
    }

    /*Bearing weight*/
    {
        DATATYPE angleDif;
        angleDif = ABS(finalBear - hypoNew.finalBear);
        if(angleDif>Pi) angleDif = 2*Pi-angleDif;
        weight += - angleDif*angleDif/(2*BearDev*BearDev);

        if(angleDif>BearPunishThre) weight += BearPunish;
    }

    /*Distance weight*/
    if(hypoNew.link != hypoOld.link)
    {
        DATATYPE DisDif;
        DisDif = MAX(1 - hypoNew.disOnLink/hypoOld.link->linkLen,0);
        weight += - DisDif*DisDif/(2*BearDev*BearDev);
    }

    return weight;
}