DenseMatrix64F dphi_dq() { double err = 2 / (dcm22 * dcm22 + dcm12 * dcm12); double[][] m = { {q1 * dcm22}, {q0 * dcm22 + 2 * q1 * dcm12}, {q3 * dcm22 + 2 * q2 * dcm12}, {q2 * dcm22} }; DenseMatrix64F mErr = new DenseMatrix64F(m); CommonOps.scale(err, mErr); return mErr; // Fix this to work in place without allocating... }
/** * Since the sign of the homography is ambiguous a point is required to make sure the correct * one was selected. * * @param p test point, used to determine the sign of the matrix. */ protected void adjustHomographSign( AssociatedPair p , DenseMatrix64F H ) { double val = GeometryMath_F64.innerProd(p.p2, H, p.p1); if( val < 0 ) CommonOps.scale(-1, H); }
@SuppressWarnings("unused") private void timeDerivative(DenseMatrix64F spatialVectorRateToPack, DenseMatrix64F spatialVector, DenseMatrix64F previousSpatialVector) { CommonOps.subtract(spatialVector, previousSpatialVector, spatialVectorRateToPack); CommonOps.scale(1.0 / controlDT, spatialVectorRateToPack); }
DenseMatrix64F dpsi_dq() { double err = 2 / (dcm00 * dcm00 + dcm01 * dcm01); double[][] m = { {q3 * dcm00}, {q2 * dcm00}, {q1 * dcm00 + 2 * q2 * dcm01}, {q0 * dcm00 + 2 * q3 * dcm01} }; DenseMatrix64F mErr = new DenseMatrix64F(m); CommonOps.scale(err, mErr); return mErr; // Fix this to work in place without allocating... }
DenseMatrix64F dphi_dq() { double err = 2 / (dcm22 * dcm22 + dcm12 * dcm12); double[][] m = { {q1 * dcm22}, {q0 * dcm22 + 2 * q1 * dcm12}, {q3 * dcm22 + 2 * q2 * dcm12}, {q2 * dcm22} }; DenseMatrix64F mErr = new DenseMatrix64F(m); CommonOps.scale(err, mErr); return mErr; // Fix this to work in place without allocating... }
public void packLinearVelocityTermForPosition(DenseMatrix64F block, QuaternionReadOnly orientation, double dt) { orientation.get(tempRotationMatrix); tempRotationMatrix.get(block); CommonOps.scale(dt, block); }
/** * Get the covariance matrix corresponding to the dataset added beforehand. * @return the 3-by-3 covariance matrix. */ public DenseMatrix64F getCovariance() { DenseMatrix64F covariance = new DenseMatrix64F(3, 3); double div = 1.0 / (double) (sampleSize); covariance.set(secondMoment); CommonOps.scale(div, covariance); return covariance; }
private void updateSolution(double tau) { // update solution CommonOps.scale(tau, step); CommonOps.addEquals(solutionInfo.getSolution(), step); }
private void updateSolution(double tau) { // update solution CommonOps.scale(tau, step); CommonOps.addEquals(solutionInfo.getSolution(), step); }
public DenseMatrix64F getMeasurementCovarianceMatrixBlock() { scaledMeasurementCovarianceMatrixBlock.set(measurementCovarianceMatrixBlock); CommonOps.scale(covarianceMatrixScaling.getDoubleValue(), scaledMeasurementCovarianceMatrixBlock); return scaledMeasurementCovarianceMatrixBlock; }
public DenseMatrix64F getProcessCovarianceMatrixBlock() { scaledProcessNoiseCovarianceMatrixBlock.set(processNoiseCovarianceBlock); CommonOps.scale(covarianceMatrixScaling.getDoubleValue(), scaledProcessNoiseCovarianceMatrixBlock); return scaledProcessNoiseCovarianceMatrixBlock; }
public static void numericallyDifferentiate(DenseMatrix64F derivativeToPack, DenseMatrix64F previousMatrixToUpdate, DenseMatrix64F newMatrix, double dt) { derivativeToPack.set(newMatrix); CommonOps.subtractEquals(derivativeToPack, previousMatrixToUpdate); CommonOps.scale(1.0 / dt, derivativeToPack); previousMatrixToUpdate.set(newMatrix); }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(3, 3); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; } }
@Override public double getObjectiveCost(DenseMatrix64F x) { multQuad(x, quadraticCostQMatrix, computedObjectiveFunctionValue); CommonOps.scale(0.5, computedObjectiveFunctionValue); CommonOps.multAddTransA(quadraticCostQVector, x, computedObjectiveFunctionValue); return computedObjectiveFunctionValue.get(0, 0) + quadraticCostScalar; }
@Override public void getQMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(size, size); CommonOps.setIdentity(matrixToPack); CommonOps.scale(variance.getValue() * sqrtHz, matrixToPack); }
private void initializeCovariance() { DenseMatrix64F x = kalmanFilter.getState(); kalmanFilter.computeSteadyStateGainAndCovariance(50); // TODO: magic number DenseMatrix64F P = kalmanFilter.getCovariance(); CommonOps.scale(10.0, P); // TODO: magic number kalmanFilter.setState(x, P); }
@Override public void getRMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getMeasurementSize(), getMeasurementSize()); CommonOps.setIdentity(matrixToPack); CommonOps.scale(variance.getValue() * sqrtHz, matrixToPack); }
private void printSystemMatrices(String name, DenseMatrix64F A, DenseMatrix64F B, DenseMatrix64F Q, DenseMatrix64F R, double deltaT) { DenseMatrix64F QDividedByDeltaT = new DenseMatrix64F(Q); CommonOps.scale(1.0 / (deltaT), QDividedByDeltaT); printIfDebug(name); printIfDebug("A = " + A); printIfDebug("B = " + B); printIfDebug("Scaled Q = " + QDividedByDeltaT); printIfDebug("R = " + R); }