/**
* * 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");
}
}
@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));
}
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
}
/**
* 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));
}
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;
}
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;
}
@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);
}
@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;
}
/**
* 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;
}
@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;
}
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;
}
/** 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");
*/
}
@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());
}
@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());
}
/**
* 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;
}
/**
* 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;
}
/**
* 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);
}
public MatrixObject scalarMultiply(int val) {
return new MatrixObject(matrix.times(val));
}
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;
}