1
\$\begingroup\$

I would like to reduce the complexity of this calculate method as it is too long in my opinion. There is already a number of helper methods exist which are called throughout the method. I can't really see any other alternative than adding more helper methods. Is there perhaps a way to split the for loop out into a separate class? It is a work in progress.

public class StarTrackerAttitudeCalculator { 
    private static final Logger LOGGER =     Logger.getLogger(StarTrackerAttitudeCalculator.class.getName()); 


    private static final String RECORD_TIME_COLUMN_NAME = "recordTime"; 

    private static final int FOUR_HERTZ = 4; 

    private static final double RAD_TO_ARCSEC = 6.48e5 / Math.PI; 

    private final SpacecraftVelocity velocity; 
    private final CalibratedValues calibratedValues; 

    public StarTrackerAttitudeCalculator(SpacecraftVelocity velocity, CalibratedValues calibratedValues) { 
        this.velocity = velocity; 
        this.calibratedValues = calibratedValues; 
    } 

    /** 
     * Calculate the corrected attitude measurements 
     * 
     * @param pp The PointingProduct 
     * @param acms The AcmsTelemetryProduct 
     * @param probThresh The probability threshold 
     * @throws AuxException 
     */ 
    public void calculate(PointingProduct pp, AcmsTelemetryProduct acms, double probThresh) throws AuxException { 
        //Extract main 4Hz ACMS dataset from acmsProduct 
        AcmsScmTMDataset acmsScmTM = acms.getAcmsScmTMDataset(); 
        //Extract STR-specific diagnostics dataset from acmsProduct 
        AcmsDtmStrDataset acmsDtmStr = acms.getAcmsDtmStrDataset(); 

        //If no STR-specific diagnostics dataset, raise an exception - see HCSS-19234 
        if (acmsDtmStr.getRowCount() == 0) { 
            throw new AuxException("No STR-specific diagnostics dataset found in the ACMS product - " 
                    + "using on-board filtered attitude as absolute reference."); 
        } 

        //Number of entries in STR-specific diagnostics dataset 
        int n1hz = acmsDtmStr.getRowCount(); 

        //q is holding the STR quaternion corresponding to the 1Hz samples 
        Double2d q = newDouble2d(n1hz, FOUR_HERTZ); 
        //estq contains the on-board filtered AKA quaternions at 1Hz 
        Double2d estq = newDouble2d(n1hz, FOUR_HERTZ); 
        Long1d acmsScm1hzRecordTime = null; 

        //If four times as many entries in the main ACMS dataset 
        int size = getData(acmsScmTM, RECORD_TIME_COLUMN_NAME).getSize(); 
        if (size == FOUR_HERTZ * n1hz) { 
            Long2d recordTime = (Long2d) reshapeColumn(acmsScmTM, RECORD_TIME_COLUMN_NAME, n1hz, FOUR_HERTZ); 
            acmsScm1hzRecordTime = sliceLong2d(recordTime, 1); 

            setSTRQuaternion(acmsScmTM, n1hz, q); 
            setOnBoardFilteredAKAQuaternions(acmsScmTM, n1hz, estq); 
        } else { 
            //For those observations where this is not the case (HCSS-18039 & HCSS-19454) 
            //Find the closest 4Hz entry and fill the arrays 
            LOGGER.warning("1Hz and 4Hz diagnostic ACMS do not match - finding matching telemetry will take some time"); 
            acmsScm1hzRecordTime = (Long1d) getData(acmsDtmStr, RECORD_TIME_COLUMN_NAME); 
            findClosest4HzEntryAndFillQuaternions(acmsScmTM, acmsDtmStr, n1hz, q, estq); 
        } 

        final int rowCount = acmsDtmStr.getRowCount(); 

        //Catalogue RA and DEC of the stars 
        Double2d starRa = createCatalogueRAForStars(acmsDtmStr, rowCount); 
        Double2d starDec = createCatalogueDECForStars(acmsDtmStr, rowCount); 

        //Determine ACMS housekeeping packets covering the time in the old pointing product 
        Long1d recordTimes = (Long1d)getData(acmsDtmStr, RECORD_TIME_COLUMN_NAME); 
        Selection recordTimeSelection = recordTimes.where(recordTimes.gt(pp.getStartTime()).and( 
                recordTimes.le(pp.getEndTime()))); 
        Int1d indices = recordTimeSelection.toInt1d(); 
        int numRows = indices.getSize(); 

        //Output table: Table dataset covering the timespan of the old pointing product 
        //We apply the time offset of STR attitudes here (HCSS-18371) 
        Long1d obts = acmsScm1hzRecordTime.copy().get(recordTimeSelection) 
.subtract(StarTrackerCalibrationProperties.TIME_OFFSET); 
        CorrectedAttitudeDataset correctedAttitude = new CorrectedAttitudeDataset(numRows, obts); 


        double sumTaste = 0.0; 
        int sumStars = 0; 

        //The star IDs used at every STR telemetry entry 
        Long2d starIds = createStarIdsForSTRTelem(acmsDtmStr, rowCount); 
        //Array to hold the spacecraft velocity at the time of the STR telemetry 
        Double2d sv = createSpacecraftVelocityForSTRTelem(rowCount); 
        //Array to hold the measured unit vectors of the 9 pointing stars in the STR telemetry 
        Double2d vmeas = newDouble2d(9, 3); 


        //Loop over every diagnostic ACMS packet within timespan of pointing product 
        for(int j=0; j<numRows; j++){ 
            int jj = indices.get(j); 

            //Filtered quaternion 
            Quaternion estQuat = new Quaternion(estq.get(jj,0), estq.get(jj,1), estq.get(jj,2), estq.get(jj,3)); 

            //Star IDs 
            Long1d starsIdsForRange = starIds.get(new Range(0,starIds.getDimension(0)), jj); 

            //Transform velocity vector to BRF using STR quaternion as determined on-board 
            Quaternion strQuatOb = new Quaternion(q.get(jj,0), q.get(jj,1), q.get(jj,2), q.get(jj,3)); 
            Vector3 svBrf = strQuatOb.rotateAxes(new Vector3(sv.get(0,jj), sv.get(1,jj), sv.get(2,jj))); 

            //Complete the unit vector of the measured star positions (BRF) from the STR TM 
            fillVmeas(vmeas, acmsDtmStr, jj); 

            //Variables used for attitude determination 
            Double2d stars_brf_old = newDouble2d(9,3); 
            Double2d stars_brf_new = newDouble2d(9,3); 
            Double2d stars_meq     = newDouble2d(9,3); 
            int num_stars     = 0; 


            for(int star=0; star<9; star++){ 
                //Check there is a star and that it is not in the bad star list 
                long starId = starsIdsForRange.get(star); 

                //TODO replace call with contains method in ArrayUtil if added 
                if ((Arrays.asList(StarTrackerCalibrationProperties.BAD_STARS).contains(starId)) || (starId > 4000) 
                        || (starId == 0)) { 
                    LOGGER.warning("Bad star detected for id: " + starId); 
                } else { 
                    //Extract measured star vector (with aberration correction) 
                    Vector3 starvecAberOld = new Vector3(vmeas.get(star, 0), vmeas.get(star, 1), vmeas.get(star, 2)); 

                    //Calculate focal length for star (now average is used) 
                    double focalLength = calculateFocalLength(starIds.get(star, jj), acmsDtmStr, rowCount, star, jj); 

                    //Remove aberration correction 
                    Vector3 starvec = starvecAberOld.subtract(starvecAberOld.cross(starvecAberOld.cross(svBrf)) 
                            .multiply(1.0 / StarTrackerCalibrationProperties.C)); 

                    //Calculate distortion-corrected CCD coords of star (mm) 
                    double yP =  focalLength * starvec.get(1) / starvec.get(0); 
                    double zP =  focalLength * starvec.get(2) / starvec.get(0); 

                    //Remove the distortion-correction which was applied on-board 
                    double y = removeDistortion(calibratedValues.getKinv(), yP, zP); 
                    double z = removeDistortion(calibratedValues.getHinv(), zP, yP); //deliberately swap zP & yP 

                    //Calculate indices into residual distortion arrays of fitted star centre 
                    //int yyy = applyFix(y).intValue(); 
                    //int zzz = applyFix(z).intValue(); 
                    //Calculate indices into residual distortion arrays of fitted star centre 
                    Int1d regionyyy = calculateSubpixelDistortion(applyFix(y).intValue()); 
                    Int1d regionzzz = calculateSubpixelDistortion(applyFix(z).intValue()); 

                    //TODO - more calculations to follow

                } 




            } 

        } 

    } 

