From 22a2e242b9627615716bb106eee29b05cad6d713 Mon Sep 17 00:00:00 2001 From: David Berry Date: Fri, 7 Sep 2012 17:45:16 +0100 Subject: [PATCH] smurf: New version of POL-2 bonus POL_ANG fixing code The old version found lots of bonus points because it did not correct later data as soon as it found a bonus point and therefore got confused by the long RTS_END gap every 25th sample. The new version is different to the example python code written by Ryan in that it does not assume a constant rotation speed, and does not need to be told what the nominal rotation speed is. --- applications/smurf/libsmf/smf_fix_pol2.c | 447 ++++++++++++++++------- 1 file changed, 313 insertions(+), 134 deletions(-) diff --git a/applications/smurf/libsmf/smf_fix_pol2.c b/applications/smurf/libsmf/smf_fix_pol2.c index 61e987345e0..8d4edfcd542 100644 --- a/applications/smurf/libsmf/smf_fix_pol2.c +++ b/applications/smurf/libsmf/smf_fix_pol2.c @@ -29,17 +29,6 @@ * extra spurious value to be inserted into the POL_ANG array in the * JCMTSTATE extension, causing later valid values to appear to be * delayed. -* -* This function corrects for these by first finding candidate "bonus" -* POL_ANG values, and then seeing if the removal of such values would -* result in a reduction in the residuals between the (rts_end,pol_ang) -* points and the best fitting straight line. If so, the point is -* removed, and later POL_ANG values are shuffled down to fill the gap -* (a VAL__BADD value is pushed onto the end of the array). -* -* Candidate bonus points are identified by the fact that there is -* an unusually low angular speed in the time step leading up to the -* point. * Authors: * David Berry (JAC) @@ -48,6 +37,8 @@ * History: * 4-SEP_2012 (DSB): * Original version. +* 7-SEP-2012 (DSB): +* Complete re-write. * Copyright: * Copyright (C) 2012 Science and Technology Facilities Council. @@ -86,15 +77,9 @@ /* POL_ANG is given in integer units where 944000 is equivalent to 2*PI */ #define MAXANG 944000 -/* The length of the box over which a linear fit is performed to the - (rts_end,pol_ang) positions around each potential binus point. */ -#define BOX 50 - -/* The number of sigma below the mean angular speed at which POL_ANG - values are considered as candidate bonus points. */ -#define NSIGMA 10 - - +/* The half-width of the box over which a linear fit is done to the + POL_ANG values. */ +#define HALF_BOX 25 void smf_fix_pol2( ThrWorkForce *wf, smfArray *array, int *status ){ @@ -106,164 +91,359 @@ void smf_fix_pol2( ThrWorkForce *wf, smfArray *array, int *status ){ dim_t iframe; dim_t ijump; dim_t j; - dim_t jmin; + dim_t jtop; dim_t njump; double *angles; double *pa; - double *pc; - double *ps; + double *pcenx; + double *pceny; + double *pnewx; + double *pnewy; + double *pnewytop; + double *poldx; + double *poldy; double *pt; - double *speeds; double *times; double ang_change; double ang_offset; + double angle; double c; + double denom; + double langle; + double limit; + double lresid; double m; - double mean_speed; - double minspeed; + double pop; + double resid; double rts_origin; - double s1; - double s2; - double sigma; - double work[ 2*BOX ]; - int lbnd; - int ubnd; + double sx; + double sxx; + double sxy; + double sy; + double syy; + double time; smfHead *hdr; /* Check inherited status. */ if( *status != SAI__OK ) return; -/* Loop over subarray */ +/* Loop over subarray. They probably all shre the same header, but is it + guaranteed? */ for( idx = 0; idx < array->ndat && *status == SAI__OK; idx++ ) { hdr = array->sdata[ idx ]->hdr; -/* Store all the POL_ANG and RTS_END values in local arrays, and also - store the angular speed between each pair of values. Take accout of - the wrap-around in POL_ANG from MAXANG back to zero at the end of each - revolution. */ - pt = times = astMalloc( hdr->nframes*sizeof( *times ) ); - pa = angles = astMalloc( hdr->nframes*sizeof( *angles ) ); - ps = speeds = astMalloc( hdr->nframes*sizeof( *speeds ) ); + + +FILE *fd = fopen( "data.asc", "w" ); +fprintf( fd, "# iframe time angle\n" ); + + + + +/* Allocate work arrays to hold the offset from the start of the chunk in + days, and the rotation since the start of the chunk, in arbitrary encoder + units. */ + times = astMalloc( hdr->nframes*sizeof(*pt) ); + angles = astMalloc( hdr->nframes*sizeof(*pt) ); + +/* Loop round all frames. The "langle" variable is used to record the angle + for the previous frame. The "rts_origin" value is the RTS_END value at + the first valid frame - it is removed from every subsequent RTS_END + value in order to reduced the dynamic range of the time values. The + "ang_offset" value is added to the POL_ANG values in order to remove + the downard jumps from 360 degs to zero degs at the end of each + revolution. The "pa" and "pt" pointers point to the element of + "angles" and "times" to which the angle and time at the next Frame + should be written. */ + langle = VAL__BADD; rts_origin = VAL__BADD; - ang_offset = 0; + ang_offset = VAL__BADD; + + pa = angles; + pt = times; + state = hdr->allState; - for( iframe = 0; iframe < hdr->nframes; iframe++,state++,pa++,pt++,ps++ ) { - if( rts_origin == VAL__BADD ) rts_origin = state->rts_end; + for( iframe = 0; iframe < hdr->nframes; iframe++,state++,pa++,pt++ ) { + +/* Not sure if there ever will be bad RTS_END or POL_ANG values, but just + in case... */ if( state->rts_end != VAL__BADD && state->pol_ang != VAL__BADD ) { - *pt = state->rts_end - rts_origin; - *pa = state->pol_ang + ang_offset; - if( iframe > 0 && pa[-1] != VAL__BADD ) { - ang_change = ( pa[0] - pa[-1] ); +/* If this is the first valid frame, record the initial RTS_END and + POL_ANG values so that "time" and "angle" values are zero for the + first frame. */ + if( rts_origin == VAL__BADD ) rts_origin = state->rts_end; + if( ang_offset == VAL__BADD ) ang_offset = -state->pol_ang; + +/* Store the time since the first valid frame, in days */ + time = state->rts_end - rts_origin; + +/* Store the POL_ANG rotation since the first valid frame, in arbitrary + encoder counts ( MAXANG corresponds to 360 degs). */ + angle = state->pol_ang + ang_offset; + +/* If the previous frame was also valid, find the increment in POL_ANG. */ + if( langle != VAL__BADD ) { + ang_change = angle - langle; + +/* If there has been a hugedrop in angle, we must have passed through a + 360->zero degs discontinuity. Increase the angle offset by 360 degs to + remove this discontinity, and adjust the current angle and angle + increment to use this new angle offset. */ if( ang_change < -MAXANG/2 ) { ang_offset += MAXANG; - pa[0] += MAXANG; + angle += MAXANG; ang_change += MAXANG; } - *ps = ang_change/( pt[0] - pt[-1] ); - } else { - *ps = VAL__BADD; } + +/* The angle, time and speed are unknown if the current frame is not valid. */ } else { - *pt = VAL__BADD; - *pa = VAL__BADD; - *ps = VAL__BADD; + angle = VAL__BADD; + time = VAL__BADD; + } + +/* Store the final time and angle in the work arrays. */ + *pt = time; + *pa = angle; + + + + + + fprintf( fd, "%d ", (int) iframe ); + if( times[iframe] != VAL__BADD ) { fprintf( fd, "%.*g ", DBL_DIG, times[iframe] ); } else { fprintf( fd, "null " ); } + if( angles[iframe] != VAL__BADD ) { fprintf( fd, "%.*g ", DBL_DIG, angles[iframe] ); } else { fprintf( fd, "null " ); } + fprintf( fd, "\n" ); + + + + + + + +/* Record the angle of the current frame for use on the next pass round this + loop. */ + langle = angle; + } + + + + + + + +fclose( fd ); + + + + + + +fd = fopen( "data2.asc", "w" ); +fprintf( fd, "# iframe time angle resid limit\n" ); + + + + + + +/* Now we have the angles and time, we step through the array, and at + each frame perform a least squares linear fit to produce the gradient of + offset of the line through a box of values centred on the current frame. + We find the residual between this line and the POL_ANG value at the + central time. As a bonus point is approached, the residual will get + more and more negative until it suddenly switches sign at the bonus + point itself. We then remove the bonus point by shuffling down later + POL_ANG values to replace it, and continue to look for the next bonus + point. The least squares fitting is done by maining the required + statistics for a box of values centred on the required frame. As we + move to a new frame, a new point is added to the statistics, and the + oldest point is removed.. + + First initialise the required running sums to hold the sums of the data + in the first HALF_BOX+1 data values. The independent variable in the fit + is time. This means the "x*y" product summed in "sxy" does not depend + on the current centre of the box, which is good. */ + sy = 0.0; + sx = 0.0; + sxy = 0.0; + sxx = 0.0; + syy = 0.0; + pop = 0; + + pnewy = angles; + pnewx = times; + for( iframe = 0; iframe <= HALF_BOX; iframe++,pnewy++,pnewx++ ) { + if( *pnewy != VAL__BADD && *pnewx != VAL__BADD ) { + sy += *pnewy; + sx += *pnewx; + sxy += ( *pnewx )*( *pnewy ); + sxx += ( *pnewx )*( *pnewx ); + syy += ( *pnewy )*( *pnewy ); + pop++; } } -/* Work out the sigma clipped mean speed. This should ignore the frames that - involve one of the extra "bonus" POL_ANG values. */ - mean_speed = smf_sigmaclip( hdr->nframes, speeds, NULL, 2.0, 3, &sigma, - status ); +/* The "pnewy" and "pnewx" pointers now point to the input values that are + about to be added to the fit box. Initialise another pair of pointers + "poldx" and "poldy" to point to the input values that are about to leave + the fit box. These will initially point to elements before the start of + the input arrays. */ + poldy = angles - HALF_BOX; + poldx = times - HALF_BOX; + +/* Initialise another pair of pointers to point to the central values + in the fit box. */ + pceny = angles; + pcenx = times; -/* Now go through the values again looking for samples that have an - unusually low speed (i.e. more than NSIGMA standard deviations below the - mean speed). */ +/* Store a pointer to the fist value after the end of the angles array. */ + pnewytop = angles + hdr->nframes; + +/* Initialise the list of POL_ANG values to remove. */ jumps = NULL; njump = 0; - pt = times; - pa = angles; - ps = speeds; - for( iframe = 0; iframe < hdr->nframes; iframe++,pa++,pt++,ps++ ) { - if( *ps != VAL__BADD ) { - if( *ps < mean_speed - NSIGMA*sigma ) { - -/* Find the upper and lower bounds of a small box centered on the current - candidate bonus point. */ - lbnd = iframe - BOX/2; - if( lbnd < 0 ) lbnd = 0; - ubnd = iframe + BOX/2; - if( ubnd >= (int) hdr->nframes ) ubnd = hdr->nframes - 1; - -/* Find the point with the lowest speed in this box. We do not need to - check the lower half of the box as this will have been done on previous - passes through the "iframe" loop. */ - minspeed = *ps; - jmin = iframe; - for( j = iframe + 1; j < (dim_t) ubnd; j++ ) { - if( speeds[ j ] != VAL__BADD && speeds[ j ] < minspeed ) { - minspeed = speeds[ j ]; - jmin = j; + +/* Do each frame in turn. */ + lresid = VAL__BADD; + for( iframe = 0; iframe < hdr->nframes; iframe++,poldx++,pcenx++,pnewx++,poldy++,pceny++,pnewy++ ) { + resid = VAL__BADD; + +/* Calculate the gradient and offset of the least squares fit to the data + currently in the fittting box. The offset here ("c") is the value of the + fitted line at time == 0*/ + denom = pop*sxx - sx*sx; + if( denom > 0 && pop > 0 ) { + m = ( pop*sxy - sx*sy )/denom; + c = ( sxx*sy - sx*sxy )/denom; + +/* Correct the offset to be the value of the line at the centre of the + box. All this least square sfitting would be unnecessary if we knew + that the mean time in the box was equal to the central time. If that + were the case we could use the mean angle in the box as "c", but alas + it's not the case. */ + c += m*( *pcenx ); + +/* Get the residual between the central fit value and the central angle + value. */ + if( *pceny != VAL__BADD ) { + resid = c - *pceny; + +/* If the previous residual was large and negative, and the current + residual is large and positive, then we have found a bonus point. + Here, "large" is defined as more than 25% of the angular step since the + last sample. */ + if( pceny[ -1 ] != VAL__BADD ) { + limit = 0.25*( pceny[0] - pceny[-1] ); + if( lresid != VAL__BADD && lresid < -limit && resid > limit ) { + + + + + + +fprintf( fd, "%d %d %.*g %.*g %.*g\n", njump + 1, iframe + njump, DBL_DIG, + lresid, DBL_DIG, resid, DBL_DIG, limit ); + + + +/* Remove the central value and the upper half of the box from the stats. */ + jtop = iframe + HALF_BOX + 1; + if( jtop > hdr->nframes ) jtop = hdr->nframes; + for( j = iframe; j < jtop; j++ ) { + if( angles[ j ] != VAL__BADD && times[ j ] != VAL__BADD ) { + sy -= angles[ j ]; + sx -= times[ j ]; + sxy -= times[ j ]*angles[ j ]; + sxx -= times[ j ]*times[ j ]; + syy -= angles[ j ]*angles[ j ]; + pop--; + } + } + +/* Shuffle down the remaining angle values to replace the hole left by + removing the central value. Pad the end with a blank value. */ + for( j = iframe + 1; j < hdr->nframes; j++ ) { + angles[ j - 1 ] = angles[ j ]; + } + angles[ j - 1 ] = VAL__BADD; + +/* Add back the new central value and the upper half of the box into the + stats. */ + for( j = iframe; j < jtop; j++ ) { + if( angles[ j ] != VAL__BADD && times[ j ] != VAL__BADD ) { + sy += angles[ j ]; + sx += times[ j ]; + sxy += times[ j ]*angles[ j ]; + sxx += times[ j ]*times[ j ]; + syy += angles[ j ]*angles[ j ]; + pop++; + } + } + +/* Ensure we use a bad value for "lresid" next time. */ + resid = VAL__BADD; + +/* Add the *original* index of the current frame (i.e. assuming no points + have been removed) to the list of POL_ANG values to remove. */ + jumps = astGrow( jumps, ++njump, sizeof( *jumps ) ); + jumps[ njump - 1 ] = iframe + njump - 1; } } + } + } -/* Move on so we are centred on the lowest speed in the box. */ - if( jmin != iframe ) { - iframe = jmin; - pa = angles + iframe; - pt = times + iframe; - ps = speeds + iframe; - lbnd = iframe - BOX/2; - if( lbnd < 0 ) lbnd = 0; - ubnd = iframe + BOX/2; - if( ubnd >= (int) hdr->nframes ) ubnd = hdr->nframes - 1; - } +/* If there will be another pass through the "iframe" loop... */ + if( iframe < hdr->nframes ) { -/* Do a least squares linear fit to the (time,angle) points in the box, and - note the RMS residuals ("s1"). */ - lbnd = iframe - BOX/2; - if( lbnd < 0 ) lbnd = 0; - ubnd = iframe + BOX/2; - if( ubnd >= (int) hdr->nframes ) ubnd = hdr->nframes - 1; - kpg1Fit1d( lbnd, ubnd, angles + lbnd, times + lbnd, &m, &c, - &s1, status ); - -/* Now try removing the current point to see if that produces a better - linear fit. Copy the angle (except the candidate bonus point) into the - work array. */ - pc = work; - for( j = lbnd; j < iframe; j++ ) { - *(pc++) = angles[ j ]; - } - for( j = iframe + 1; j < (dim_t) ubnd; j++ ) { - *(pc++) = angles[ j ]; - } +/* Record the residual */ + lresid = resid; -/* Now do the fit. The RMS of the residuals is put into "s2". */ - kpg1Fit1d( lbnd, ubnd, work, times + lbnd, &m, &c, &s2, - status ); - -/* Tell the user about the test if in debug mode. */ - msgOutiff( MSG__DEBUG, "", "smf_fix_pol2: Testing " - "POL_ANG[%d]: RMS with: %g without: %g", status, - (int) iframe, s1, s2 ); - -/* If the linear fit is significantly improved by removing the current - candidate bonus point, add the current sample index to the list of steps - to be removed. */ - if( s2 < 0.95*s1 ) { - jumps = astGrow( jumps, ++njump, sizeof( *jumps ) ); - jumps[ njump - 1 ] = iframe; - } +/* Add the next input value into the running sums. */ + if( pnewy < pnewytop && *pnewy != VAL__BADD && *pnewx != VAL__BADD ) { + sy += *pnewy; + sx += *pnewx; + sxy += ( *pnewx )*( *pnewy ); + sxx += ( *pnewx )*( *pnewx ); + syy += ( *pnewy )*( *pnewy ); + pop++; + } + +/* Remove the oldest input value from the running sums. There is nothing + to add in on the last pass through the "i" loop. */ + if( poldy >= angles && *poldy != VAL__BADD && *poldx != VAL__BADD ) { + sy -= *poldy; + sx -= *poldx; + sxy -= ( *poldx )*( *poldy ); + sxx -= ( *poldx )*( *poldx ); + syy -= ( *poldy )*( *poldy ); + pop--; } } } + + + + + + + + +fclose( fd ); + + + + + + /* Now shuffle the POL_ANG values down to remove the accepted bonus values. */ if( njump > 0 ) { state = wstate = hdr->allState; ijump = 0; - for( iframe = 0; iframe < hdr->nframes; iframe++,state++ ) { + for( iframe = 0; iframe < hdr->nframes && ijump < njump; + iframe++,state++ ) { if( iframe < jumps[ ijump ] ) { (wstate++)->pol_ang = state->pol_ang; } else { @@ -277,11 +457,10 @@ void smf_fix_pol2( ThrWorkForce *wf, smfArray *array, int *status ){ while( wstate < state ) (wstate++)->pol_ang = VAL__BADD; } -/* Release temporary work space */ +/* Free work space. */ jumps = astFree( jumps ); times = astFree( times ); - angles = astFree( angles ); - speeds = astFree( speeds ); + angles = astFree( times ); } }