Пример #1
0
  /**
   * * Least-square solution y = X * b where: y_i = b_0 + b_1*x_1i + b_2*x_2i + ... + b_k*x_ki
   * including intercep term y_i = b_1*x_1i + b_2*x_2i + ... + b_k*x_ki without intercep term
   *
   * @param datay
   * @param dataX
   */
  private void multipleLinearRegression(Matrix datay, Matrix dataX) {
    Matrix X, y;
    try {
      X = dataX;
      y = datay;
      b = X.solve(y);
      coeffs = new double[b.getRowDimension()];
      for (int j = 0; j < b.getRowDimension(); j++) {
        coeffs[j] = b.get(j, 0);
        // System.out.println("coeff[" + j + "]=" + coeffs[j]);
      }

      // Residuals:
      Matrix r = X.times(b).minus(y);
      residuals = r.getColumnPackedCopy();
      // root mean square error (RMSE)
      rmse = Math.sqrt(MathUtils.sumSquared(residuals) / residuals.length);

      // Predicted values
      Matrix p = X.times(b);
      predictedValues = p.getColumnPackedCopy();

      // Correlation between original values and predicted ones
      correlation = MathUtils.correlation(predictedValues, y.getColumnPackedCopy());

    } catch (RuntimeException re) {
      throw new Error("Error solving Least-square solution: y = X * b");
    }
  }
Пример #2
0
 @Override
 public CalcObject multiply(CalcObject b) {
   if (b.getType() == CType.INTEGER) {
     IntObject bInt = (IntObject) b;
     return new MatrixObject(matrix.times(bInt.getValue()));
   } else if (b.getType() == CType.MATRIX) {
     MatrixObject bMatrix = (MatrixObject) b;
     return new MatrixObject(matrix.times(bMatrix.matrix));
   } else {
     throw new IllegalArgumentException("Bad object type: " + b.getType());
   }
 }
  /*
   * Calculates second moments matrix square root
   */
  float calcSecondMomentSqrt(AbstractStructureTensorIPD ipd, Pixel p, Matrix Mk) {
    Matrix M, V, eigVal, Vinv;

    M = calcSecondMomentMatrix(ipd, p.x, p.y);

    /* *
     * M = V * D * V.inv()
     * V has eigenvectors as columns
     * D is a diagonal Matrix with eigenvalues as elements
     * V.inv() is the inverse of V
     * */
    //		EigenvalueDecomposition meig = M.eig();
    EigenValueVectorPair meig = MatrixUtils.symmetricEig2x2(M);
    eigVal = meig.getValues();
    V = meig.getVectors();

    //		V = V.transpose();
    Vinv = V.inverse();

    double eval1 = Math.sqrt(eigVal.get(0, 0));
    eigVal.set(0, 0, eval1);
    double eval2 = Math.sqrt(eigVal.get(1, 1));
    eigVal.set(1, 1, eval2);

    // square root of M
    Mk.setMatrix(0, 1, 0, 1, V.times(eigVal).times(Vinv));

    // return q isotropic measure
    return (float) (Math.min(eval1, eval2) / Math.max(eval1, eval2));
  }
Пример #4
0
  private Matrix walk() throws IOException {

    // Step 0. Initialise transition probabilities
    this.logger.showTimedMessage("Initialise transition probabilities");
    Matrix P = this.initialiseTransitionProbabilities();

    logger.showMemoryUsage();

    // 2. initialise random walkers.
    // keep in mind that this makes sense because of the way the indexes were
    // loaded into this.goTermIndex. Otherwise, we would have to retrieve
    // from this.goTermIndex
    Matrix W = new Matrix(this.getNumGoTerms(), this.getNumGoTerms());
    for (int i = 0; i < this.getNumGoTerms(); i++) {
      W.set(i, i, 1.0f);
    }

    // walk!
    Matrix W_star = W.copy();
    double convergence;
    // int i=0;
    do {
      W = W_star;
      W_star = P.times(W);
      convergence = W_star.minus(W).normF();
      this.logger.showTimedMessage(
          "\t Convergence difference: " + (new Double(convergence)).toString());
      // this.logger.showTimedMessage("\t Doing iteration i=" + i);
      //  i++;
      // }while (i<15);
    } while (convergence > this.epsilon);

    return W_star;
  }
  float normMaxEval(Matrix U, Matrix uVal, Matrix uVec) {
    /* *
     * Decomposition:
     * U = V * D * V.inv()
     * V has eigenvectors as columns
     * D is a diagonal Matrix with eigenvalues as elements
     * V.inv() is the inverse of V
     * */
    //		uVec = uVec.transpose();
    Matrix uVinv = uVec.inverse();

    // Normalize min eigenvalue to 1 to expand patch in the direction of min eigenvalue of U.inv()
    double uval1 = uVal.get(0, 0);
    double uval2 = uVal.get(1, 1);

    if (Math.abs(uval1) < Math.abs(uval2)) {
      uVal.set(0, 0, 1);
      uVal.set(1, 1, uval2 / uval1);

    } else {
      uVal.set(1, 1, 1);
      uVal.set(0, 0, uval1 / uval2);
    }

    // U normalized
    U.setMatrix(0, 1, 0, 1, uVec.times(uVal).times(uVinv));

    return (float)
        (Math.max(Math.abs(uVal.get(0, 0)), Math.abs(uVal.get(1, 1)))
            / Math.min(
                Math.abs(uVal.get(0, 0)),
                Math.abs(uVal.get(1, 1)))); // define the direction of warping
  }