    /** 
     * Use periodicity of 10 (= 1 pixel) to be less susceptible to local errors 
     * we calculate the median subpixel distortion in the 11x11 pixels around 
     * the fitted star center, excluding the pixels outside the active 
     * region (where the distortion map is 0.) 
     */ 
    private Int1d calculateSubpixelDistortion(int value) { 
        return Int1d.range(11).subtract(5).multiply(10).add(value); 
    } 

    /**Calculate indices into residual distortion arrays of fitted star centre**/ 
    private Long applyFix(double value) { 
        return ((Long1d ) Basic.FIX.of(new Double1d(1, (value + 256.0 * StarTrackerCalibrationProperties.PY) 
                / StarTrackerCalibrationProperties.DPIX))).get(0); 
    } 

    /** Remove the distortion-correction which was applied on-board**/ 
    private double removeDistortion(double[] array, double yP, double zP) { 
        final Pow pow = new Pow(2.0); 
        final double neg = -array[0]; 
        final double sumYPZP = pow.calc(yP) + pow.calc(zP); 

        return neg + array[1] * yP + array[2] * zP + array[3] * yP * (sumYPZP) + array[4] * yP * (pow.calc(sumYPZP)) 
                - array[5] * pow.calc(yP) - array[6] * yP * zP - array[7] * pow.calc(zP); 
    } 

