CASL 根据轮速计算行驶轨迹的曲率

function curveFromWhlSpd (var sigWhlSpdReRiSts,var sigWhlSpdReRiMovDir,var sigWhlSpdReRi,var sigWhlSpdReLeSts,var sigWhlSpdReLeMovDir,var sigWhlSpdReLe)
{
  double WhlSpdReRi_mps;
  double WhlSpdReLe_mps;

  double whlBase=1.672;//m
  
  if (sigWhlSpdReLeSts==0)
  {
    if (sigWhlSpdReLeMovDir ==1)
    {
      WhlSpdReLe_mps=sigWhlSpdReLe/3.6;
    }
    else if(sigWhlSpdReLeMovDir==2)
    {
      WhlSpdReLe_mps=-sigWhlSpdReLe/3.6;
    }
    else
    {
      WhlSpdReLe_mps=0;
    }
  }
  else
  {
    WhlSpdReLe_mps=0;
  }

  if (sigWhlSpdReRiSts==0)
  {
    if (sigWhlSpdReRiMovDir ==1)
    {
      WhlSpdReRi_mps=sigWhlSpdReRi/3.6;
    }
    else if(sigWhlSpdReRiMovDir==2)
    {
      WhlSpdReRi_mps=-sigWhlSpdReRi/3.6;
    }
    else
    {
      WhlSpdReRi_mps=0;
    }
  }
  else
  {
    WhlSpdReRi_mps=0;
  }

  if ((WhlSpdReLe_mps+WhlSpdReRi_mps)*0.5*3.6 <10)
  {
    return 0;//车速小于10kph，不计算曲率
  }

  if (abs(WhlSpdReLe_mps+WhlSpdReRi_mps) < 0.0001)
  {
    return 0;
  }
  else
  {
    //return WhlSpdReRi_mps+WhlSpdReLe_mps;
    return 2*(WhlSpdReRi_mps-WhlSpdReLe_mps)/(WhlSpdReLe_mps+WhlSpdReRi_mps)/whlBase*0.98;
  }
  
}

计算结果可以使用以下函数进行滤波处理：

function LowPassFilterNew_DrvR (var sigDrvRFromWhlSpd)
{
  double LastValue = 0;
  double result = 0;
  double pi = 3.141592653589;
  double deltaT = 0;
  //!
  double fc = 0.2; //cutoff frequency (Grenzfrequenz)
  double alpha = 0;
  Long CallCounter = 0;

  double sigDrvRFromWhlSpd_absValue;

  double lftCnt;
  double rgtCnt;

  sigDrvRFromWhlSpd_absValue = abs(sigDrvRFromWhlSpd);

  CallCounter++;
  if (CallCounter >= 2)
  {
    deltaT = time(sigDrvRFromWhlSpd) - time(sigDrvRFromWhlSpd[-1]);
    alpha = 1/(1/2/pi/deltaT/fc + 1);
    result = alpha * sigDrvRFromWhlSpd_absValue + (1 - alpha) * LastValue;
  }
  else
  {
    cancel;  //No return value
  }

  LastValue = result;

  if (result > 1000)
  {
    if (sigDrvRFromWhlSpd<0)
    {
      lftCnt --;
      rgtCnt ++;
    }
    else
    {
      lftCnt --;
      rgtCnt ++;
    }
  }
  else
  {
    lftCnt = 0;
    rgtCnt = 0;
  }

  lftCnt = min(max(0,lftCnt),15);
  rgtCnt = min(max(0,rgtCnt),15);

  if (result > 750)
  {
    if (lftCnt > rgtCnt)
    {
      return result;
    }
    else
    {
      return -result;
    }
  }
  else
  {
    if (sigDrvRFromWhlSpd < 0)
    {
      return -result;
    }
    else
    {
      return result;
    }
  }
}