Пример #6
0
  /**
   * Computes the pseudo-inverse of a matrix using Singular Value Decomposition
   *
   * @param M - the input {@link Matrix}
   * @param threshold - the threshold for inverting diagonal elements (suggested 0.001)
   * @return the inverted {@link Matrix} or an approximation with lowest possible squared error
   */
  public static final Matrix computePseudoInverseMatrix(final Matrix M, final double threshold) {
    final SingularValueDecomposition svd = new SingularValueDecomposition(M);

    Matrix U = svd.getU(); // U Left Matrix
    final Matrix S = svd.getS(); // W
    final Matrix V = svd.getV(); // VT Right Matrix

    double temp;

    // invert S
    for (int j = 0; j < S.getRowDimension(); ++j) {
      temp = S.get(j, j);

      if (temp < threshold) // this is an inaccurate inverting of the matrix
      temp = 1.0 / threshold;
      else temp = 1.0 / temp;

      S.set(j, j, temp);
    }

    // transponse U
    U = U.transpose();

    //
    // compute result
    //
    return ((V.times(S)).times(U));
  }
Пример #7
0
    private Matrix SqrtSPKF(Matrix PDash) {
      // Only works with a symmetric Positive Definite matrix
      // [V,D] = eig(A)
      // S = V*diag(sqrt(diag(D)))*V'
      //
      // //PDash in
      // System.out.println("PDash: ");
      // PDash.print(3, 2);

      // Matrix NegKeeper = Matrix.identity(9, 9);
      // //Testing Forced Compliance
      // for(int i = 0; i< 9; i++){
      // if (PDash.get(i, i)< 0){
      // NegKeeper.set(i,i,-1);
      // PDash.set(i, i, -PDash.get(i, i));
      // }
      // }
      EigenvalueDecomposition eig = PDash.eig();
      Matrix V = eig.getV();
      Matrix D = eig.getD();
      int iDSize = D.getRowDimension();
      for (int i = 0; i < iDSize; i++) {
        D.set(i, i, Math.sqrt(D.get(i, i)));
      }
      Matrix S = V.times(D).times(V.inverse());
      // S = S.times(NegKeeper);
      return S;
    }
  /**
   * Computes the euclidean centroid, it may not be the formal centroid for different metrics but
   * intuitively the average counts should provide a good cluster representative which is what this
   * method intends to return
   *
   * @param pwmSet - Collection of pwms from which to compute the centroid0
   * @return The euclidean centroid
   * @throws IllegalArgumentException - When not all PWMs have the same dimension.
   */
  public PositionWeightMatrix centroidOf(Collection<PositionWeightMatrix> pwmSet)
      throws IllegalArgumentException {
    Matrix centroidMatrix = null;
    Iterator<PositionWeightMatrix> pwmIt = pwmSet.iterator();
    if (pwmIt.hasNext()) {
      PositionWeightMatrix first = pwmIt.next();
      PositionWeightColumn firstCol = first.get(0);
      centroidMatrix = new Matrix(firstCol.getAlphabetSize(), first.getNumCol());
      addToCentroid(centroidMatrix, first);
    }
    while (pwmIt.hasNext()) {
      PositionWeightMatrix pwm = pwmIt.next();
      if (pwm.getNumCol() != centroidMatrix.getColumnDimension()) {
        throw new IllegalArgumentException(
            "Error computing centroid. All PWMs in set should have the same dimension");
      }
      addToCentroid(centroidMatrix, pwm);
    }

    centroidMatrix.times(1 / (double) pwmSet.size());

    PositionWeightMatrix centroid = new PositionWeightMatrix("centroid");
    for (int j = 0; j < centroidMatrix.getColumnDimension(); j++) {
      centroid.addColumn(centroidMatrix.getColumn(j));
    }
    return centroid;
  }
  /**
   * 将X'T*X'的特征向量 转换成 X'*X'T的特征向量 的方法
   *
   * @param eigenValue为特征值
   * @param featureVec为特征向量
   */
  public static double[][] changeFeatureVector() {
    double[][] AveDeviation = Features.getInstance().getAveDev(); // 每幅图的均差
    double[] eigenValue = Features.getInstance().getEigenValue(); // 特征值
    double[][] featureVec = Features.getInstance().getFeatureVector(); // 特征向量

    //		System.out.println("width*height:" + width*height);//4096
    //		System.out.println("featureVec[0].length" + featureVec[0].length);//10
    double[][] resultFeatureVector = new double[width * height][featureVec[0].length];

    for (int i = 0; i < featureVec[0].length; i++) {
      // 将N副图片的特征向量[N][N]分离提取出来,提取第i张图片的特征向量[N][1]
      double[][] temp = new double[featureVec.length][1];
      for (int j = 0; j < featureVec.length; j++) {
        temp[j][0] = featureVec[j][i];
      }

      Matrix xOld = new Matrix(AveDeviation);
      Matrix featureVecOfOneObject = new Matrix(temp);
      //			System.out.println("[" + xOld.getRowDimension() + "]" +
      //					"[" + xOld.getColumnDimension() +"]");//[16384][10]
      //			System.out.println("[" + featureVecOfOneObject.getRowDimension() + "]" +
      //					"[" + featureVecOfOneObject.getColumnDimension() +"]");//[10][1]

      Matrix resultMatrix = xOld.times(featureVecOfOneObject);
      //			System.out.println("[" + resultMatrix.getRowDimension() + "]" +
      //					"[" + resultMatrix.getColumnDimension() +"]");

      double factor = 1 / (Math.sqrt(eigenValue[i]));
      resultMatrix.times(factor);

      double[][] temp2 = new double[1][1];
      temp2 = resultMatrix.getArray();

      //			System.out.println("temp2.length:" + temp2.length);//16384-------------
      //			System.out.println("temp2[0].length:" + temp2[0].length);//1
      // 将构成的[N][1] 复制到[N][i]中
      for (int j = 0; j < temp2.length; j++) {
        resultFeatureVector[j][i] = temp2[j][0];
      }
    }

    Features.getInstance().setResultFeatureVector(resultFeatureVector);
    return resultFeatureVector;
  }