    /** Calculate focal length for star (now average is used)**/ 
    private double calculateFocalLength(long value, AcmsDtmStrDataset acmsDtmStr, int rowCount, int starIndex, 
            int packetIndex) { 
        //Apply alphaC correction of the stars - only available in hcss 11 products and later 
        Double2d starAlphaC = applyAlphaCCorrectionForStars(acmsDtmStr, rowCount); 
        if (value < 3600) { 
            return getFocalLength(starIndex, packetIndex, starAlphaC); 
        } 
        return calibratedValues.getF(); 
    } 

    /** Get the focal length for star and packet**/ 
    private double getFocalLength(int starIndex, int packetIndex, Double2d starAlphaC) { 
        return calibratedValues.getF() 
                * (1. + StarTrackerCalibrationProperties.ALPHA_T 
                        * (StarTrackerCalibrationProperties.T - StarTrackerCalibrationProperties.T0) + starAlphaC.get( 
                        starIndex, packetIndex)); 
    } 

    private void fillVmeas(Double2d vmeas, AcmsDtmStrDataset acmsDtmStr, int index) { 
        final Pow pow = new Pow(2.0); 
        for (int i = 0; i < vmeas.getDimension(0); i++) { 
            double strPosY = getDataItem(acmsDtmStr, "str0" + (i + 1) + "PosY", index); 
            double strPosZ = getDataItem(acmsDtmStr, "str0" + (i + 1) + "PosZ", index); 

            double sum = Basic.SQRT.calc(1.0 - pow.calc(strPosY) - pow.calc(strPosZ)); 
            vmeas.set(i, new Double1d(new double[] { sum, strPosY, strPosZ })); 
        } 

    } 

