/***************************************************************************** Copyright 1993 MBARI ****************************************************************************** Summary : Kalman filter routines header file Filename : kalman_filter.h Author : Michael B. Matthews Project : New ROV Version : Version 1.0 Created : February 12, 1996 Modified : Archived : Notes : *****************************************************************************/ #ifndef kalman_h #define kalman_h #define KALMAN_DM_PERIOD 20000 /* 50Hz smaple rate for Kalman filter */ #define STATE_DM_PERIOD 50000 /* 20Hz smaple rate for state switch */ #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 NAN -99999 /* Kalman filter running status */ typedef enum{ FILTER_OFF, /* Kalman filter is not running */ FILTER_INITIALIZING, /* Kalman filter is initializing, output invalid */ FILTER_NORMAL, /* Kalman filter is running */ FILTER_ERROR_SINGULAR, /* Kalman filter has singular HPH' + R matrix */ FILTER_ERROR_NEGATIVE_DEFINITE, /* Kalman filter has neg definite cov */ FILTER_ERROR_DIVERGENCE, /* Kalman filter is divergent */ FILTER_ERROR_INNOVATIONS /* Kalman filter has nonwhite innovations proces */ } filterStatus; /* Vehicle navigational state status */ typedef enum{ INVALID, /* state is invalid */ GYRO_COMPASS_FILTERED, /* state is computed in heading filter */ KALMAN_FILTERED, /* state is an acceptable Kalman filtered estimate */ COMPASS, /* state is a direct measurement from the compass */ GYRO, /* state is a direct measurement from the gyro */ GYRO_RATE, /* state is a direct measurement from the gyro rate output */ 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 */ ADV, /* state is a direct measurement from ADV sensor */ USBL, /* state is based on USBL measurement only */ LBL, /* state is based on LBL measurement only */ USBL_LBL /* state is based on USBL and LBL measurements only */ } stateStatus; /* Sensor status */ typedef enum{ SENSOR_UPDATE, /* sensor measurement used in last update */ SENSOR_VALID, /* sensor valid, no new measurement */ SENSOR_INVALID /* sensor invalid, measurement ignored */ } sensorStatus; /* heading state estimate structure */ typedef struct{ Flt32 psi; /* heading estimate */ Flt32 r; /* angular velocity estimate */ Flt32 xi; /* compass state */ Flt32 d; /* drift state */ } Heading_State; /* measurement input structure */ typedef struct{ MBool gyroFlag; Flt32 gyro; Flt32 gyroRate; Flt32 gyroOffset; MBool compassFlag; Flt32 compass; MBool pitchSensorFlag; Flt32 pitchSensor; MBool rollSensorFlag; Flt32 rollSensor; MBool depthSensorFlag; Flt32 depthSensor; MBool velocityADVFlag[3]; Flt32 velocityADV[3]; } Measurements; /* Vehicle state DM structure */ typedef struct{ filterStatus filterStatus; /* 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; } State; typedef struct { Flt32 sx, sy, sz; /* position state vector */ Flt32 vx, vy, vz; /* velocity state vector */ } Position; typedef struct { Word sxStatus, syStatus, szStatus; /* estimate status */ Word vxStatus, vyStatus, vzStatus; } PositionStatus; typedef struct { Flt32 heading, pitch, roll; /* attitude in Euler angles */ Flt32 headingRate, pitchRate, rollRate; /* Euler angular rate vector */ } Attitude; typedef struct { Word headingStatus, pitchStatus, rollStatus; Word headingRateStatus, pitchRateStatus, rollRateStatus; } AttitudeStatus; typedef struct { DM_Item compassOffset; /* compass magnetic variation */ DM_Item compassRadians; /* compass heading measurement */ DM_Item compassDataValid; /* compass data valid flag */ DM_Item gyroRadians; /* gyro heading measurement */ DM_Item gyroRateRadians; /* gyro heading rate measurement */ DM_Item gyroDataValid; /* gyro data valid flag */ DM_Item Heading; /* filtered heading */ DM_Item pitchSensor; /* pitch sensor */ DM_Item rollSensor; /* roll sensor */ DM_Item depthSensor; /* depth sensor */ DM_Item depthDataValid; /* depth sensor data valid */ DM_Item velocityADV[3]; /* ADV velocity data */ DM_Item dataValidADV[3]; /* ADV data valid */ DM_Item Heading_Rate; /* filtered heading rate */ DM_Item Pitch_Rate; /* filtered pitch rate */ DM_Item Roll_Rate; /* filtered roll rate */ DM_Item Acceleration_X; /* acceleration x-axis */ DM_Item Acceleration_Y; /* acceleration y-axis */ DM_Item Acceleration_Z; /* acceleration z-axis */ DM_Item Heading_Rate_Bias; /* heading rate bias */ DM_Item Pitch_Rate_Bias; /* pitch rate bias */ DM_Item Roll_Rate_Bias; /* roll rate bias */ DM_Item Heading_Rate_Bias_2D; /* heading rate bias 2nd order */ DM_Item Gyro_Drift; /* gyro drift */ DM_Item Gyro_Drift_2D; /* gyro drift 2nd order */ DM_Item Status; /* status process flag */ DM_Item position; /* vehicle position */ DM_Item positionStatus; /* vehicle position status */ DM_Item attitude; /* vehicle attitude */ DM_Item attitudeStatus; /* vehicle attitude status */ } kalmanDMItems; /* sensors group id bits */ typedef struct { DWord compassBit; /* compass bit */ DWord gyroBit; /* gyro bit */ DWord pitchBit; /* pitch bit */ DWord rollBit; /* roll bit */ DWord depthBit; /* depth bit */ DWord ADVBit[3]; /* ADV bit */ } sensorBits; /* buffer pointer structure for logger */ typedef struct { MBool status; MBool state; Flt32 compass; Flt32 gyro; Int32 count; MotionPak_Input_Buffer input_buffer; MotionPak_Output_Buffer* output_buffer; MotionPak_Output_Buffer* write_buffer; MotionPak_Output_Buffer* temp_buffer; MotionPak_Output_Buffer output_buffer1; MotionPak_Output_Buffer output_buffer2; MotionPak_Output output; FIR_Filter filter; } MotionPak_Buffers; /* Kalman status commands */ enum { START, STOP, STOP_LOGGING, START_LOGGING, HEADING_RESET, ATTITUDE_RESET, NOP }; enum { BUFFER_OK, BUFFER_FULL, BUFFER_READY, BUFFER_BUSY }; /* function prototypes */ #ifdef __STDC__ Errno initKalmanFilter(kalmanDMItems *kalmanDmi, DM_Group *sensorGroup, sensorBits *sensorId); #endif STATUS Kalman_Filter(Void); STATUS Kalman_Filter_Offline(char*); int Logger_MotionPak_Write_Buffer(MotionPak_Buffers*, FILE*, SEM_ID); #endif