Пример #10
0
  private double[] metNewton(double R0, double Z0) {
    Matrix actuel = new Matrix(2, 1);
    Matrix prochain = new Matrix(2, 1);
    Matrix hessienne = new Matrix(2, 2);
    Matrix gradient = new Matrix(2, 1);
    Matrix inverse = new Matrix(2, 2);
    int nPas = 0;
    boolean trouve = false;
    // point de depart
    actuel.set(0, 0, R0);
    actuel.set(1, 0, Z0);

    while (nPas <= MAX_PAS && trouve == false) {
      // Construction de la matrice hessienne au point actuel
      hessienne = makeHessienne(R0, Z0);
      // Test si matrice singulaire
      if (hessienne.det() == 0) {
        return null;
      }
      // Calcul de la matrice inverse
      inverse = hessienne.inverse();
      // Construction du gradient au point actuel
      gradient.set(0, 0, d_phi_d_R(actuel.get(0, 0), actuel.get(1, 0)));
      gradient.set(1, 0, d_phi_d_Z(actuel.get(0, 0), actuel.get(1, 0)));
      // Calcul du prochain point
      prochain = actuel.minus(inverse.times(gradient));

      // Test de la solution
      if (Math.pow(
              Math.pow(prochain.get(0, 0) - actuel.get(0, 0), 2)
                  + Math.pow(prochain.get(1, 0) - actuel.get(1, 0), 2),
              0.5)
          < TOLERANCE) {
        // Solution trouvé
        trouve = true;
      }

      // Actualisation du point actuel
      actuel = prochain;
      nPas++;
    }

    if (trouve == true) // Si point trouvé, renvoie la réponse
    {
      // Conversion de Matrix vers double[]
      double[] reponse = new double[2];
      reponse[0] = actuel.get(0, 0);
      reponse[1] = actuel.get(1, 0);

      return reponse;
    }
    // Renvoie 0 si la méthode ne converge pas
    return null;
  }
Пример #11
0
 @Override
 public CalcObject divide(CalcObject b) {
   if (b.getType() == CType.INTEGER) {
     IntObject bInt = (IntObject) b;
     return new MatrixObject(matrix.times(1.0 / ((double) bInt.getValue())));
   } else if (b.getType() == CType.MATRIX) {
     MatrixObject bMatrix = (MatrixObject) b;
     throw new UnsupportedOperationException("Have not implemented / for matricies yet");
   } else {
     throw new IllegalArgumentException("Bad object type: " + b.getType());
   }
 }
  /* ERROR DETERMINATION */
  private void determineError() {
    double var_est = 1 / ((double) getDegreesOfFreedom()) * sumOfSquares();

    Matrix J = calcJ();
    Matrix JT = J.transpose();
    Matrix err = (JT.times(J)).inverse();
    covar = err.times(var_est);
    param_stdDev = new double[numFittable];
    double[][] matrixVals = new double[numFittable][numFittable];
    for (int i = 0; i < numFittable; i++) {
      param_stdDev[i] = Math.sqrt(covar.get(i, i));
      matrixVals[i][i] = param_stdDev[i];
    }
    for (int i = 0; i < numFittable; i++) {
      for (int j = i; j < numFittable; j++) {
        matrixVals[i][j] = covar.get(i, j) / (matrixVals[i][i] * matrixVals[j][j]);
      }
    }

    corr = new Matrix(matrixVals);
  }
Пример #13
0
  @Override
  public double getFunctionValue(Matrix x) {
    Matrix err = coefficients.times(x).minus(y);
    for (int i = 0; i < n; i++) {
      err.set(i, 0, Math.pow(err.get(i, 0), 2));
    }

    double value = 0.0;
    for (int i = 0; i < n; i++) {
      value += err.get(i, 0);
    }
    return value;
  }