    private Double2d applyAlphaCCorrectionForStars(AcmsDtmStrDataset acmsDtmStr, final int rowCount) { 
        if (acmsDtmStr.getColumnNames().contains("alpha_c_01")) { 
            Double2d starAlphaC = newDouble2d(9, rowCount); 
            starAlphaC.set(0, (Double1d) getData(acmsDtmStr, "alpha_c_01")); 
            starAlphaC.set(1, (Double1d) getData(acmsDtmStr, "alpha_c_02")); 
            starAlphaC.set(2, (Double1d) getData(acmsDtmStr, "alpha_c_03")); 
            starAlphaC.set(3, (Double1d) getData(acmsDtmStr, "alpha_c_04")); 
            starAlphaC.set(4, (Double1d) getData(acmsDtmStr, "alpha_c_05")); 
            starAlphaC.set(5, (Double1d) getData(acmsDtmStr, "alpha_c_06")); 
            starAlphaC.set(6, (Double1d) getData(acmsDtmStr, "alpha_c_07")); 
            starAlphaC.set(7, (Double1d) getData(acmsDtmStr, "alpha_c_08")); 
            starAlphaC.set(8, (Double1d) getData(acmsDtmStr, "alpha_c_09")); 
            return starAlphaC; 
        } 
        LOGGER.warning("No alpha C for each star in ACMS product - using mean value"); 
        return new Double2d(9, rowCount, StarTrackerCalibrationProperties.ALPHA_C); 
    } 

    private Double2d createCatalogueDECForStars(AcmsDtmStrDataset acmsDtmStr, int rowCount) { 
        Double2d starDec = newDouble2d(9, rowCount); 
        starDec.set(0, (Double1d) getData(acmsDtmStr, "str01Dec")); 
        starDec.set(1, (Double1d) getData(acmsDtmStr, "str02Dec")); 
        starDec.set(2, (Double1d) getData(acmsDtmStr, "str03Dec")); 
        starDec.set(3, (Double1d) getData(acmsDtmStr, "str04Dec")); 
        starDec.set(4, (Double1d) getData(acmsDtmStr, "str05Dec")); 
        starDec.set(5, (Double1d) getData(acmsDtmStr, "str06Dec")); 
        starDec.set(6, (Double1d) getData(acmsDtmStr, "str07Dec")); 
        starDec.set(7, (Double1d) getData(acmsDtmStr, "str08Dec")); 
        starDec.set(8, (Double1d) getData(acmsDtmStr, "str09Dec")); 
        return starDec; 
    } 

    private Double2d createCatalogueRAForStars(AcmsDtmStrDataset acmsDtmStr, int rowCount) { 
        Double2d starRa = newDouble2d(9, rowCount); 
        starRa.set(0, (Double1d) getData(acmsDtmStr, "str01Ra")); 
        starRa.set(1, (Double1d) getData(acmsDtmStr, "str02Ra")); 
        starRa.set(2, (Double1d) getData(acmsDtmStr, "str03Ra")); 
        starRa.set(3, (Double1d) getData(acmsDtmStr, "str04Ra")); 
        starRa.set(4, (Double1d) getData(acmsDtmStr, "str05Ra")); 
        starRa.set(5, (Double1d) getData(acmsDtmStr, "str06Ra")); 
        starRa.set(6, (Double1d) getData(acmsDtmStr, "str07Ra")); 
        starRa.set(7, (Double1d) getData(acmsDtmStr, "str08Ra")); 
        starRa.set(8, (Double1d) getData(acmsDtmStr, "str09Ra")); 
        return starRa; 
    } 

    private Long2d createStarIdsForSTRTelem(AcmsDtmStrDataset acmsDtmStr, final int rowCount) { 
        Long2d starId = newLong2d(9, rowCount); 
        starId.set(0, (Long1d) getData(acmsDtmStr, "str01catId")); 
        starId.set(1, (Long1d) getData(acmsDtmStr, "str02catId")); 
        starId.set(2, (Long1d) getData(acmsDtmStr, "str03catId")); 
        starId.set(3, (Long1d) getData(acmsDtmStr, "str04catId")); 
        starId.set(4, (Long1d) getData(acmsDtmStr, "str05catId")); 
        starId.set(5, (Long1d) getData(acmsDtmStr, "str06catId")); 
        starId.set(6, (Long1d) getData(acmsDtmStr, "str07catId")); 
        starId.set(7, (Long1d) getData(acmsDtmStr, "str08catId")); 
        starId.set(8, (Long1d) getData(acmsDtmStr, "str09catId")); 
        return starId; 
    } 

