USARBot.INSSensor

/*
  * INSSensor.uc
  * Inertial Navigation System
  * author:  Chris Scrapper
  * brief :  Simulates an INS sensor by using angular velocities
  *          and distance traveled.
  *
  *          This sensor uses gaussian random number generator to
  *          add noise to angular velocities.  This sensor uses this
  *          information to update the sensor's current prediction of
  *          orientation.   The total distance traveled in one time
  *          step is computed, agian adding gaussian noise, and is
  *          decomposed into distance vectors using polar coordinates.
  *          This is added to estimates of location. All noise is
  *          proportional to rate of change, more change causes
  *          more error.
*/
class INSSensor extends sensor config (USARBot);

var USARRemoteBot rBot;

var config float ScanInterval;

var string insData;  // Data string
var Vector rotEst;   // Sensor data, estimated Orientation
var Vector xyzEst;   // Sensor data, estimated Location
var Vector rotPrev;  // Previous ground truth Orientation
var Vector xyzPrev;  // Previous ground truth Location
var bool go;         // variable set to begin sensor calculation

simulated function PostNetBeginPlay()
{
    Super.PostNetBeginPlay();
    insData="";
    if (ScanInterval>0.0)
        SetTimer(ScanInterval,true);
}

function Init(String SName, Actor parent, vector position, rotator direction, KVehicle platform, name mount)
{
        Super.Init(SName, parent, position, direction, platform, mount);
    rotEst=converter.RotatorFromUU(base.Rotation);
    xyzEst=converter.LengthVectorFromUU(base.Location);
    rotPrev = rotEst;
    xyzPrev = xyzEst;
    go = false;
}


function timer()
{
     local Vector rotTrue;
     local Vector xyzTrue;
     local Vector rotRate;
     local float dist;

     // Get rate of change from ground truth
     xyzTrue = converter.LengthVectorFromUU(base.Location);
     rotTrue = converter.RotatorFromUU(base.Rotation);
     // Normalize orientation between [0,2PI]
     rotTrue.x = converter.normRad_ZeroTo2PI(rotTrue.x);
     rotTrue.y = converter.normRad_ZeroTo2PI(rotTrue.y);
     rotTrue.z = converter.normRad_ZeroTo2PI(rotTrue.z);

     // Calculate rate of change for time step
     rotRate = (rotTrue - rotPrev);

     if(go)
     {
         // Add gaussian noise and update orientation estimate
         rotEst.x += rotRate.x + (rotRate.x * utils.gaussRand(Mean,Sigma));
         rotEst.y += rotRate.y + (rotRate.y * utils.gaussRand(Mean,Sigma));
         rotEst.z += rotRate.z + (rotRate.z * utils.gaussRand(Mean,Sigma));
         // Normalize orientation between [0,2PI]
         rotEst.x = converter.normRad_ZeroTo2PI(rotEst.x);
         rotEst.y = converter.normRad_ZeroTo2PI(rotEst.y);
         rotEst.z = converter.normRad_ZeroTo2PI(rotEst.z);
         // Calculate total distance traveled in one time step
         dist = sqrt(  (xyzTrue.x-xyzPrev.x) * (xyzTrue.x-xyzPrev.x) +
                       (xyzTrue.y-xyzPrev.y) * (xyzTrue.y-xyzPrev.y) +
                       (xyzTrue.z-xyzPrev.z) * (xyzTrue.z-xyzPrev.z) );
         // Add Gaussian noise
         dist += dist * utils.gaussRand(Mean,Sigma);
         // Update location estimate
         xyzEst.x = xyzEst.x + dist*Cos(rotEst.z)*Cos(rotEst.y);
         xyzEst.y = xyzEst.y + dist*Sin(rotEst.z)*Cos(rotEst.y);
         xyzEst.z = xyzEst.z + dist*Sin(rotEst.y);
      }
      else
      {
          //log("NO GO");
          // make sure vehicle is at rest before starting INS
          if (rotRate.x == 0 && rotRate.y == 0 && rotRate.z == 0)
               go = true;
      }

      //log(xyzTrue$" "$rotTrue$" "$xyzEst$" "$rotEst);
      rotPrev=rotTrue;
      xyzPrev=xyzTrue;

      if (converter!=None)
      {
         insData="{Location "$xyzEst$"} {Orientation "$rotEst$"}";
         //log(insData);
      }
      else
      {
         insData="{Location "$converter.LengthVectorToUU(xyzEst)$"} {Orientation "$converter.LengthVectorToUU(rotEst)$"}";
      }
}

function string Set(String opcode, String args)
{
    local string rVal;
        local array<string> toks;

        if(Caps(opcode)=="POSE")
        {
              //Parse string into tokens
              toks=utils.tokenizer(args,",");
              if(toks.Length == 6)
              {
                  // Block, don't know if need but never hurts
                  go=false;
                  // Set Estimates
                  xyzEst.x=Float(toks[0]);
                  xyzEst.y=Float(toks[1]);
                  xyzEst.z=Float(toks[2]);
                  rotEst.x=Float(toks[3]);
                  rotEst.y=Float(toks[4]);
                  rotEst.z=Float(toks[5]);
                  //UnBlock
                  go=true;
                  rVal="OK";
              }
        }
        return rVal;
}


function String GetData()
{
    local string outstring;

    if (ScanInterval==0.0)
        timer();
    if (insData == "")
        return "";

    outstring = "{Name "$ItemName$"} "$insData;
    insData = "";
    return outstring;
}

function String GetConfData()
{
    local string outstring;
    outstring = Super.GetConfData();
    outstring = outstring@"{ScanInterval "$ScanInterval$"}";
    return outstring;
}

defaultproperties
{
    ItemType="INS"
    HiddenSensor=true
    ScanInterval=0.5
    Noise=0.01
    DrawScale=0.9524
    StaticMesh=StaticMesh'USARSim_VehicleParts_Meshes.Sensors.Sensor'
    Mass=0.1
}