Пример #14
0
  /**
   * rotate the given vector around our center with given direction and angle
   *
   * @param vector the vector to rotate
   * @param dir direction to take (clockwise, counter-clockwise)
   * @param angle in radians
   * @return the rotated vector
   */
  public static Vector2d rotateVector(
      final Vector2d vector, final RotationDirection dir, final double angle) {

    // rotMatrix * vector
    Matrix rotMatrix = getRotationMatrix(angle, dir);
    Matrix pointAsMatrix = tupleToMatrix(vector);
    Matrix result = rotMatrix.times(pointAsMatrix);

    // translate back
    Vector2d target = matrixToVector(result);

    return target;
  }
Пример #15
0
  @Override
  public Matrix getGradientValue(Matrix x) {
    Matrix err = coefficients.times(x).minus(y).times(2);
    Matrix grad = new Matrix(n, 1);
    for (int i = 0; i < n; i++) {
      double value = 0.0;
      for (int j = 0; j < n; j++) {
        value += err.get(j, 0) * coefficients.get(j, i);
      }
      grad.set(i, 0, value);
    }

    return grad;
  }
  /**
   * Update the covariance Matrix of the weight posterior distribution (SIGMA) along with its
   * cholesky factor:
   *
   * <p>SIGMA = (A + beta * PHI^t * PHI)^{-1}
   *
   * <p>SIGMA_chol with SIGMA_chol * SIGMA_chol^t = SIGMA
   */
  protected void updateSIGMA() {

    Matrix SIGMA_inv = PHI_t.times(PHI_t.transpose());
    SIGMA_inv.timesEquals(beta);
    SIGMA_inv.plusEquals(A);

    /** Update the factor ... */
    SECholeskyDecomposition CD = new SECholeskyDecomposition(SIGMA_inv.getArray());
    Matrix U = CD.getPTR().times(CD.getL());
    SIGMA_chol = U.inverse();

    /** Update SIGMA */
    SIGMA = (SIGMA_chol.transpose()).times(SIGMA_chol);
  }
  /**
   * Compute the scalars s_m, q_m which are part of the criterium for inclusion / deletion of the
   * given basis m:
   *
   * <p>S_m = beta * phi^t_m * phi_m - beta^2 * phi^t_m * PHI * SIGMA * PHI^t * phi_m Q_m = beta *
   * phi^t_m * t - beta^2 * phi^t_m * PHI * SIGMA * PHI^t * t
   *
   * <p>s_m = alpha_m * S_m / (alpha_m - S_m) q_m = alpha_m * Q_m / (alpha_m - S_m)
   */
  protected void updateCriteriumScalars(int selectedBasis) {

    Matrix SigmaStuff = (PHI_t.transpose()).times(SIGMA.times(PHI_t));

    double S =
        beta * innerProduct(phi[selectedBasis], phi[selectedBasis])
            - beta
                * beta
                * innerProduct(
                    phi[selectedBasis],
                    SigmaStuff.times(new Matrix(phi[selectedBasis], phi[selectedBasis].length))
                        .getRowPackedCopy());

    double Q =
        beta * innerProduct(phi[selectedBasis], tVector)
            - beta
                * beta
                * innerProduct(
                    phi[selectedBasis], SigmaStuff.times(new Matrix(t)).getRowPackedCopy());

    s = alpha[selectedBasis] * S / (alpha[selectedBasis] - S);
    q = alpha[selectedBasis] * Q / (alpha[selectedBasis] - S);
  }
  private static Matrix rotatedMatrix(Matrix m, Matrix r) {
    Matrix translationMatrix = new Matrix(m.getRowDimension(), m.getColumnDimension());
    for (int c = 0; c < translationMatrix.getColumnDimension(); c++) {
      // T

      // x
      translationMatrix.set(0, c, 2.0);
      // y
      translationMatrix.set(1, c, 3.0);
      // z
      // translationMatrix.set(2, c, 2.0);
    }

    Matrix rotatedMatrix = r.times(m.minus(translationMatrix)).plus(translationMatrix);
    return rotatedMatrix;
  }
Пример #19
0
 public static PImage invert(Matrix H, PImage img, Point dstDimension) {
   PImage res = new PImage(dstDimension.x, dstDimension.y);
   //		PImage res = new PImage(img.width, img.height);
   for (int x = 0; x < img.width; x++) {
     for (int y = 0; y < img.height; y++) {
       // Point inverse theorique (x0,y0) ?
       double[][] q_array = {{x}, {y}, {1}};
       Matrix P = H.times(new Matrix(q_array));
       double x0 = P.get(0, 0) / P.get(2, 0);
       double y0 = P.get(1, 0) / P.get(2, 0);
       // S'il est hors de l'image originale, on rend le pixel transparant.
       if (x0 < 0 || x0 >= img.width || y0 < 0 || y0 >= img.height) res.set(x, y, 0x00000000);
       else res.set(x, y, img.get((int) x0, (int) y0));
     }
   }
   return res;
 }