    private Double2d createSpacecraftVelocityForSTRTelem(final int rowCount) { 
        Double2d sv = newDouble2d(3,  rowCount); 
        sv.set(0, new Range(0, sv.getDimension(1)), velocity.getStrVelX()); 
        sv.set(1, new Range(0, sv.getDimension(1)), velocity.getStrVelY()); 
        sv.set(2, new Range(0, sv.getDimension(1)), velocity.getStrVelZ()); 
        return sv; 
    } 

    private void findClosest4HzEntryAndFillQuaternions(AcmsScmTMDataset acmsScmTM, AcmsDtmStrDataset acmsDtmStr, 
            int n1hz, Double2d q, Double2d estq) { 
        Long1d obt4hz = (Long1d) getData(acmsScmTM, RECORD_TIME_COLUMN_NAME); 
        Long1d obt1hz = (Long1d) getData(acmsDtmStr, RECORD_TIME_COLUMN_NAME); 

        for (int i = 0; i < n1hz; i++) { 
            Long1d obt4hzSubtracted = obt4hz.copy().subtract(obt1hz.get(i)); 
            Long1d absolute = (Long1d) obt4hzSubtracted.apply(Basic.ABS); 
            long min = (long) absolute.apply(Basic.MIN); 

            Bool1d mask = absolute.eq(min); 
            int w = obt4hz.where(mask).toInt1d().get(0); 
            q.set(i, 0, ((Double1d) getData(acmsScmTM, "strmAttQ1")).get(w)); 
            q.set(i, 1, ((Double1d) getData(acmsScmTM, "strmAttQ2")).get(w)); 
            q.set(i, 2, ((Double1d) getData(acmsScmTM, "strmAttQ3")).get(w)); 
            q.set(i, 3, ((Double1d) getData(acmsScmTM, "strmAttQ4")).get(w)); 

            estq.set(i, 0, ((Double1d) getData(acmsScmTM, "estAttQ1")).get(w)); 
            estq.set(i, 1, ((Double1d) getData(acmsScmTM, "estAttQ2")).get(w)); 
            estq.set(i, 2, ((Double1d) getData(acmsScmTM, "estAttQ3")).get(w)); 
            estq.set(i, 3, ((Double1d) getData(acmsScmTM, "estAttQ4")).get(w)); 
        } 
    } 

    private void setOnBoardFilteredAKAQuaternions(AcmsScmTMDataset acmsScmTM, int n1hz, Double2d estq) { 
        Double2d estAttQ1 = (Double2d) reshapeColumn(acmsScmTM, "estAttQ1", n1hz, FOUR_HERTZ); 
        Double1d estAttQ1Sliced = sliceDouble2d(estAttQ1, 1); 
        appendToColumnDouble2d(estq, estAttQ1Sliced, 0); 

        Double2d estAttQ2 = (Double2d) reshapeColumn(acmsScmTM, "estAttQ2", n1hz, FOUR_HERTZ); 
        Double1d estAttQ2Sliced = sliceDouble2d(estAttQ2, 1); 
        appendToColumnDouble2d(estq, estAttQ2Sliced, 1); 

        Double2d estAttQ3 = (Double2d) reshapeColumn(acmsScmTM, "estAttQ3", n1hz, FOUR_HERTZ); 
        Double1d estAttQ3Sliced = sliceDouble2d(estAttQ3, 1); 
        appendToColumnDouble2d(estq, estAttQ3Sliced, 2); 

        Double2d estAttQ4 = (Double2d) reshapeColumn(acmsScmTM, "estAttQ4", n1hz, FOUR_HERTZ); 
        Double1d estAttQ4Sliced = sliceDouble2d(estAttQ4, 1); 
        appendToColumnDouble2d(estq, estAttQ4Sliced, 3); 
    } 

