/* -*- Mode: Text -*- */

/********************************************************************\
* File: gps.hs                                                       *
* Date: 05/25/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Simple positioning sensor using GPS modeling                       *
*                                                                    *
* Longitudinal and lateral position measurements from the GPS are    *
* modeled using independent Gaussian noise distribution. Operation   *
* modes are based on the precipitation on the current segment.       *
*                                                                    *
* Sensor parameters can be set in this file, or the type 'creating'  *
* the sensor.                                                        *
*                                                                    *
* External C functions: -                                            *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_GPS_HS
#define SAHS_GPS_HS

#include <cmu/sensors/noise.hs>

type PositionSensor_GPS
{
  output continuous number pos_x, pos_y;   // Sensor readings 
         continuous number s;              // Measurement signal:
                                           // "exists" [1] or "not" [0]
         continuous number err_x, err_y;   // Measurement errors;

  input  continuous number gxp, gyp;    // Actual position and
         continuous number precip;      // Precipitation
                                        // from associated VREP

  state  continuous number mean, var;   // Measurement noise parameters
         Gaussian err_signals;

  discrete normal {measure;},    // Discrete states for operation modes
           problem {measure;},
           nodata {nomeasure;};

  setup
    define
    {
      Gaussian tnoise := create(Gaussian);
    }
    do
    {
      err_signals := tnoise;
      mean := 0;         // both measurements are characterized as
      var := 0.3*0.3;    // Gaussian distributions: 
                         // N(0,7) [data from MIT LL]
                         // N(0,1.8E-5) [data from Navlab; static]
                         // N(0,0.09) [new NAVLAB toy]
    };

    flow
      measure
        {
          err_x = sqrt(var) * sg1(err_signals) + mean;
          err_y = sqrt(var) * sg2(err_signals) + mean; 
          pos_x = gxp + err_x;
          pos_y = gyp + err_y;
        },

      nomeasure
        {
          pos_x = 0.0; pos_y = 0.0; err_x = 0; err_y = 0;
        };

    transition
      
      normal -> problem {}
      when precip >= 10 and precip < 60
        do
          {
            var := 0.9*0.9; s := 1;
          },
      
      normal -> nodata {}
      when precip >= 60
        do
          {
            s := 0;
          },

      problem -> normal {}
      when precip <= 10
        do
          {
            var := 0.3*0.3; s := 1;
          },
      
      problem -> nodata {}
      when precip > 60
        do
          {
            s := 0;
          },

      nodata -> normal {}
      when precip <= 10
        do
          {
            var := 0.3*0.3; s := 1;
          },

      nodata -> problem {}
      when precip <= 60
        do
          {
            var := 0.9*0.9; s := 1;
          };
}

#endif // SAHS_GPS_HS