Пример #20
0
  /** toy example */
  public static void test2() {
    int N = 500;
    double[][] m1 = new double[N][N];
    double[][] m2 = new double[N][N];
    double[][] m3 = new double[N][N];

    // init
    Random rand = new Random();
    for (int i = 0; i < N; i++)
      for (int j = 0; j < N; j++) {
        m1[i][j] = 10 * (rand.nextDouble() - 0.2);
        m2[i][j] = 20 * (rand.nextDouble() - 0.8);
      }

    // inverse
    System.out.println("Start");
    Matrix mat1 = new Matrix(m1);
    Matrix mat2 = mat1.inverse();
    Matrix mat3 = mat1.times(mat2);
    double[][] m4 = mat3.getArray();
    /*
       for (int i = 0; i < m4.length; i++) {
         int ss = 10;
         for (int j = 0; j < ss; j++) {
    System.out.printf("%f ", m4[i][j]);
         }
         System.out.print("\n");
       }
       */
    System.out.println("Done");

    /*
        // matrix *
        System.out.println("Start");
        for (int i = 0; i < N; i++) for (int j = 0; j < N; j++) {
          double cell = 0;
          for (int k = 0; k < N; k++)
    	cell += m1[i][k] * m2[k][j];
    //      System.out.printf("%f ", cell);
          m3[i][j] = cell;
        }
        System.out.println("Done");
        */
  }
Пример #21
0
      @Override
      public void render(
          final MBFImageRenderer renderer, final Matrix transform, final Rectangle rectangle) {
        if (this.toRender == null) {
          this.toRender =
              new XuggleVideo(
                  VideoColourSIFT.class.getResource("/org/openimaj/demos/video/keyboardcat.flv"),
                  true);
          this.renderToBounds =
              TransformUtilities.makeTransform(
                  new Rectangle(0, 0, this.toRender.getWidth(), this.toRender.getHeight()),
                  rectangle);
        }

        final MBFProjectionProcessor mbfPP = new MBFProjectionProcessor();
        mbfPP.setMatrix(transform.times(this.renderToBounds));
        mbfPP.accumulate(this.toRender.getNextFrame());
        mbfPP.performProjection(0, 0, renderer.getImage());
      }
Пример #22
0
      @Override
      public void render(
          final MBFImageRenderer renderer, final Matrix transform, final Rectangle rectangle) {
        if (this.toRender == null) {
          try {
            this.toRender =
                ImageUtilities.readMBF(
                    VideoColourSIFT.class.getResource("/org/openimaj/demos/OpenIMAJ.png"));
          } catch (final IOException e) {
            System.err.println("Can't load image to render");
          }
          this.renderToBounds =
              TransformUtilities.makeTransform(this.toRender.getBounds(), rectangle);
        }

        final MBFProjectionProcessor mbfPP = new MBFProjectionProcessor();
        mbfPP.setMatrix(transform.times(this.renderToBounds));
        mbfPP.accumulate(this.toRender);
        mbfPP.performProjection(0, 0, renderer.getImage());
      }
Пример #23
0
  /**
   * rotate the given point around our center with given direction and angle
   *
   * @param point the point to rotate
   * @param center the point around which we want to rotate
   * @param dir direction to take (clockwise, counter-clockwise)
   * @param angle in radians
   * @return the rotated point
   */
  public static Point2d rotatePoint(
      final Point2d point, final Point2d center, final RotationDirection dir, final double angle) {

    // copy point
    Point2d p = new Point2d(point);

    // translate so that our center is the origin
    p.sub(center);

    // rotMatrix * point
    Matrix rotMatrix = getRotationMatrix(angle, dir);
    Matrix pointAsMatrix = tupleToMatrix(p);
    Matrix result = rotMatrix.times(pointAsMatrix);

    // translate back
    Point2d target = matrixToPoint(result);
    target.add(center);

    return target;
  }
Пример #24
0
  /**
   * use the online model to predict the target position given the positions measured at the bpms
   */
  public void performAnalysis(final List<BPM> bpms) throws Exception {
    final int measurementCount = VIEW_SCREEN_MEASUREMENTS.size();
    double meanDifference = 0.0; // TODO: CKA - NEVER USED
    final Matrix diag = new Matrix(measurementCount, 2);
    final Matrix viewScreenMeas = new Matrix(measurementCount, 1);
    final List<TargetAnalysisResultRecord> rawResults =
        new ArrayList<TargetAnalysisResultRecord>(measurementCount);
    int row = 0;
    for (final ViewScreenMeasurement measurement : VIEW_SCREEN_MEASUREMENTS) {
      final TargetAnalysisResultRecord rawResult = predictWithMatcher(bpms, measurement);
      rawResults.add(rawResult);
      final double measuredPosition = rawResult.getMeasuredPosition();
      viewScreenMeas.set(row, 0, measuredPosition);
      diag.set(row, 0, 1.0);
      diag.set(row, 1, rawResult.getPredictedPosition());
      meanDifference += rawResult.getMeasuredPosition() - rawResult.getPredictedPosition();
      ++row;
    }
    meanDifference /= measurementCount;
    final Matrix diagT = diag.transpose();
    final Matrix coef = diagT.times(diag).inverse().times(diagT).times(viewScreenMeas);
    final double offset = coef.get(0, 0);
    final double scale = coef.get(1, 0);

    final List<TargetAnalysisResultRecord> results = new ArrayList<TargetAnalysisResultRecord>();
    double errorSum = 0.0;
    for (final TargetAnalysisResultRecord rawResult : rawResults) {
      final double predictedPosition = scale * rawResult.getPredictedPosition() + offset;
      final double measuredPosition = rawResult.getMeasuredPosition();
      final TargetAnalysisResultRecord result =
          new TargetAnalysisResultRecord(measuredPosition, predictedPosition);
      results.add(result);
      errorSum += (measuredPosition - predictedPosition) * (measuredPosition - predictedPosition);
    }
    final double rmsError = Math.sqrt(errorSum / rawResults.size());

    _scale = scale;
    _offset = offset;
    _rmsError = rmsError;
  }