    private void setSTRQuaternion(AcmsScmTMDataset acmsScmTM, int n1hz, Double2d q) { 
        Double2d strmAttQ1 = (Double2d) reshapeColumn(acmsScmTM, "strmAttQ1", n1hz, FOUR_HERTZ); 
        Double1d strmAttQ1Sliced = sliceDouble2d(strmAttQ1, 1); 
        appendToColumnDouble2d(q, strmAttQ1Sliced, 0); 

        Double2d strmAttQ2 = (Double2d) reshapeColumn(acmsScmTM, "strmAttQ2", n1hz, FOUR_HERTZ); 
        Double1d strmAttQ2Sliced = sliceDouble2d(strmAttQ2, 1); 
        appendToColumnDouble2d(q, strmAttQ2Sliced, 1); 

        Double2d strmAttQ3 = (Double2d) reshapeColumn(acmsScmTM, "strmAttQ3", n1hz, FOUR_HERTZ); 
        Double1d strmAttQ3Sliced = sliceDouble2d(strmAttQ3, 1); 
        appendToColumnDouble2d(q, strmAttQ3Sliced, 2); 

        Double2d strmAttQ4 = (Double2d) reshapeColumn(acmsScmTM, "strmAttQ4", n1hz, FOUR_HERTZ); 
        Double1d strmAttQ4Sliced = sliceDouble2d(strmAttQ4, 1); 
        appendToColumnDouble2d(q, strmAttQ4Sliced, 3); 
    } 

    private void appendToColumnDouble2d(Double2d array, Double1d slice, int columnIndex) { 
        for (int row = 0; row < slice.getSize(); row++) { 
            array.set(row, columnIndex, slice.get(row)); 
        } 
    } 

    private Double1d sliceDouble2d(Double2d data, int columnIndex) { 
        double[][] array2d = data.getArray(); 

        double[] array1d = new double[array2d.length]; 
        for (int row = 0; row < array2d.length; row++) { 
            array1d[row] = array2d[row][columnIndex]; 
        } 

        return new Double1d(array1d); 
    } 

    private Long1d sliceLong2d(Long2d data, int columnIndex) { 
        long[][] array2d = data.getArray(); 

        long[] array1d = new long[array2d.length]; 
        for (int row = 0; row < array2d.length; row++) { 
            array1d[row] = array2d[row][columnIndex]; 
        } 

        return new Long1d(array1d); 
    } 

    private ArrayData reshapeColumn(TableDataset dataset, String name, int height, int width) { 
        return getData(dataset, name).apply(new Reshape(height, width)); 
    } 

    private ArrayData getData(TableDataset dataset, String name) { 
        return dataset.getColumn(name).getData(); 
    } 

    private double getDataItem(TableDataset dataset, String name, int index) { 
        return ((Double1d) getData(dataset, name)).get(index); 
    } 

    private Double2d newDouble2d(int d0, int d1) { 
        return new Double2d(d0, d1); 
    } 

    private Long2d newLong2d(int d0, int d1) { 
        return new Long2d(d0, d1); 
    } 



    }
\$\endgroup\$
  • 6
    \$\begingroup\$ Please change your title to reflect wat you are calculating, and add a description of the calculation too. \$\endgroup\$ – rolfl Oct 18 '14 at 10:11
  • 4
    \$\begingroup\$ Yes, your method definitely seems a bit long. But it also looks incomplete; it just stops in the middle. Is this a copy-paste error? \$\endgroup\$ – tim Oct 18 '14 at 10:29
  • \$\begingroup\$ as tim stated, please put in the entire method \$\endgroup\$ – Joeblade Oct 18 '14 at 10:32
  • \$\begingroup\$ Sorry, yes I think it was a copy/paste error. I have put the complete class so far.. \$\endgroup\$ – user1472672 Oct 19 '14 at 14:37
2
\$\begingroup\$

One guide I use to determine how to organize my code is to ask myself the question "What should this method/class/variable be named?" calculate is a poor name for this function because it does much more than calculate. It loads data, processes it, and does some calculations. But what does it calculate? We don't have many clues from the name or the method's signature -- it returns void!

Another question I find useful is "How will I test this code?" Your method is doing so many things that it will be really difficult to produce a reasonable number of test cases to verify its behavior.

There's also a bit of unclear data flow. You don't use return values very often—instead you pass in output parameters. It makes it difficult to read the code and understand what is being done.

One concrete bit of advice I would give is to pull out the data processing that computes q and estq; these should be passed as arguments to a method that calls itself calculate. calculate shouldn't have to know how to read the telemetry data and massage it into something it knows how to deal with. That's the caller's concern.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.