List of usage examples for org.apache.commons.math3.linear RealMatrix setEntry
void setEntry(int row, int column, double value) throws OutOfRangeException;
From source file:com.google.location.lbs.gnss.gps.pseudorange.UserPositionVelocityWeightedLeastSquare.java
/** * Least square solution to calculate the user position given the navigation message, pseudorange * and accumulated delta range measurements. Also calculates user velocity non-iteratively from * Least square position solution.//from w w w .j a va2 s . c o m * * <p>The method fills the user position and velocity in ECEF coordinates and receiver clock * offset in meters and clock offset rate in meters per second. * * <p>One can choose between no smoothing, using the carrier phase measurements (accumulated delta * range) or the doppler measurements (pseudorange rate) for smoothing the pseudorange. The * smoothing is applied only if time has changed below a specific threshold since last invocation. * * <p>Source for least squares: * * <ul> * <li>http://www.u-blox.com/images/downloads/Product_Docs/GPS_Compendium%28GPS-X-02007%29.pdf * page 81 - 85 * <li>Parkinson, B.W., Spilker Jr., J.J.: Global positioning system: theory and applications * page 412 - 414 * </ul> * * <p>Sources for smoothing pseudorange with carrier phase measurements: * * <ul> * <li>Satellite Communications and Navigation Systems book, page 424, * <li>Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, page 388, * 389. * </ul> * * <p>The function does not modify the smoothed measurement list {@code * immutableSmoothedSatellitesToReceiverMeasurements} * * @param navMessageProto parameters of the navigation message * @param usefulSatellitesToReceiverMeasurements Map of useful satellite PRN to {@link * GpsMeasurementWithRangeAndUncertainty} containing receiver measurements for computing the * position solution. * @param receiverGPSTowAtReceptionSeconds Receiver estimate of GPS time of week (seconds) * @param receiverGPSWeek Receiver estimate of GPS week (0-1024+) * @param dayOfYear1To366 The day of the year between 1 and 366 * @param positionVelocitySolutionECEF Solution array of the following format: * [0-2] xyz solution of user. * [3] clock bias of user. * [4-6] velocity of user. * [7] clock bias rate of user. * @param positionVelocityUncertaintyEnu Uncertainty of calculated position and velocity solution * in meters and mps local ENU system. Array has the following format: * [0-2] Enu uncertainty of position solution in meters * [3-5] Enu uncertainty of velocity solution in meters per second. * @param pseudorangeResidualMeters The pseudorange residual corrected by subtracting expected * psudorange calculated with the use clock bias of the highest elevation satellites. */ public void calculateUserPositionVelocityLeastSquare(GpsNavMessageProto navMessageProto, List<GpsMeasurementWithRangeAndUncertainty> usefulSatellitesToReceiverMeasurements, double receiverGPSTowAtReceptionSeconds, int receiverGPSWeek, int dayOfYear1To366, double[] positionVelocitySolutionECEF, double[] positionVelocityUncertaintyEnu, double[] pseudorangeResidualMeters) throws Exception { // Use PseudorangeSmoother to smooth the pseudorange according to: Satellite Communications and // Navigation Systems book, page 424 and Principles of GNSS, Inertial, and Multisensor // Integrated Navigation Systems, page 388, 389. double[] deltaPositionMeters; List<GpsMeasurementWithRangeAndUncertainty> immutableSmoothedSatellitesToReceiverMeasurements = pseudorangeSmoother .updatePseudorangeSmoothingResult( Collections.unmodifiableList(usefulSatellitesToReceiverMeasurements)); List<GpsMeasurementWithRangeAndUncertainty> mutableSmoothedSatellitesToReceiverMeasurements = Lists .newArrayList(immutableSmoothedSatellitesToReceiverMeasurements); int numberOfUsefulSatellites = getNumberOfUsefulSatellites(mutableSmoothedSatellitesToReceiverMeasurements); // Least square position solution is supported only if 4 or more satellites visible Preconditions.checkArgument(numberOfUsefulSatellites >= MINIMUM_NUMER_OF_SATELLITES, "At least 4 satellites have to be visible... Only 3D mode is supported..."); boolean repeatLeastSquare = false; SatellitesPositionPseudorangesResidualAndCovarianceMatrix satPosPseudorangeResidualAndWeight; boolean isFirstWLS = true; do { // Calculate satellites' positions, measurement residuals per visible satellite and // weight matrix for the iterative least square boolean doAtmosphericCorrections = false; satPosPseudorangeResidualAndWeight = calculateSatPosAndPseudorangeResidual(navMessageProto, mutableSmoothedSatellitesToReceiverMeasurements, receiverGPSTowAtReceptionSeconds, receiverGPSWeek, dayOfYear1To366, positionVelocitySolutionECEF, doAtmosphericCorrections); // Calculate the geometry matrix according to "Global Positioning System: Theory and // Applications", Parkinson and Spilker page 413 RealMatrix covarianceMatrixM2 = new Array2DRowRealMatrix( satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare); geometryMatrix = new Array2DRowRealMatrix(calculateGeometryMatrix( satPosPseudorangeResidualAndWeight.satellitesPositionsMeters, positionVelocitySolutionECEF)); RealMatrix weightedGeometryMatrix; RealMatrix weightMatrixMetersMinus2 = null; // Apply weighted least square only if the covariance matrix is not singular (has a non-zero // determinant), otherwise apply ordinary least square. The reason is to ignore reported // signal to noise ratios by the receiver that can lead to such singularities LUDecomposition ludCovMatrixM2 = new LUDecomposition(covarianceMatrixM2); double det = ludCovMatrixM2.getDeterminant(); if (det <= DOUBLE_ROUND_OFF_TOLERANCE) { // Do not weight the geometry matrix if covariance matrix is singular. weightedGeometryMatrix = geometryMatrix; } else { weightMatrixMetersMinus2 = ludCovMatrixM2.getSolver().getInverse(); RealMatrix hMatrix = calculateHMatrix(weightMatrixMetersMinus2, geometryMatrix); weightedGeometryMatrix = hMatrix.multiply(geometryMatrix.transpose()) .multiply(weightMatrixMetersMinus2); } // Equation 9 page 413 from "Global Positioning System: Theory and Applicaitons", Parkinson // and Spilker deltaPositionMeters = GpsMathOperations.matrixByColVectMultiplication(weightedGeometryMatrix.getData(), satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters); // Apply corrections to the position estimate positionVelocitySolutionECEF[0] += deltaPositionMeters[0]; positionVelocitySolutionECEF[1] += deltaPositionMeters[1]; positionVelocitySolutionECEF[2] += deltaPositionMeters[2]; positionVelocitySolutionECEF[3] += deltaPositionMeters[3]; // Iterate applying corrections to the position solution until correction is below threshold satPosPseudorangeResidualAndWeight = applyWeightedLeastSquare(navMessageProto, mutableSmoothedSatellitesToReceiverMeasurements, receiverGPSTowAtReceptionSeconds, receiverGPSWeek, dayOfYear1To366, positionVelocitySolutionECEF, deltaPositionMeters, doAtmosphericCorrections, satPosPseudorangeResidualAndWeight, weightMatrixMetersMinus2); // We use the first WLS iteration results and correct them based on the ground truth position // and using a clock error computed from high elevation satellites. The first iteration is // used before satellite with high residuals being removed. if (isFirstWLS && truthLocationForCorrectedResidualComputationEcef != null) { // Snapshot the information needed before high residual satellites are removed System.arraycopy( ResidualCorrectionCalculator.calculateCorrectedResiduals(satPosPseudorangeResidualAndWeight, positionVelocitySolutionECEF.clone(), truthLocationForCorrectedResidualComputationEcef), 0 /*source starting pos*/, pseudorangeResidualMeters, 0 /*destination starting pos*/, GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES /*length of elements*/); isFirstWLS = false; } repeatLeastSquare = false; int satsWithResidualBelowThreshold = satPosPseudorangeResidualAndWeight.pseudorangeResidualsMeters.length; // remove satellites that have residuals above RESIDUAL_TO_REPEAT_LEAST_SQUARE_METERS as they // worsen the position solution accuracy. If any satellite is removed, repeat the least square repeatLeastSquare = removeHighResidualSats(mutableSmoothedSatellitesToReceiverMeasurements, repeatLeastSquare, satPosPseudorangeResidualAndWeight, satsWithResidualBelowThreshold); } while (repeatLeastSquare); calculateGeoidMeters = false; // The computed ECEF position will be used next to compute the user velocity. // we calculate and fill in the user velocity solutions based on following equation: // Weight Matrix * GeometryMatrix * User Velocity Vector // = Weight Matrix * deltaPseudoRangeRateWeightedMps // Reference: Pratap Misra and Per Enge // "Global Positioning System: Signals, Measurements, and Performance" Page 218. // Get the number of satellite used in Geometry Matrix numberOfUsefulSatellites = geometryMatrix.getRowDimension(); RealMatrix rangeRateMps = new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); RealMatrix deltaPseudoRangeRateMps = new Array2DRowRealMatrix(numberOfUsefulSatellites, 1); RealMatrix pseudorangeRateWeight = new Array2DRowRealMatrix(numberOfUsefulSatellites, numberOfUsefulSatellites); // Correct the receiver time of week with the estimated receiver clock bias receiverGPSTowAtReceptionSeconds = receiverGPSTowAtReceptionSeconds - positionVelocitySolutionECEF[3] / SPEED_OF_LIGHT_MPS; int measurementCount = 0; // Calculate range rates for (int i = 0; i < GpsNavigationMessageStore.MAX_NUMBER_OF_SATELLITES; i++) { if (mutableSmoothedSatellitesToReceiverMeasurements.get(i) != null) { GpsEphemerisProto ephemeridesProto = getEphemerisForSatellite(navMessageProto, i + 1); double pseudorangeMeasurementMeters = mutableSmoothedSatellitesToReceiverMeasurements .get(i).pseudorangeMeters; GpsTimeOfWeekAndWeekNumber correctedTowAndWeek = calculateCorrectedTransmitTowAndWeek( ephemeridesProto, receiverGPSTowAtReceptionSeconds, receiverGPSWeek, pseudorangeMeasurementMeters); // Calculate satellite velocity PositionAndVelocity satPosECEFMetersVelocityMPS = SatellitePositionCalculator .calculateSatellitePositionAndVelocityFromEphemeris(ephemeridesProto, correctedTowAndWeek.gpsTimeOfWeekSeconds, correctedTowAndWeek.weekNumber, positionVelocitySolutionECEF[0], positionVelocitySolutionECEF[1], positionVelocitySolutionECEF[2]); // Calculate satellite clock error rate double satelliteClockErrorRateMps = SatelliteClockCorrectionCalculator .calculateSatClockCorrErrorRate(ephemeridesProto, correctedTowAndWeek.gpsTimeOfWeekSeconds, correctedTowAndWeek.weekNumber); // Fill in range rates. range rate = satellite velocity (dot product) line-of-sight vector rangeRateMps.setEntry(measurementCount, 0, -1 * (satPosECEFMetersVelocityMPS.velocityXMetersPerSec * geometryMatrix.getEntry(measurementCount, 0) + satPosECEFMetersVelocityMPS.velocityYMetersPerSec * geometryMatrix.getEntry(measurementCount, 1) + satPosECEFMetersVelocityMPS.velocityZMetersPerSec * geometryMatrix.getEntry(measurementCount, 2))); deltaPseudoRangeRateMps.setEntry(measurementCount, 0, mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeRateMps - rangeRateMps.getEntry(measurementCount, 0) + satelliteClockErrorRateMps - positionVelocitySolutionECEF[7]); // Calculate the velocity weight matrix by using 1 / square(Pseudorangerate Uncertainty) // along the diagonal pseudorangeRateWeight.setEntry(measurementCount, measurementCount, 1 / (mutableSmoothedSatellitesToReceiverMeasurements.get(i).pseudorangeRateUncertaintyMps * mutableSmoothedSatellitesToReceiverMeasurements .get(i).pseudorangeRateUncertaintyMps)); measurementCount++; } } RealMatrix weightedGeoMatrix = pseudorangeRateWeight.multiply(geometryMatrix); RealMatrix deltaPseudoRangeRateWeightedMps = pseudorangeRateWeight.multiply(deltaPseudoRangeRateMps); QRDecomposition qrdWeightedGeoMatrix = new QRDecomposition(weightedGeoMatrix); RealMatrix velocityMps = qrdWeightedGeoMatrix.getSolver().solve(deltaPseudoRangeRateWeightedMps); positionVelocitySolutionECEF[4] = velocityMps.getEntry(0, 0); positionVelocitySolutionECEF[5] = velocityMps.getEntry(1, 0); positionVelocitySolutionECEF[6] = velocityMps.getEntry(2, 0); positionVelocitySolutionECEF[7] = velocityMps.getEntry(3, 0); RealMatrix pseudorangeWeight = new LUDecomposition( new Array2DRowRealMatrix(satPosPseudorangeResidualAndWeight.covarianceMatrixMetersSquare)) .getSolver().getInverse(); // Calculate and store the uncertainties of position and velocity in local ENU system in meters // and meters per second. double[] pvUncertainty = calculatePositionVelocityUncertaintyEnu(pseudorangeRateWeight, pseudorangeWeight, positionVelocitySolutionECEF); System.arraycopy(pvUncertainty, 0 /*source starting pos*/, positionVelocityUncertaintyEnu, 0 /*destination starting pos*/, 6 /*length of elements*/); }
From source file:lambertmrev.Lambert.java
/** Constructs and solves a Lambert problem. * * \param[in] R1 first cartesian position * \param[in] R2 second cartesian position * \param[in] tof time of flight//from w w w . ja v a 2 s.c o m * \param[in] mu gravity parameter * \param[in] cw when 1 a retrograde orbit is assumed * \param[in] multi_revs maximum number of multirevolutions to compute */ public void lambert_problem(Vector3D r1, Vector3D r2, double tof, double mu, Boolean cw, int multi_revs) { // sanity checks if (tof <= 0) { System.out.println("ToF is negative! \n"); } if (mu <= 0) { System.out.println("mu is below zero"); } // 1 - getting lambda and T double m_c = FastMath.sqrt((r2.getX() - r1.getX()) * (r2.getX() - r1.getX()) + (r2.getY() - r1.getY()) * (r2.getY() - r1.getY()) + (r2.getZ() - r1.getZ()) * (r2.getZ() - r1.getZ())); double R1 = r1.getNorm(); double R2 = r2.getNorm(); double m_s = (m_c + R1 + R2) / 2.0; Vector3D ir1 = r1.normalize(); Vector3D ir2 = r2.normalize(); Vector3D ih = Vector3D.crossProduct(ir1, ir2); ih = ih.normalize(); if (ih.getZ() == 0) { System.out.println("angular momentum vector has no z component \n"); } double lambda2 = 1.0 - m_c / m_s; double m_lambda = FastMath.sqrt(lambda2); Vector3D it1 = new Vector3D(0.0, 0.0, 0.0); Vector3D it2 = new Vector3D(0.0, 0.0, 0.0); if (ih.getZ() < 0.0) { // Transfer angle is larger than 180 degrees as seen from abive the z axis m_lambda = -m_lambda; it1 = Vector3D.crossProduct(ir1, ih); it2 = Vector3D.crossProduct(ir2, ih); } else { it1 = Vector3D.crossProduct(ih, ir1); it2 = Vector3D.crossProduct(ih, ir2); } it1.normalize(); it2.normalize(); if (cw) { // Retrograde motion m_lambda = -m_lambda; it1.negate(); it2.negate(); } double lambda3 = m_lambda * lambda2; double T = FastMath.sqrt(2.0 * mu / m_s / m_s / m_s) * tof; // 2 - We now hava lambda, T and we will find all x // 2.1 - let us first detect the maximum number of revolutions for which there exists a solution int m_Nmax = FastMath.toIntExact(FastMath.round(T / FastMath.PI)); double T00 = FastMath.acos(m_lambda) + m_lambda * FastMath.sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * FastMath.PI); double T1 = 2.0 / 3.0 * (1.0 - lambda3); double DT = 0.0; double DDT = 0.0; double DDDT = 0.0; if (m_Nmax > 0) { if (T < T0) { // We use Halley iterations to find xM and TM int it = 0; double err = 1.0; double T_min = T0; double x_old = 0.0, x_new = 0.0; while (true) { ArrayRealVector deriv = dTdx(x_old, T_min, m_lambda); DT = deriv.getEntry(0); DDT = deriv.getEntry(1); DDDT = deriv.getEntry(2); if (DT != 0.0) { x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); } err = FastMath.abs(x_old - x_new); if ((err < 1e-13) || (it > 12)) { break; } tof = x2tof(x_new, m_Nmax, m_lambda); x_old = x_new; it++; } if (T_min > T) { m_Nmax -= 1; } } } // We exit this if clause with Mmax being the maximum number of revolutions // for which there exists a solution. We crop it to multi_revs m_Nmax = FastMath.min(multi_revs, m_Nmax); // 2.2 We now allocate the memory for the output variables m_v1 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_v2 = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); RealMatrix m_iters = MatrixUtils.createRealMatrix(m_Nmax * 2 + 1, 3); //RealMatrix m_x = MatrixUtils.createRealMatrix(m_Nmax*2+1, 3); ArrayRealVector m_x = new ArrayRealVector(m_Nmax * 2 + 1); // 3 - We may now find all solution in x,y // 3.1 0 rev solution // 3.1.1 initial guess if (T >= T00) { m_x.setEntry(0, -(T - T00) / (T - T00 + 4)); } else if (T <= T1) { m_x.setEntry(0, T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1); } else { m_x.setEntry(0, FastMath.pow((T / T00), 0.69314718055994529 / FastMath.log(T1 / T00)) - 1.0); } // 3.1.2 Householder iterations //m_iters.setEntry(0, 0, housOutTmp.getEntry(0)); m_x.setEntry(0, householder(T, m_x.getEntry(0), 0, 1e-5, 15, m_lambda)); // 3.2 multi rev solutions double tmp; double x0; for (int i = 1; i < m_Nmax + 1; i++) { // 3.2.1 left householder iterations tmp = FastMath.pow((i * FastMath.PI + FastMath.PI) / (8.0 * T), 2.0 / 3.0); m_x.setEntry(2 * i - 1, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i - 1), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i - 1, x0); //m_iters.setEntry(2*i-1, 0, housOutTmp.getEntry(0)); //3.2.1 right Householder iterations tmp = FastMath.pow((8.0 * T) / (i * FastMath.PI), 2.0 / 3.0); m_x.setEntry(2 * i, (tmp - 1) / (tmp + 1)); x0 = householder(T, m_x.getEntry(2 * i), i, 1e-8, 15, m_lambda); m_x.setEntry(2 * i, x0); //m_iters.setEntry(2*i, 0, housOutTmp.getEntry(0)); } // 4 - For each found x value we recontruct the terminal velocities double gamma = FastMath.sqrt(mu * m_s / 2.0); double rho = (R1 - R2) / m_c; double sigma = FastMath.sqrt(1 - rho * rho); double vr1, vt1, vr2, vt2, y; ArrayRealVector ir1_vec = new ArrayRealVector(3); ArrayRealVector ir2_vec = new ArrayRealVector(3); ArrayRealVector it1_vec = new ArrayRealVector(3); ArrayRealVector it2_vec = new ArrayRealVector(3); // set Vector3D values to a mutable type ir1_vec.setEntry(0, ir1.getX()); ir1_vec.setEntry(1, ir1.getY()); ir1_vec.setEntry(2, ir1.getZ()); ir2_vec.setEntry(0, ir2.getX()); ir2_vec.setEntry(1, ir2.getY()); ir2_vec.setEntry(2, ir2.getZ()); it1_vec.setEntry(0, it1.getX()); it1_vec.setEntry(1, it1.getY()); it1_vec.setEntry(2, it1.getZ()); it2_vec.setEntry(0, it2.getX()); it2_vec.setEntry(1, it2.getY()); it2_vec.setEntry(2, it2.getZ()); for (int i = 0; i < m_x.getDimension(); i++) { y = FastMath.sqrt(1.0 - lambda2 + lambda2 * m_x.getEntry(i) * m_x.getEntry(i)); vr1 = gamma * ((m_lambda * y - m_x.getEntry(i)) - rho * (m_lambda * y + m_x.getEntry(i))) / R1; vr2 = -gamma * ((m_lambda * y - m_x.getEntry(i)) + rho * (m_lambda * y + m_x.getEntry(i))) / R2; double vt = gamma * sigma * (y + m_lambda * m_x.getEntry(i)); vt1 = vt / R1; vt2 = vt / R2; for (int j = 0; j < 3; ++j) m_v1.setEntry(i, j, vr1 * ir1_vec.getEntry(j) + vt1 * it1_vec.getEntry(j)); for (int j = 0; j < 3; ++j) m_v2.setEntry(i, j, vr2 * ir2_vec.getEntry(j) + vt2 * it2_vec.getEntry(j)); } }
From source file:ffnn.FFNNTubesAI.java
@Override public void buildClassifier(Instances i) throws Exception { Instance temp_instance = null;/*w w w.jav a2 s .co m*/ RealMatrix error_output; RealMatrix error_hidden; RealMatrix input_matrix; RealMatrix hidden_matrix; RealMatrix output_matrix; Instances temp_instances; int r = 0; Scanner scan = new Scanner(System.in); output_layer = i.numDistinctValues(i.classIndex()); //3 temp_instances = filterNominalNumeric(i); if (output_layer == 2) { Add filter = new Add(); filter.setAttributeIndex("last"); filter.setAttributeName("dummy"); filter.setInputFormat(temp_instances); temp_instances = Filter.useFilter(temp_instances, filter); // System.out.println(temp_instances); for (int j = 0; j < temp_instances.numInstances(); j++) { if (temp_instances.instance(j).value(temp_instances.numAttributes() - 2) == 0) { temp_instances.instance(j).setValue(temp_instances.numAttributes() - 2, 1); temp_instances.instance(j).setValue(temp_instances.numAttributes() - 1, 0); } else { temp_instances.instance(j).setValue(temp_instances.numAttributes() - 2, 0); temp_instances.instance(j).setValue(temp_instances.numAttributes() - 1, 1); } } } //temp_instances.randomize(temp_instances.getRandomNumberGenerator(1)); //System.out.println(temp_instances); input_layer = temp_instances.numAttributes() - output_layer; //4 hidden_layer = 0; while (hidden_layer < 1) { System.out.print("Hidden layer : "); hidden_layer = scan.nextInt(); } int init_hidden = hidden_layer; error_hidden = new BlockRealMatrix(1, hidden_layer); error_output = new BlockRealMatrix(1, output_layer); input_matrix = new BlockRealMatrix(1, input_layer + 1); //Menambahkan bias buildWeight(input_layer, hidden_layer, output_layer); long last_time = System.nanoTime(); double last_error_rate = 1; double best_error_rate = 1; double last_update = System.nanoTime(); // brp iterasi // for( long itr = 0; last_error_rate > 0.001; ++ itr ){ for (long itr = 0; itr < 50000; ++itr) { if (r == 10) { break; } long time = System.nanoTime(); if (time - last_time > 2000000000) { Evaluation eval = new Evaluation(i); eval.evaluateModel(this, i); double accry = eval.correct() / eval.numInstances(); if (eval.errorRate() < last_error_rate) { last_update = System.nanoTime(); if (eval.errorRate() < best_error_rate) SerializationHelper.write(accry + "-" + time + ".model", this); } if (accry > 0) last_error_rate = eval.errorRate(); // 2 minute without improvement restart if (time - last_update > 30000000000L) { last_update = System.nanoTime(); learning_rate = random() * 0.05; hidden_layer = (int) (10 + floor(random() * 15)); hidden_layer = (int) floor((hidden_layer / 25) * init_hidden); if (hidden_layer == 0) { hidden_layer = 1; } itr = 0; System.out.println("RESTART " + learning_rate + " " + hidden_layer); buildWeight(input_layer, hidden_layer, output_layer); r++; } System.out.println(accry + " " + itr); last_time = time; } for (int j = 0; j < temp_instances.numInstances(); j++) { // foward !! temp_instance = temp_instances.instance(j); for (int k = 0; k < input_layer; k++) { input_matrix.setEntry(0, k, temp_instance.value(k)); } input_matrix.setEntry(0, input_layer, 1.0); // bias hidden_matrix = input_matrix.multiply(weight1); for (int y = 0; y < hidden_layer; ++y) { hidden_matrix.setEntry(0, y, sig(hidden_matrix.getEntry(0, y))); } output_matrix = hidden_matrix.multiply(weight2).add(bias2); for (int y = 0; y < output_layer; ++y) { output_matrix.setEntry(0, y, sig(output_matrix.getEntry(0, y))); } // backward << // error layer 2 double total_err = 0; for (int k = 0; k < output_layer; k++) { double o = output_matrix.getEntry(0, k); double t = temp_instance.value(input_layer + k); double err = o * (1 - o) * (t - o); total_err += err * err; error_output.setEntry(0, k, err); } // back propagation layer 2 for (int y = 0; y < hidden_layer; y++) { for (int x = 0; x < output_layer; ++x) { double wold = weight2.getEntry(y, x); double correction = learning_rate * error_output.getEntry(0, x) * hidden_matrix.getEntry(0, y); weight2.setEntry(y, x, wold + correction); } } for (int x = 0; x < output_layer; ++x) { double correction = learning_rate * error_output.getEntry(0, x); // anggap 1 inputnya bias2.setEntry(0, x, bias2.getEntry(0, x) + correction); } // error layer 1 for (int k = 0; k < hidden_layer; ++k) { double o = hidden_matrix.getEntry(0, k); double t = 0; for (int x = 0; x < output_layer; ++x) { t += error_output.getEntry(0, x) * weight2.getEntry(k, x); } double err = o * (1 - o) * t; error_hidden.setEntry(0, k, err); } // back propagation layer 1 for (int y = 0; y < input_layer + 1; ++y) { for (int x = 0; x < hidden_layer; ++x) { double wold = weight1.getEntry(y, x); double correction = learning_rate * error_hidden.getEntry(0, x) * input_matrix.getEntry(0, y); weight1.setEntry(y, x, wold + correction); } } } } }
From source file:edu.cmu.tetrad.search.TestHippocampus.java
private double[][] calcFirstPrincipleComponent(int sampleSize, DataSet data, int[][] voxellation) { double[][] sums = new double[sampleSize][voxellation.length]; for (int s = 0; s < voxellation.length; s++) { RealMatrix m = new BlockRealMatrix(sampleSize, voxellation[s].length); for (int i = 0; i < sampleSize; i++) { for (int j = 0; j < voxellation[s].length; j++) { m.setEntry(i, j, data.getDouble(i, voxellation[s][j])); }/*from w w w. j a v a 2 s. c om*/ } SingularValueDecomposition d = new SingularValueDecomposition(m); RealMatrix s1 = d.getU(); double[] c = s1.getColumn(0); for (int i = 0; i < sampleSize; i++) { sums[i][s] = c[i]; } } return sums; }
From source file:nova.core.render.model.MeshModel.java
public Set<Model> flatten(MatrixStack matrixStack) { Set<Model> models = new HashSet<>(); matrixStack.pushMatrix();/* ww w . j a va2 s .c om*/ matrixStack.transform(matrix.getMatrix()); //Create a new model with transformation applied. MeshModel transformedModel = clone(); // correct formula for Normal Matrix is transpose(inverse(mat3(model_mat)) // we have to augemnt that to 4x4 RealMatrix normalMatrix3x3 = new LUDecomposition(matrixStack.getMatrix().getSubMatrix(0, 2, 0, 2), 1e-5) .getSolver().getInverse().transpose(); RealMatrix normalMatrix = MatrixUtils.createRealMatrix(4, 4); normalMatrix.setSubMatrix(normalMatrix3x3.getData(), 0, 0); normalMatrix.setEntry(3, 3, 1); transformedModel.faces.stream().forEach(f -> { f.normal = TransformUtil.transform(f.normal, normalMatrix); f.vertices.forEach(v -> v.vec = matrixStack.apply(v.vec)); }); models.add(transformedModel); //Flatten child models models.addAll(children.stream().flatMap(m -> m.flatten(matrixStack).stream()).collect(Collectors.toSet())); matrixStack.popMatrix(); return models; }
From source file:nova.core.util.math.MatrixStack.java
/** * Rotates the current matrix// w ww . jav a 2 s . c o m * * @param rotation The rotation to aply * @return The rorated matrix */ public MatrixStack rotate(Rotation rotation) { RealMatrix rotMat = MatrixUtils.createRealMatrix(4, 4); rotMat.setSubMatrix(rotation.getMatrix(), 0, 0); rotMat.setEntry(3, 3, 1); current = current.preMultiply(rotMat); return this; }
From source file:nova.core.util.math.MatrixUtil.java
public static RealMatrix augmentWithIdentity(RealMatrix matrix, int dimensions) { RealMatrix augmented = augment(matrix, dimensions, dimensions); for (int i = MathUtil.max(matrix.getRowDimension(), matrix.getColumnDimension()) + 1; i <= dimensions; i++) { augmented.setEntry(i - 1, i - 1, 1); }/*from ww w .j ava 2 s . co m*/ return augmented; }
From source file:org.akvo.caddisfly.sensor.colorimetry.strip.calibration.CalibrationCard.java
@NonNull private static Mat doIlluminationCorrection(@NonNull Mat imgLab, @NonNull CalibrationData calData) { // create HLS image for homogeneous illumination calibration int pHeight = imgLab.rows(); int pWidth = imgLab.cols(); RealMatrix points = createWhitePointMatrix(imgLab, calData); // create coefficient matrix for all three variables L,A,B // the model for all three is y = ax + bx^2 + cy + dy^2 + exy + f // 6th row is the constant 1 RealMatrix coefficient = new Array2DRowRealMatrix(points.getRowDimension(), 6); coefficient.setColumnMatrix(0, points.getColumnMatrix(0)); coefficient.setColumnMatrix(2, points.getColumnMatrix(1)); //create constant, x^2, y^2 and xy terms for (int i = 0; i < points.getRowDimension(); i++) { coefficient.setEntry(i, 1, Math.pow(coefficient.getEntry(i, 0), 2)); // x^2 coefficient.setEntry(i, 3, Math.pow(coefficient.getEntry(i, 2), 2)); // y^2 coefficient.setEntry(i, 4, coefficient.getEntry(i, 0) * coefficient.getEntry(i, 2)); // xy coefficient.setEntry(i, 5, 1d); // constant = 1 }/* w w w .ja v a 2 s.c o m*/ // create vectors RealVector L = points.getColumnVector(2); RealVector A = points.getColumnVector(3); RealVector B = points.getColumnVector(4); // solve the least squares problem for all three variables DecompositionSolver solver = new SingularValueDecomposition(coefficient).getSolver(); RealVector solutionL = solver.solve(L); RealVector solutionA = solver.solve(A); RealVector solutionB = solver.solve(B); // get individual coefficients float La = (float) solutionL.getEntry(0); float Lb = (float) solutionL.getEntry(1); float Lc = (float) solutionL.getEntry(2); float Ld = (float) solutionL.getEntry(3); float Le = (float) solutionL.getEntry(4); float Lf = (float) solutionL.getEntry(5); float Aa = (float) solutionA.getEntry(0); float Ab = (float) solutionA.getEntry(1); float Ac = (float) solutionA.getEntry(2); float Ad = (float) solutionA.getEntry(3); float Ae = (float) solutionA.getEntry(4); float Af = (float) solutionA.getEntry(5); float Ba = (float) solutionB.getEntry(0); float Bb = (float) solutionB.getEntry(1); float Bc = (float) solutionB.getEntry(2); float Bd = (float) solutionB.getEntry(3); float Be = (float) solutionB.getEntry(4); float Bf = (float) solutionB.getEntry(5); // compute mean (the luminosity value of the plane in the middle of the image) float L_mean = (float) (0.5 * La * pWidth + 0.5 * Lc * pHeight + Lb * pWidth * pWidth / 3.0 + Ld * pHeight * pHeight / 3.0 + Le * 0.25 * pHeight * pWidth + Lf); float A_mean = (float) (0.5 * Aa * pWidth + 0.5 * Ac * pHeight + Ab * pWidth * pWidth / 3.0 + Ad * pHeight * pHeight / 3.0 + Ae * 0.25 * pHeight * pWidth + Af); float B_mean = (float) (0.5 * Ba * pWidth + 0.5 * Bc * pHeight + Bb * pWidth * pWidth / 3.0 + Bd * pHeight * pHeight / 3.0 + Be * 0.25 * pHeight * pWidth + Bf); // Correct image // we do this per row. We tried to do it in one block, but there is no speed difference. byte[] temp = new byte[imgLab.cols() * imgLab.channels()]; int valL, valA, valB; int ii, ii3; float iiSq, iSq; int imgCols = imgLab.cols(); int imgRows = imgLab.rows(); // use lookup tables to speed up computation // create lookup tables float[] L_aii = new float[imgCols]; float[] L_biiSq = new float[imgCols]; float[] A_aii = new float[imgCols]; float[] A_biiSq = new float[imgCols]; float[] B_aii = new float[imgCols]; float[] B_biiSq = new float[imgCols]; float[] Lci = new float[imgRows]; float[] LdiSq = new float[imgRows]; float[] Aci = new float[imgRows]; float[] AdiSq = new float[imgRows]; float[] Bci = new float[imgRows]; float[] BdiSq = new float[imgRows]; for (ii = 0; ii < imgCols; ii++) { iiSq = ii * ii; L_aii[ii] = La * ii; L_biiSq[ii] = Lb * iiSq; A_aii[ii] = Aa * ii; A_biiSq[ii] = Ab * iiSq; B_aii[ii] = Ba * ii; B_biiSq[ii] = Bb * iiSq; } for (int i = 0; i < imgRows; i++) { iSq = i * i; Lci[i] = Lc * i; LdiSq[i] = Ld * iSq; Aci[i] = Ac * i; AdiSq[i] = Ad * iSq; Bci[i] = Bc * i; BdiSq[i] = Bd * iSq; } // We can also improve the performance of the i,ii term, if we want, but it won't make much difference. for (int i = 0; i < imgRows; i++) { // y imgLab.get(i, 0, temp); ii3 = 0; for (ii = 0; ii < imgCols; ii++) { //x valL = capValue( Math.round((temp[ii3] & 0xFF) - (L_aii[ii] + L_biiSq[ii] + Lci[i] + LdiSq[i] + Le * i * ii + Lf) + L_mean), 0, 255); valA = capValue( Math.round((temp[ii3 + 1] & 0xFF) - (A_aii[ii] + A_biiSq[ii] + Aci[i] + AdiSq[i] + Ae * i * ii + Af) + A_mean), 0, 255); valB = capValue( Math.round((temp[ii3 + 2] & 0xFF) - (B_aii[ii] + B_biiSq[ii] + Bci[i] + BdiSq[i] + Be * i * ii + Bf) + B_mean), 0, 255); temp[ii3] = (byte) valL; temp[ii3 + 1] = (byte) valA; temp[ii3 + 2] = (byte) valB; ii3 += 3; } imgLab.put(i, 0, temp); } return imgLab; }
From source file:org.akvo.caddisfly.sensor.colorimetry.strip.calibration.CalibrationCard.java
@NonNull private static Mat do1D_3DCorrection(@NonNull Mat imgMat, @Nullable CalibrationData calData) throws CalibrationException { if (calData == null) { throw new CalibrationException("no calibration data."); }//w w w. j a v a 2 s . co m final WeightedObservedPoints obsL = new WeightedObservedPoints(); final WeightedObservedPoints obsA = new WeightedObservedPoints(); final WeightedObservedPoints obsB = new WeightedObservedPoints(); Map<String, double[]> calResultIllumination = new HashMap<>(); // iterate over all patches try { for (String label : calData.getCalValues().keySet()) { CalibrationData.CalValue cal = calData.getCalValues().get(label); CalibrationData.Location loc = calData.getLocations().get(label); float[] LAB_color = measurePatch(imgMat, loc.x, loc.y, calData); // measure patch color obsL.add(LAB_color[0], cal.getL()); obsA.add(LAB_color[1], cal.getA()); obsB.add(LAB_color[2], cal.getB()); calResultIllumination.put(label, new double[] { LAB_color[0], LAB_color[1], LAB_color[2] }); } } catch (Exception e) { throw new CalibrationException("1D calibration: error iterating over all patches.", e); } // Instantiate a second-degree polynomial fitter. final PolynomialCurveFitter fitter = PolynomialCurveFitter.create(2); // Retrieve fitted parameters (coefficients of the polynomial function). // order of coefficients is (c + bx + ax^2), so [c,b,a] try { final double[] coefficientL = fitter.fit(obsL.toList()); final double[] coefficientA = fitter.fit(obsA.toList()); final double[] coefficientB = fitter.fit(obsB.toList()); double[] valIllumination; double L_orig, A_orig, B_orig, L_new, A_new, B_new; // transform patch values using the 1d calibration results Map<String, double[]> calResult1D = new HashMap<>(); for (String label : calData.getCalValues().keySet()) { valIllumination = calResultIllumination.get(label); L_orig = valIllumination[0]; A_orig = valIllumination[1]; B_orig = valIllumination[2]; L_new = coefficientL[2] * L_orig * L_orig + coefficientL[1] * L_orig + coefficientL[0]; A_new = coefficientA[2] * A_orig * A_orig + coefficientA[1] * A_orig + coefficientA[0]; B_new = coefficientB[2] * B_orig * B_orig + coefficientB[1] * B_orig + coefficientB[0]; calResult1D.put(label, new double[] { L_new, A_new, B_new }); } // use the 1D calibration result for the second calibration step // Following http://docs.scipy.org/doc/scipy/reference/tutorial/linalg.html#solving-linear-least-squares-problems-and-pseudo-inverses // we will solve P = M x int total = calData.getLocations().keySet().size(); RealMatrix coefficient = new Array2DRowRealMatrix(total, 3); RealMatrix cal = new Array2DRowRealMatrix(total, 3); int index = 0; // create coefficient and calibration vectors for (String label : calData.getCalValues().keySet()) { CalibrationData.CalValue calv = calData.getCalValues().get(label); double[] cal1dResult = calResult1D.get(label); coefficient.setEntry(index, 0, cal1dResult[0]); coefficient.setEntry(index, 1, cal1dResult[1]); coefficient.setEntry(index, 2, cal1dResult[2]); cal.setEntry(index, 0, calv.getL()); cal.setEntry(index, 1, calv.getA()); cal.setEntry(index, 2, calv.getB()); index++; } DecompositionSolver solver = new SingularValueDecomposition(coefficient).getSolver(); RealMatrix sol = solver.solve(cal); float a_L, b_L, c_L, a_A, b_A, c_A, a_B, b_B, c_B; a_L = (float) sol.getEntry(0, 0); b_L = (float) sol.getEntry(1, 0); c_L = (float) sol.getEntry(2, 0); a_A = (float) sol.getEntry(0, 1); b_A = (float) sol.getEntry(1, 1); c_A = (float) sol.getEntry(2, 1); a_B = (float) sol.getEntry(0, 2); b_B = (float) sol.getEntry(1, 2); c_B = (float) sol.getEntry(2, 2); //use the solution to correct the image double L_temp, A_temp, B_temp, L_mid, A_mid, B_mid; int L_fin, A_fin, B_fin; int ii3; byte[] temp = new byte[imgMat.cols() * imgMat.channels()]; for (int i = 0; i < imgMat.rows(); i++) { // y imgMat.get(i, 0, temp); ii3 = 0; for (int ii = 0; ii < imgMat.cols(); ii++) { //x L_temp = temp[ii3] & 0xFF; A_temp = temp[ii3 + 1] & 0xFF; B_temp = temp[ii3 + 2] & 0xFF; L_mid = coefficientL[2] * L_temp * L_temp + coefficientL[1] * L_temp + coefficientL[0]; A_mid = coefficientA[2] * A_temp * A_temp + coefficientA[1] * A_temp + coefficientA[0]; B_mid = coefficientB[2] * B_temp * B_temp + coefficientB[1] * B_temp + coefficientB[0]; L_fin = (int) Math.round(a_L * L_mid + b_L * A_mid + c_L * B_mid); A_fin = (int) Math.round(a_A * L_mid + b_A * A_mid + c_A * B_mid); B_fin = (int) Math.round(a_B * L_mid + b_B * A_mid + c_B * B_mid); // cap values L_fin = capValue(L_fin, 0, 255); A_fin = capValue(A_fin, 0, 255); B_fin = capValue(B_fin, 0, 255); temp[ii3] = (byte) L_fin; temp[ii3 + 1] = (byte) A_fin; temp[ii3 + 2] = (byte) B_fin; ii3 += 3; } imgMat.put(i, 0, temp); } return imgMat; } catch (Exception e) { throw new CalibrationException("error while performing calibration: ", e); } }
From source file:org.cirdles.calamari.algorithms.WeightedMeanCalculators.java
public static WeightedLinearCorrResults weightedLinearCorr(double[] y, double[] x, double[][] sigmaRhoY) { WeightedLinearCorrResults weightedLinearCorrResults = new WeightedLinearCorrResults(); RealMatrix omega = new BlockRealMatrix(convertCorrelationsToCovariances(sigmaRhoY)); RealMatrix invOmega = MatrixUtils.inverse(omega); int n = y.length; double mX = 0; double pX = 0; double pY = 0; double pXY = 0; double w = 0; for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { double invOm = invOmega.getEntry(i, j); w += invOm;//from w ww .j av a 2 s . c o m pX += (invOm * (x[i] + x[j])); pY += (invOm * (y[i] + y[j])); pXY += (invOm * (((x[i] * y[j]) + (x[j] * y[i])))); mX += (invOm * x[i] * x[j]); } } double slope = ((2 * pXY * w) - (pX * pY)) / ((4 * mX * w) - (pX * pX)); double intercept = (pY - (slope * pX)) / (2 * w); RealMatrix fischer = new BlockRealMatrix(new double[][] { { mX, pX / 2.0 }, { pX / 2.0, w } }); RealMatrix fischerInv = MatrixUtils.inverse(fischer); double slopeSig = StrictMath.sqrt(fischerInv.getEntry(0, 0)); double interceptSig = StrictMath.sqrt(fischerInv.getEntry(1, 1)); double slopeInterceptCov = fischerInv.getEntry(0, 1); RealMatrix resid = new BlockRealMatrix(n, 1); for (int i = 0; i < n; i++) { resid.setEntry(i, 0, y[i] - (slope * x[i]) - intercept); } RealMatrix residT = resid.transpose(); RealMatrix mM = residT.multiply(invOmega).multiply(resid); double sumSqWtdResids = mM.getEntry(0, 0); double mswd = sumSqWtdResids / (n - 2); // http://commons.apache.org/proper/commons-math/apidocs/org/apache/commons/math3/distribution/FDistribution.html FDistribution fdist = new org.apache.commons.math3.distribution.FDistribution((n - 2), 1E9); double prob = 1.0 - fdist.cumulativeProbability(mswd); weightedLinearCorrResults.setBad(false); weightedLinearCorrResults.setSlope(slope); weightedLinearCorrResults.setIntercept(intercept); weightedLinearCorrResults.setSlopeSig(slopeSig); weightedLinearCorrResults.setInterceptSig(interceptSig); weightedLinearCorrResults.setSlopeInterceptCov(slopeInterceptCov); weightedLinearCorrResults.setMswd(mswd); weightedLinearCorrResults.setProb(prob); return weightedLinearCorrResults; }