/*****************************************************************************
Copyright 1999 MBARI
******************************************************************************
Summary  : Kalman filter routines header file
Filename : kalman.h
Author   : Michael B. Matthews
Project  : New ROV
Version  : Version 1.1
Created  : 12.2.96  - V1.0
Modified : 22.10.99 - V1.1 included heading DMI's
Archived :
Notes    :
*****************************************************************************/

#ifndef kalman_h
#define kalman_h


#define KALMAN_DM_PERIOD 50000   /* 20Hz output rate of the Kalman filter */

#define NAVIGATION_STATE_DM     "TIBURON.NAV.STATE"
#define POSITION_DM             "TIBURON.VEHICLE.POSITION"
#define POSITION_STATUS_DM      "TIBURON.VEHICLE.POSITION_STATUS"
#define ATTITUDE_DM             "TIBURON.VEHICLE.ATTITUDE"
#define ATTITUDE_STATUS_DM      "TIBURON.VEHICLE.ATTITUDE_STATUS"
#define KALMAN_COMPASS_DM       "TIBURON.SENSOR.COMPASS.RADIANS"
#define KALMAN_GYRO_DM          "TIBURON.SENSOR.GYRO.RADIANS"
#define HEADING_FILTER_RESET_DM "TIBURON.SENSOR.HEADING_FILTER_RESET"
#define HEADING_DATA_VALID_DM   "TIBURON.VEHICLE.HEADING.DATA_VALID"
#define HEADING_RAD_DM          "TIBURON.VEHICLE.HEADING.RADIANS"
#define HEADING_DEG_DM          "TIBURON.VEHICLE.HEADING.DEGREES"
#define HEADING_RATE_RAD_DM     "TIBURON.VEHICLE.HEADING_RATE.RADIANS"
#define HEADING_RATE_DEG_DM     "TIBURON.VEHICLE.HEADING_RATE.DEGREES"

#define NAN -99999


/* Kalman filter running status */
enum filterStatus
{
  FILTER_OFF,               /* Kalman filter is not running                  */
  FILTER_INITIALIZING,      /* Kalman filter is initializing, output invalid */
  FILTER_NORMAL,            /* Kalman filter is running                      */
  FILTER_SINGULAR,          /* Kalman filter has singular HPH' + R matrix    */
  FILTER_NEG_DEFINITE,      /* Kalman filter has neg definite covariance     */
  FILTER_DIVERGENCE,        /* Kalman filter is divergent                    */
  FILTER_INNOVATIONS        /* Kalman filter has nonwhite innovations proces */
};


/* vehicle navigational state status */
enum stateStatus
{
  INVALID,            /* state is invalid                                    */
  HEADING_FILTERED,   /* state is computed in heading filter                 */
  KALMAN_FILTERED,    /* state is an acceptable Kalman filtered estimate     */
  COMPASS_SENSOR,     /* state is a direct measurement from the compass      */
  GYRO_SENSOR,        /* state is a direct measurement from the gyro         */
  GYRO_RATE,          /* state is a direct measurement from the gyro rate    */
  MOTIONPAK,          /* state is a direct measurement from the MotionPak    */
  PITCH_SENSOR,       /* state is a direct measurement from pitch sensor     */
  ROLL_SENSOR,        /* state is a direct measurement from roll sensor      */
  DEPTH_SENSOR,       /* state is a direct measurement from depth sensor     */
  DVL_SENSOR,         /* state is a direct measurement from ADV sensor       */
  USBL_SENSOR,        /* state is based on USBL measurement only             */
  LBL_SENSOR,         /* state is based on LBL measurement only              */
  USBL_LBL            /* state is based on USBL and LBL measurements only    */
};


/* sensor status */
enum sensorStatus
{
  SENSOR_UPDATE,      /* sensor measurement used in last update              */
  SENSOR_VALID,       /* sensor valid, no new measurement                    */
  SENSOR_INVALID      /* sensor invalid, measurement ignored                 */
};


/* vehicle state structure */
struct State
{
  filterStatus   status;                     /* filter status              */

  Flt32  sx, sy, sz;                         /* position state vector      */
  Flt32  vx, vy, vz;                         /* velocity state vector      */
  Flt32  ax, ay, az;                         /* acceleration state vector  */
  Flt32  heading, pitch, roll;               /* attitude in Euler angles   */
  Flt32  p, q, r;                            /* angular rate state vector  */
  Flt32  headingRate, pitchRate, rollRate;   /* Euler angular rate vector  */

  Flt32  sxVar, syVar, szVar;                /* estimate variances         */
  Flt32  vxVar, vyVar, vzVar;
  Flt32  axVar, ayVar, azVar;
  Flt32  headingVar, pitchVar, rollVar;
  Flt32  headingRateVar, pitchRateVar, rollRateVar;
  Flt32  pVar, qVar, rVar;

  stateStatus   sxStatus, syStatus, szStatus;
  stateStatus   vxStatus, vyStatus, vzStatus;
  stateStatus   axStatus, ayStatus, azStatus;
  stateStatus   headingStatus, pitchStatus, rollStatus;
  stateStatus   headingRateStatus, pitchRateStatus, rollRateStatus;
  stateStatus   pStatus, qStatus, rStatus;

  sensorStatus  compassStatus, gyroStatus, gyroRateStatus;
  sensorStatus  MotionPakStatus, pitchSensorStatus, rollSensorStatus;
  sensorStatus  ADVStatus, USBLStatus, LBLStatus;
};


/* filter position output structs */
struct  VehiclePosition
{
  Flt32  sx, sy, sz;                        /* position state vector      */
  Flt32  vx, vy, vz;                        /* velocity state vector      */
};

/* filter status structs */
struct  VehiclePositionStatus
{
  stateStatus  sxStatus, syStatus, szStatus;
  stateStatus  vxStatus, vyStatus, vzStatus;
};


/* filter attitude output structs */
struct  VehicleAttitude
{
  Flt32  heading, pitch, roll;               /* attitude in Euler angles   */
  Flt32  headingRate, pitchRate, rollRate;   /* Euler angular rate vector  */
};

/* filter status structs */
struct  VehicleAttitudeStatus
{
  stateStatus  headingStatus, pitchStatus, rollStatus;
  stateStatus  headingRateStatus, pitchRateStatus, rollRateStatus;
};

#endif