/*************************************************************** SMOOTHR.C INPUT: 1) output of REFABS: absolute ref layer velocities averaged over fix intervals, with gap information 2) output of ADCPSECT, navigation option: time, and ship relative to reference layer for each profile 3) first estimate of starting latitude and longitude OUTPUT: 1) absolute ref layer velocity, interpolated and smoothed to the profile times. 2) absolute ship velocity at profile times. STARTED: Sun 07-31-1988 89/03/20 E.F. new version, separated into modules, and modified to optimize positions correctly in two iterations even when there are consecutive segments without fixes. This version also applies the good_ref test (based on max_gap_ratio) to refabs lines when being read in for smoothing, but not when they are used only as a source of position fixes in optimize_positions(). 89/05/01 E.F. Fixed bug in OPTPOS, in handling of cruise tracks crossing the dateline. Wed 05-10-1989 E.F. In OPTPOS, added checking for missing velocity data in sm structure before calculating distance between fix and cruise track. Thu 07-27-1989 E.F. In OPTPOS, made track segment array part of data area (instead of auto variable) to avoid possible stack overflow. In SMOOTHR.H, increased size of NSEGMENTS. Mon 07-31-1989 E.F. In OPTPOS, changed segmentation algorithm so that each gap forms its own segment. Wed 08-02-1989 fb: In OPTPOS: changed dimensions of array segindex in function adjust_nofix_segments from 40 to NSEGMENTS, which is currently 200. This seems to be the threshhold for a small memory model. Mon 91-09-09 JR: added HP_UNIX_C in preprocessor for memmove function (HP Unix C compiler has a memmove function) Mon 96-07-08 EF: Fixed a bug in the ref layer smoothing; the smoother was being positioned at the end of the ensemble instead of the middle. --------------------------------------------------------------- control file structure: reference_file: [output from ADCPSECT, with time and ref layer vel.] refabs_output: [file name] output: [file name] filter_hwidth= [half-width of filter in days] min_filter_fraction= [acceptable fraction of filter area with data; suggest 0.05] max_gap_ratio= [parameter limiting gap acceptance; suggest 0.05] max_gap_distance= [meters, distance uncertainty threshold for position calc] (suggest 1000 if data are poor, perhaps 200 otherwise) max_gap_time= [seconds, gap time threshold for starting a segment] (suggest 3600, one hour) ensemble_time= [seconds, ensemble time; e.g. 300 for usual 5-min ensemble] max_speed= [max speed of ship in question (m/s)] min_speed= [expected short-term speed variability, on or off station] iterations= [number of times to optimize the positions: 1 or 2 should do] fix_to_dr_limit= [threshold for printing fix times and deviations; in degrees] ***************************************************************/ #include "common.h" #include "dbhost.h" /* COMPILER */ #include "dbext.h" #include "use_db.h" #include "ioserv.h" /* PARAMETER_LIST_ENTRY_TYPE, ... */ #include "vstat.h" #include "pos_to_m.h" #include "cal.h" #include "smoothr.h" #ifndef M_PI #define M_PI 3.14159265358979323846 #endif #define VBUFSIZE 8192 #if (COMPILER != TURBO_C && COMPILER != HP_UNIX_C && COMPILER != ANSI_C) #if PROTOTYPE_ALLOWED char *memmove(char *dest, char *source, int nbytes) #else char *memmove(dest, source, nbytes) char *dest, *source; int nbytes; #endif { char *temp; int i; if ( (temp = malloc(nbytes)) == NULL ) return(NULL); for (i = 0; i < nbytes; i++) temp[i] = source[i]; for (i = 0; i < nbytes; i++) dest[i] = temp[i]; free(temp); return(dest); } #endif static int startup = 1; /* used (and set to zero) only in get_refabs */ static double filter_hwidth = 0.25; /* half-width of filter in days */ static REFABS_TYPE avg_fix2fix[NFMAX]; static int n_fix2fix; static FILE_NAME_TYPE fn_fix2fix, fn_ref, fn_log, fn_bin, fn_out; static FILE *fp_fix2fix, /* REFABS output */ *fp_ref, /* ship relative to reference layer */ *fp_log, /* statistics */ *fp_smbin, /* binary file for calculation and output */ *fp_out; /* ASCII version of output */ static double min_filt_fract; static double max_gap_ratio = 0.05; static double max_speed = 8; /* m/s; PRC ship speed */ static double min_speed = 0.5; /* m/s; minimum speed uncertainty in a gap */ /*glo*/double max_gap_distance; /* in meters, the distance uncertainty threshold for starting a new segment when integrating velocity to position. */ /*glo*/double max_gap_time; /* in seconds, max gap time for segmentation */ /*glo*/double ensemble_time; /* in seconds, ensemble interval */ static int iterations; /* for position optimization */ static double fix_to_dr_limit; static PARAMETER_LIST_ENTRY_TYPE smoothr_params[] = { {" reference_file:" , " %79s" , fn_ref , TYPE_STRING}, {" refabs_output:" , " %79s" , fn_fix2fix , TYPE_STRING}, {" output:" , " %79s" , fn_out , TYPE_STRING}, {" filter_hwidth=" , " %lf" , &filter_hwidth , TYPE_DOUBLE}, {" min_filter_fraction=", " %lf" , &min_filt_fract , TYPE_DOUBLE}, {" max_gap_ratio=" , " %lf" , &max_gap_ratio , TYPE_DOUBLE}, {" max_gap_distance=" , " %lf" , &max_gap_distance, TYPE_DOUBLE}, {" max_gap_time=" , " %lf" , &max_gap_time , TYPE_DOUBLE}, {" ensemble_time=" , " %lf" , &ensemble_time , TYPE_DOUBLE}, {" max_speed=" , " %lf" , &max_speed , TYPE_DOUBLE}, {" min_speed=" , " %lf" , &min_speed , TYPE_DOUBLE}, {" iterations=" , " %d" , &iterations , TYPE_INT}, {" fix_to_dr_limit=" , " %lf" , &fix_to_dr_limit , TYPE_DOUBLE}, {NULL , NULL , NULL , 0} }; #if PROTOTYPE_ALLOWED void make_bad_record(SMOOTHR_OUT_TYPE *r, VELOCITY_TYPE *s) #else void make_bad_record(r, s) SMOOTHR_OUT_TYPE *r; VELOCITY_TYPE *s; #endif { r->t = s->t; r->ru = r->rv = r->su = r->sv = r->x = r->y = BADFLOAT; r->ff = 0.0; } #if PROTOTYPE_ALLOWED void make_record(SMOOTHR_OUT_TYPE *r, VELOCITY_TYPE *ref, VELOCITY_TYPE *ship, POSITION_TYPE *pos, double fraction) #else void make_record(r, ref, ship, pos, fraction) SMOOTHR_OUT_TYPE *r; VELOCITY_TYPE *ref, *ship; POSITION_TYPE *pos; double fraction; #endif { r->t = ref->t; r->ru = ref->u; r->rv = ref->v; r->su = ship->u; r->sv = ship->v; r->x = pos->x; r->y = pos->y; r->ff = fraction; } /* FUNCTION: read_shipref() This function reads the velocity of the ship relative to the reference layer, as calculated and written to an ascii file by the "navigation" option in ADCPSECT.C. RETURNS: 0 if successful 1 on EOF or failure to scan a full line */ #if PROTOTYPE_ALLOWED int read_shipref(FILE *fp_ref, VELOCITY_TYPE *ref) #else int read_shipref(fp_ref, ref) FILE *fp_ref; VELOCITY_TYPE *ref; #endif { int nscan; char buf[120]; if (getline_nc(fp_ref, buf, 120) == EOF) return 1; nscan = sscanf(buf, " %lf %lf %lf", &(ref->t), &(ref->u), &(ref->v)); if (nscan != 3) return 1; return(0); } /*-------------------------------------------------------------- good_ref Calculates the distance that the ship might have gone during the gap times (based on the gap durations and the ship speeds at the start and end of the gaps) and compares it to the distance the ship would have gone during the reference interval if underway at full speed. If the ratio exceeds a threshold, we presume that there is no useful information in that reference interval, and return 0. */ #if PROTOTYPE_ALLOWED int good_ref(REFABS_TYPE *r) #else int good_ref(r) REFABS_TYPE *r; #endif { extern double min_speed, max_speed, max_gap_ratio; double speed, ratio; speed = sqrt( sqr(r->gap_u) + sqr(r->gap_v) ); speed = max_val(speed, min_speed); if (r->dt == 0.0) { printf("\n Warning: duplicate fix at time %f\n", r->t); fprintf(fp_log, "\n Warning: duplicate fix at time %f\n", r->t); return 0; } ratio = (speed * r->gap_t) / (max_speed * r->dt); if (ratio < max_gap_ratio) return (1); /* The refabs record is good. */ else { fprintf(fp_log, "\n Rejected refabs at time %f with ratio = %f", r->t, ratio); return 0; } } #if PROTOTYPE_ALLOWED int read_refabs(FILE *fp, REFABS_TYPE *r, int good_only) #else int read_refabs(fp, r, good_only) FILE *fp; REFABS_TYPE *r; int good_only; #endif { char buf[120]; do { if ( getline_nc(fp, buf, 120) == EOF) return 1; /* end of file */ if (sscanf(buf, "%lf %lf %lf %lf %lf %lf %lf %lf %lf", &r->t, &r->x, &r->y, &r->u, &r->v, &r->dt, &r->gap_u, &r->gap_v, &r->gap_t) != 9) return 1; /* error */ /* gap_t and dt are written in minutes, for easy interpretation. Convert to days for consistency with other times. */ r->gap_t /= min_per_day; r->dt /= min_per_day; } while (good_only && !good_ref(r)); return 0; /* normal exit */ } /*-------------------------------------------------------------- get_refabs This fills the avg_fix2fix array with lines from the refabs output file as needed to calculate the smoothed reference from t0 to t1. Returns the number of lines in avg_fix2fix that are needed. Errors: returns 0 if no references were found; NFMAX if the avg_fix2fix array was not big enough. */ #if PROTOTYPE_ALLOWED int get_refabs(FILE *fp, double *t0, double *t1) #else int get_refabs(fp, t0, t1) FILE *fp; double *t0, *t1; #endif { static int n; /* The pair of fixes bracketing the last time of the previous call become the first pair of fixes to test for bracketing of the first time of the present call. */ if (startup) { if (read_refabs(fp, &(avg_fix2fix[0]), GOOD_REFABS)) return 0; n = 1; startup = 0; } /* Look first in the existing array for the first fix-pair ending time that is after the first time, *t0. (Note: memmove is supposed to move the bytes correctly, while memcpy is not, since the source and destination ranges overlap.) */ while ( (n>1) && (avg_fix2fix[0].t + avg_fix2fix[0].dt) <= *t0 ) { memmove(&(avg_fix2fix[0]), &(avg_fix2fix[1]), (n-1)*sizeof(REFABS_TYPE)); n--; } /* Then search the fix file if necessary. */ while ( ((avg_fix2fix[0].t + avg_fix2fix[0].dt) <= *t0) ) { if (read_refabs(fp, &(avg_fix2fix[0]), GOOD_REFABS)) return (0); } /* If there is a gap between fix intervals, and *t0 falls in the gap, move *t0 to the start of the following fix interval (which was just located) */ *t0 = max_val(*t0, avg_fix2fix[0].t); /* Now the first time is bracketed by avg_fix2fix[0]. Bracket *t1. */ while ( (avg_fix2fix[n-1].t + avg_fix2fix[n-1].dt < *t1) && (n < NFMAX)) { if (read_refabs(fp, &(avg_fix2fix[n]), GOOD_REFABS)) { /* *t1 can be no greater than the last fix time */ double t_end; t_end = avg_fix2fix[n-1].t + avg_fix2fix[n-1].dt; *t1 = min_val(*t1, t_end); return (n); /* end of file exit */ } n++; } if (n >= NFMAX) /* n>NFMAX should not be possible, but check anyway. */ { printf("\n Warning: out of space in avg_fix2fix array with %d elements.\n", n); return NFMAX; /* error */ } /* If *t1 falls in a gap, move it back to the end of the previous interval. */ if (*t1 <= avg_fix2fix[n-1].t) { if (n <= 1) return 0; *t1 = avg_fix2fix[n-2].t + avg_fix2fix[n-2].dt; return (n-1); } else return n; } /* get_refabs() */ #if 0 /* For simplicity, the following is using a parabolic filter. If w is the half width and t is time, the filter function is f = w^2 - t^2 The integral from t0 to t1 is w^2(t1-t0) - 1/3 (t1^3 - t0^3) Assume the input time range has been scaled by the half width, so it is in the region -1 to 1. */ #if PROTOTYPE_ALLOWED double integrate_filter(double t0, double t1) #else double integrate_filter(t0, t1) double t0, t1; #endif { /* make sure the integration is within the filter range */ t0 = max_val(t0, -1.0); t1 = min_val(t1, 1.0); return ( (t1 - t0) - (1/3) * (t1*t1*t1 - t0*t0*t0) ); } #else /* Blackman filter, symmetric about t=0. f = 0.42 + 0.5 cos(pi t/h) + 0.08 cos(2 pi t/h) where h is the half-width. The integral is 0.42 t + 0.5 h/pi sin(pi t/h) + 0.04 h/pi sin(2 pi t/h) Here, the input time range is assumed scaled by the half width, so that the filter range is -1 to 1. */ #if PROTOTYPE_ALLOWED double integrate_filter(double t0, double t1) #else double integrate_filter(t0, t1) double t0, t1; #endif { /* make sure the integration is within the filter range */ t0 = max_val(t0, -1.0); t1 = min_val(t1, 1.0); return ( 0.42 * (t1 - t0) + (0.5 / M_PI) * (sin(M_PI * t1) - sin(M_PI * t0)) + (0.04 / M_PI) * (sin(2 * M_PI * t1) - sin(2 * M_PI * t0)) ); } /* integrate_filter */ #endif static double whole_integral = 0.0; /* must be static global so it can be initialized. */ #if PROTOTYPE_ALLOWED int filter_refabs(double t, double t0, double t1, REFABS_TYPE *r, VELOCITY_TYPE *vel, double *filt_fract) #else int filter_refabs(t, t0, t1, r, vel, filt_fract) double t, t0, t1; REFABS_TYPE *r; VELOCITY_TYPE *vel; double *filt_fract; #endif { extern double filter_hwidth; extern int n_fix2fix; int i; VELOCITY_TYPE integral; double integrated_filter; double tt0, tt1; /* unscaled range */ double ft0, ft1; /* scaled range for filtering */ if (whole_integral == 0.0) whole_integral = integrate_filter(-1.0, 1.0); /* Initialize the integral accumulators. */ integral.u = 0.0; integral.v = 0.0; integral.t = 0.0; /* Loop through all the fix-to-fix intervals. */ for (i=0; i min_filt_fract) { vel->u = integral.u / integral.t; vel->v = integral.v / integral.t; return 0; /* normal exit */ } else return 1; /* error; not enough data within the filter span */ } /* filter_refabs() */ #if PROTOTYPE_ALLOWED void do_it(FILE *fp_cnt) #else void do_it(fp_cnt) FILE *fp_cnt; #endif { VELOCITY_TYPE ship_ref, /* ship rel to ref layer, from ref file */ prev_ship_ref, /* previous ship rel to ref layer */ sm_ref, /* smoothed abs. ref layer */ sm_ship; /* consequent ship velocity */ POSITION_TYPE pos; /* est. position of current profile */ double t0, t1; double profile_dt = 0.0; /* ensemble length, seconds */ double profile_middle_t; /* dd time of middle of profile */ double filt_fract; /* data as fraction of filter mass */ int first = 1; int i; long nsmrecords = 0; SMOOTHR_OUT_TYPE record; prev_ship_ref.t = prev_ship_ref.u = prev_ship_ref.v = 0.0; check_error(get_parameters(fp_cnt, smoothr_params, NULL), "getting smoothr parameters"); i = 0; while ( (fn_out[i] != '.') && fn_out[i]) fn_log[i++] = fn_out[i]; strcat(fn_log, ".log"); i = 0; while ( (fn_out[i] != '.') && fn_out[i]) fn_bin[i++] = fn_out[i]; strcat(fn_bin, ".bin"); fp_log = check_fopen(fn_log, "w"); fp_ref = check_fopen(fn_ref, "r"); fp_fix2fix = check_fopen(fn_fix2fix, "r"); fp_out = check_fopen(fn_out, "w"); fp_smbin = check_fopen(fn_bin, "w+b"); #ifdef VBUF check_error( ( setvbuf(fp_fix2fix, NULL, _IOFBF, VBUFSIZE) || setvbuf(fp_smbin , NULL, _IOFBF, VBUFSIZE) ), "setvbuf"); #endif fprintf(fp_log, "\nADCP Navigation: SMOOTHR\n"); print_parameters(fp_log, smoothr_params, NULL, "\n"); /* printf("\nSMOOTHR profile times:\n"); ef- 95/12/05 */ /* Loop through all ensemble times, as recorded with the reference velocities (output from ADCPSECT, input to REFABS). */ while (!read_shipref(fp_ref, &ship_ref)) { /* ef- 95/12/05 ref_count++; printf(" %8.4f", ship_ref.t); if ( (ref_count % 8) == 0) printf("\n"); */ /* By making a smoothr record for every reference layer velocity, bad or not, we leave open the possibility of having PUTNAV reset the velocity and/or the position when the reference layer velocity is bad. It seems reasonable to require thet every program that produces an output line for each profile should not skip profiles under any circumstances. */ if (ship_ref.u >= ADJ_BADFLOAT) { make_bad_record(&record, &ship_ref); } else { /* Estimate the time of the middle of the ensemble. */ /* First, we have to estimate the duration of the ensemble. The assumption made here is that if the time since the last end of ensemble exceeds the input "ensemble_time", then there was a gap in data collection, and the ensemble_time will be used as the estimate of the actual ensemble duration. This is a decent approximation (given a good input value for ensemble_time). It is hard to imagine how to do much better, given that the ensemble start time is not recorded--a legacy of RDI's DAS 2.48 that could be fixed for datasets recorded with other systems. */ if (prev_ship_ref.t != 0.0) { profile_dt = (ship_ref.t - prev_ship_ref.t) * s_per_day; profile_dt = min_val(profile_dt, ensemble_time); } else { profile_dt = ensemble_time; } profile_middle_t = ship_ref.t - 0.5 * profile_dt / s_per_day; /* profile_middle_t = ship_ref.t; */ /* Set filter time range. */ t0 = profile_middle_t - filter_hwidth; t1 = profile_middle_t + filter_hwidth; /* Fill avg_fix2fix with records spanning the filter. The source is the output of REFABS. */ n_fix2fix = get_refabs(fp_fix2fix, &t0, &t1); /* printf("%f %f %d", t0, t1, n_fix2fix); */ /* (t0, t1 were adjusted inward if they fell in gaps) */ sm_ref.t = ship_ref.t; /* make_record uses this as the record time */ if ( (n_fix2fix == 0) /* No data at all in filter range */ || /* or, no usable data within filter range */ (filter_refabs(profile_middle_t, t0, t1, avg_fix2fix, &sm_ref, &filt_fract)) ) { make_bad_record(&record, &ship_ref); first = 1; } else /* There is usable data within filter range. */ { /* The final ("smoothed") estimate of ship velocity is the velocity of the ship relative to the reference layer, plus the smoothed estimate of the absolute reference layer velocity. */ sm_ship.u = ship_ref.u + sm_ref.u; sm_ship.v = ship_ref.v + sm_ref.v; if (first) { /* Approximate the first position in the sequence as the position of the first fix in avg_fix2fix. This will be within a half filter width of the true starting position. */ pos.x = avg_fix2fix[0].x; pos.y = avg_fix2fix[0].y; first = 0; } else { /* Integrate the ship velocity to the current time. */ /* The adjustment made above to profile_dt is equivalent to approximating the ship velocity as zero in the gaps. This is not a good approximation, but should be adequate at this stage. Better approximations are made in optimize_position, and it is the result of the optimization that is written to the output files. */ pos.x += meters_to_dlon( (sm_ship.u * profile_dt), pos.y); pos.x = lon_180(pos.x); pos.y += meters_to_dlat( (sm_ship.v * profile_dt), pos.y); } make_record(&record, &sm_ref, &sm_ship, &pos, filt_fract); } } /* Everything has been calculated for this ADCP ensemble; write it out. */ fwrite(&record, sizeof(record), 1, fp_smbin); nsmrecords++; /* Save the previous time for calculation of profile_dt. */ prev_ship_ref = ship_ref; } /* end of loop reading from reference file */ optimize_positions(fp_smbin, fp_fix2fix, fp_log, fix_to_dr_limit, iterations); /* Write out an ASCII version of the binary file. */ printf("\nWriting ASCII final output file, %s.\n", fn_out); rewind(fp_smbin); for (i=0; i= ADJ_BADFLOAT) fprintf(fp_out, "%12.6f %7.1e %7.1e %7.1e %7.1e %12.1e %12.1e %4.2f\n", record.t, BADFLOAT, BADFLOAT, BADFLOAT, BADFLOAT, BADFLOAT, BADFLOAT, 0.0); else fprintf(fp_out, "%12.6f %7.3f %7.3f %7.3f %7.3f %12.6f %12.6f %4.2f\n", record.t, record.ru, record.rv, record.su, record.sv, record.x, record.y, record.ff); } fclose(fp_ref); fclose(fp_fix2fix); fclose(fp_out); fclose(fp_smbin); fclose(fp_log); } /* do_it() */