Пример #25
0
  /**
   * Given an activation 'act', joints angles and speed, compute the tension applied on every joint.
   *
   * @param act in [0,1]
   * @param angles (rad)
   * @param angSpeed (rad/s)
   * @return Torque (1xnb_joint) for each joint.
   */
  public Matrix computeTorque(Matrix act, Matrix angles, Matrix angSpeed) {

    // longueur du muscle
    _ln = computeLength(angles);
    // vitesse du muscle
    _vn = computeSpeed(angles, angSpeed);
    // tension (force)
    for (int col = 0; col < _tn.getColumnDimension(); col++) {
      assert act.get(0, col) >= 0.0 : "_act < 0";
      assert act.get(0, col) <= 1.0 : "_act > 1";
      double t =
          funA(act.get(0, col), _ln.get(0, col))
              * (funFl(_ln.get(0, col)) * funFv(_ln.get(0, col), _vn.get(0, col))
                  + funFp(_ln.get(0, col)));
      _tn.set(0, col, t);
    }
    // Real tension
    _t = _tn.arrayTimes(_sec).times(31.8);
    // Torque
    _cpl = _t.times(_mom);

    return _cpl;
  }
  /** 计算协方差矩阵 传入形参为 <方法:计算每张图片的均差> 的返回值 (其实这并不是协方差矩阵,若需要协方差矩阵,再用该矩阵除以N即可) */
  public static double[][] calCovarianceMatrix(double[][] vec) {
    int length = vec.length; // length指向量的维数,X={1,2,3,...,length}
    int n = vec[0].length; // n指向量的个数,X1,X2,X3...Xn;

    Matrix vecOld = new Matrix(vec);

    //		System.out.println("vecOld:");
    //		vecOld.print(6, 2);

    //		System.out.println();
    Matrix vecTrans = vecOld.transpose(); // 获取转置矩阵
    //		System.out.println("vecTrans:");
    //		vecTrans.print(6, 2);

    double[][] covArr = new double[nOld][nOld]; // 初始化协方差矩阵

    // 构造协方差矩阵
    //		Matrix cov = vecOld.times(vecTrans);
    Matrix cov = vecTrans.times(vecOld);

    covArr = cov.getArray();

    // 由于最终计算并不需要协方差矩阵,仅仅要的是一部分,所以不用除以N
    // 除以N
    //		for(int i=0; i<n; i++)
    //		{
    //			for(int j=0; j<length; j++)
    //			{
    //				covArr[i][j] = covArr[i][j] / n;
    //			}
    //		}

    Features.getInstance().setCovarianceMatrix(covArr);

    return covArr;
  }
  @Override
  public void updateEstimate(DiscreteMarginalValues<MultiballVariableState> observedValues) {
    iteration++;
    iteration %= iterationSkips;
    // turn variableDomain into an array of X values

    Set<MultiballVariableState> states = observedValues.getValues().keySet();
    xVals = new double[states.size()];
    stateArray = new MultiballVariableState[xVals.length];

    int i = 0;
    for (MultiballVariableState state : states) {
      xVals[i] = state.getValue();
      stateArray[i] = state;
      i++;
    }
    // turn marginalFunction into an array of Y values and t values

    yVals = new double[xVals.length];
    tVals = new double[xVals.length];

    for (i = 0; i < xVals.length; i++) {
      yVals[i] = observedValues.getValue(stateArray[i]);
      tVals[i] = getAndIncrementTVal(stateArray[i]);
    }

    memory.add(xVals, yVals, tVals);

    if ((iteration % iterationSkips) != 0) return;

    Matrix transform = HybridGPMSTransform.generate(memory.iterationNum);

    Matrix trainY = transform.times(memory.Y);

    // create Gaussian process Bayesian Markov chain to estimate worth of setting
    // states to new values

    GaussianProcessRegressionBMC regression = new GaussianProcessRegressionBMC();
    regression.setCovarianceFunction(
        new HybridGPMSCovarianceFunction(
            SquaredExponentialCovarianceFunction.getInstance(), transform, noise));

    BasicPrior[] priors = new BasicPrior[priorMeans.length];
    for (i = 0; i < priorMeans.length; i++) {
      priors[i] = new BasicPrior(priorSampleCounts[i], priorMeans[i], priorStandardDevs[i]);
    }

    regression.setPriors(priors);

    predictor = regression.calculateRegression(memory.X, trainY);

    worstCurrentVal = Double.POSITIVE_INFINITY;

    for (i = 0; i < xVals.length; i++) {
      double worth = evaluate(xVals[i], tVals[i] + 1);
      if (worth < worstCurrentVal) {
        worstCurrentVal = worth;
        worstCurrentIndex = i;
      }
    }

    bestSoFar = worstCurrentVal;
  }
 /**
  * Update the mean of the weight posterior distribution (mu):
  *
  * <p>mu = beta * SIGMA * PHI^t * t
  */
 protected void updateMu() {
   mu = SIGMA.times(PHI_t.times(new Matrix(t)));
   mu.timesEquals(beta);
 }
Пример #29
0
 public MatrixObject scalarMultiply(int val) {
   return new MatrixObject(matrix.times(val));
 }
Пример #30
0
    private void RunSPKF() {
      // SPKF Steps:
      // 1) Generate Test Points
      // 2) Propagate Test Points
      // 3) Compute Predicted Mean and Covariance
      // 4) Compute Measurements
      // 5) Compute Innovations and Cross Covariance
      // 6) Compute corrections and update

      // Line up initial variables from the controller!
      Double dAlpha = dGreek.get(0);
      Double dBeta = dGreek.get(1);
      cController.setAlpha(dAlpha);
      cController.setBeta(dBeta);
      cController.setKappa(dGreek.get(2));
      Double dGamma = cController.getGamma();
      Double dLambda = cController.getLambda();

      // // DEBUG - Print the Greeks
      // System.out.println("Greeks!");
      // System.out.println("Alpha - " + dAlpha);
      // System.out.println("Beta - " + dBeta);
      // System.out.println("Kappa - " + dGreek.get(2));
      // System.out.println("Lambda - " + dLambda);
      // System.out.println("Gamma - " + dGamma);

      // Let's get started:
      // Step 1: Generate Test Points
      Vector<Matrix> Chi = new Vector<Matrix>();
      Vector<Matrix> UpChi = new Vector<Matrix>();
      Vector<Matrix> UpY = new Vector<Matrix>();
      Matrix UpPx = new Matrix(3, 3, 0.0);
      Matrix UpPy = new Matrix(3, 3, 0.0);
      Matrix UpPxy = new Matrix(3, 3, 0.0);
      Matrix K;
      Vector<Double> wc = new Vector<Double>();
      Vector<Double> wm = new Vector<Double>();
      Chi.add(X); // Add Chi_0 - the current state estimate (X, Y, Z)

      // Big P Matrix is LxL diagonal
      Matrix SqrtP = SqrtSPKF(P);
      SqrtP = SqrtP.times(dGamma);

      // Set up Sigma Points
      for (int i = 0; i <= 8; i++) {
        Matrix tempVec = SqrtP.getMatrix(0, 8, i, i);
        Matrix tempX = X;
        Matrix tempPlus = tempX.plus(tempVec);
        // System.out.println("TempPlus");
        // tempPlus.print(3, 2);
        Matrix tempMinu = tempX.minus(tempVec);
        // System.out.println("TempMinus");
        // tempMinu.print(3, 2);
        // tempX = X.copy();
        // tempX.setMatrix(i, i, 0, 2, tempPlus);
        Chi.add(tempPlus);
        // tempX = X.copy();
        // tempX.setMatrix(i, i, 0, 2, tempMinu);
        Chi.add(tempMinu);
      }

      // DEBUG Print the lines inside the Chi Matrix (2L x L)
      // for (int i = 0; i<=(2*L); i++){
      // System.out.println("Chi Matrix Set: "+i);
      // Chi.get(i).print(5, 2);
      // }

      // Generate weights
      Double WeightZero = (dLambda / (L + dLambda));
      Double OtherWeight = (1 / (2 * (L + dLambda)));
      Double TotalWeight = WeightZero;
      wm.add(WeightZero);
      wc.add(WeightZero + (1 - (dAlpha * dAlpha) + dBeta));
      for (int i = 1; i <= (2 * L); i++) {
        TotalWeight = TotalWeight + OtherWeight;
        wm.add(OtherWeight);
        wc.add(OtherWeight);
      }
      // Weights MUST BE 1 in total
      for (int i = 0; i <= (2 * L); i++) {
        wm.set(i, wm.get(i) / TotalWeight);
        wc.set(i, wc.get(i) / TotalWeight);
      }

      // //DEBUG Print the weights
      // System.out.println("Total Weight:");
      // System.out.println(TotalWeight);
      // for (int i = 0; i<=(2*L); i++){
      // System.out.println("Weight M for "+i+" Entry");
      // System.out.println(wm.get(i));
      // System.out.println("Weight C for "+i+" Entry");
      // System.out.println(wc.get(i));
      // }

      // Step 2: Propagate Test Points
      // This will also handle computing the mean
      Double ux = dControl.elementAt(0);
      Double uy = dControl.elementAt(1);
      Double uz = dControl.elementAt(2);
      Matrix XhatMean = new Matrix(3, 1, 0.0);
      for (int i = 0; i < Chi.size(); i++) {
        Matrix ChiOne = Chi.get(i);
        Matrix Chixminus = new Matrix(3, 1, 0.0);
        Double Xhat = ChiOne.get(0, 0);
        Double Yhat = ChiOne.get(1, 0);
        Double Zhat = ChiOne.get(2, 0);
        Double Xerr = ChiOne.get(3, 0);
        Double Yerr = ChiOne.get(4, 0);
        Double Zerr = ChiOne.get(5, 0);

        Xhat = Xhat + ux + Xerr;
        Yhat = Yhat + uy + Yerr;
        Zhat = Zhat + uz + Zerr;

        Chixminus.set(0, 0, Xhat);
        Chixminus.set(1, 0, Yhat);
        Chixminus.set(2, 0, Zhat);
        // System.out.println("ChixMinus:");
        // Chixminus.print(3, 2);
        UpChi.add(Chixminus);
        XhatMean = XhatMean.plus(Chixminus.times(wm.get(i)));
      }

      // Mean is right!

      // System.out.println("XhatMean: ");
      // XhatMean.print(3, 2);

      // Step 3: Compute Predicted Mean and Covariance
      // Welp, we already solved the mean - let's do the covariance now
      for (int i = 0; i <= (2 * L); i++) {
        Matrix tempP = UpChi.get(i).minus(XhatMean);
        Matrix tempPw = tempP.times(wc.get(i));
        tempP = tempPw.times(tempP.transpose());
        UpPx = UpPx.plus(tempP);
      }

      // New Steps!

      // Step 4: Compute Measurements! (and Y mean!)
      Matrix YhatMean = new Matrix(3, 1, 0.0);
      for (int i = 0; i <= (2 * L); i++) {
        Matrix ChiOne = Chi.get(i);
        Matrix Chiyminus = new Matrix(3, 1, 0.0);
        Double Xhat = UpChi.get(i).get(0, 0);
        Double Yhat = UpChi.get(i).get(1, 0);
        Double Zhat = UpChi.get(i).get(2, 0);
        Double Xerr = ChiOne.get(6, 0);
        Double Yerr = ChiOne.get(7, 0);
        Double Zerr = ChiOne.get(8, 0);

        Xhat = Xhat + Xerr;
        Yhat = Yhat + Yerr;
        Zhat = Zhat + Zerr;

        Chiyminus.set(0, 0, Xhat);
        Chiyminus.set(1, 0, Yhat);
        Chiyminus.set(2, 0, Zhat);
        UpY.add(Chiyminus);
        YhatMean = YhatMean.plus(Chiyminus.times(wm.get(i)));
      }

      // // Welp, we already solved the mean - let's do the covariances
      // now
      // System.out.println("XHatMean and YHatMean = ");
      // XhatMean.print(3, 2);
      // YhatMean.print(3, 2);

      for (int i = 0; i <= (2 * L); i++) {
        Matrix tempPx = UpChi.get(i).minus(XhatMean);
        Matrix tempPy = UpY.get(i).minus(YhatMean);
        // System.out.println("ChiX - XhatMean and ChiY-YhatMean");
        // tempPx.print(3, 2);
        // tempPy.print(3, 2);

        Matrix tempPxw = tempPx.times(wc.get(i));
        Matrix tempPyw = tempPy.times(wc.get(i));

        tempPx = tempPxw.times(tempPy.transpose());
        tempPy = tempPyw.times(tempPy.transpose());
        UpPy = UpPy.plus(tempPy);
        UpPxy = UpPxy.plus(tempPx);
      }

      // Step 6: Compute Corrections and Update

      // Compute Kalman Gain!
      // System.out.println("Updated Px");
      // UpPx.print(5, 2);
      // System.out.println("Updated Py");
      // UpPy.print(5, 2);
      // System.out.println("Updated Pxy");
      // UpPxy.print(5, 2);
      K = UpPxy.times(UpPy.inverse());
      // System.out.println("Kalman");
      // K.print(5, 2);

      Matrix Mea = new Matrix(3, 1, 0.0);
      Mea.set(0, 0, dMeasure.get(0));
      Mea.set(1, 0, dMeasure.get(1));
      Mea.set(2, 0, dMeasure.get(2));

      Matrix Out = K.times(Mea.minus(YhatMean));
      Out = Out.plus(XhatMean);
      // System.out.println("Out:");
      // Out.print(3, 2);

      Matrix Px = UpPx.minus(K.times(UpPy.times(K.transpose())));

      // Update Stuff!
      // Push the P to the controller
      Matrix OutP = P.copy();
      OutP.setMatrix(0, 2, 0, 2, Px);
      X.setMatrix(0, 2, 0, 0, Out);

      Residual = XhatMean.minus(Out);
      cController.inputState(OutP, Residual);
      // cController.setL(L);

      cController.startProcess();
      while (!cController.finishedProcess()) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }

      // System.out.println("Post Greeks: " + cController.getAlpha() + " ,
      // "+ cController.getBeta());

      dGreek.set(0, cController.getAlpha());
      dGreek.set(1, cController.getBeta());
      dGreek.set(2, cController.getKappa());
      P = cController.getP();

      // System.out.println("P is post Process:");
      // P.print(3, 2);

      StepDone = true;
    }