void fit_quadratic_form(float qf[3], unsigned int N, const float phi[N], const float v[N])
{
complex float lhs[3] = { 0., 0., 0. };
complex float mat[3][3] = { { 0. } };
for (unsigned int i = 0; i < N; i++) {
float x = cosf(phi[i]);
float y = sinf(phi[i]);
lhs[0] += x * x * v[i];
lhs[1] += y * y * v[i];
lhs[2] += 2. * x * y * v[i];
mat[0][0] += x * x * x * x;
mat[0][1] += x * x * y * y;
mat[0][2] += x * x * 2. * x * y;
mat[1][0] += y * y * x * x;
mat[1][1] += y * y * y * y;
mat[1][2] += y * y * 2. * x * y;
mat[2][0] += 2. * x * y * x * x;
mat[2][1] += 2. * x * y * y * y;
mat[2][2] += 2. * x * y * 2. * x * y;
}
complex float inv[3][3];
complex float out[3];
mat_inverse(3, inv, mat);
mat_vecmul(3, 3, out, inv, lhs);
qf[0] = out[0];
qf[1] = out[1];
qf[2] = out[2];
}
/*
* generic matrix inverse code
*
* @param x, input nxn matrix
* @param y, Output inverted nxn matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool inverse(float x[], float y[], uint16_t dim)
{
switch(dim){
case 3: return inverse3x3(x,y);
case 4: return inverse4x4(x,y);
default: return mat_inverse(x,y,dim);
}
}
void icaTransformInverse(double** S, int rows, int cols, int comp, int comp_rm,
double** X, double** K, double** W, double** RowData, int startEb,
int stopEb) {
double **A;
double *Total;
FILE * OutputFile;
int i, j;
A = mat_create(comp, comp);
Total = malloc(cols * sizeof(double));
for (j = 0; j < cols; j++) {
Total[j] = 0;
for (i = 0; i < rows; i++)
Total[j] = Total[j] + RowData[i][j];
Total[j] = Total[j] / rows;
}
puts("Before");
for (j = startEb; j < stopEb; j++)
//for (j = 40; j < 90; j++)
S[j][comp_rm] = 0;//////////////////////////////////////////////////////////////////////////////////
puts("Debug");
mat_mult(K, cols, comp, W, comp, comp, A); //A[cols, comp]
mat_inverse(A, comp, W);
mat_mult(S, rows, comp, W, comp, comp, X);
//strcat(pPathFile, "_recontruction");
OutputFile = fopen("/home/truongnh/eeg-lab411/SignalProcessing/RemoveEyeblink/Data/Output.txt",
"wb");
for (i = 0; i < rows; i++) {
for (j = 0; j < cols; j++) {
if(j == comp_rm)
fprintf(OutputFile, "%f\n", X[i][j] + Total[j]);
}
//fprintf(OutputFile, "\n");
}
fclose(OutputFile);
for (j = 0; j < cols; j++) {
for (i = 0; i < rows; i++)
X[i][j] = X[i][j] + Total[j];
}
free(Total);
}
// [RING] Calculate Pseudoinverse
static void calc_pinv(unsigned int Nint, complex float pinv[3][2 * Nint], const complex float A[2 * Nint][3])
{
//AH
complex float AH[3][2 * Nint];
mat_transpose(2 * Nint, 3, AH, A);
// (AH * A)^-1
complex float dot[3][3];
mat_mul(3, 2 * Nint, 3, dot, AH, A);
complex float inv[3][3];
mat_inverse(3, inv, dot);
// (AH * A)^-1 * AH
mat_mul(3, 3, 2 * Nint, pinv, inv, AH);
return;
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S)
{
mat XT, V, TU, D, X1, _A;
vect scale, d;
// matrix creation
XT = mat_create(cols, rows);
X1 = mat_create(compc, rows);
V = mat_create(cols, cols);
D = mat_create(cols, cols);
TU = mat_create(cols, cols);
scale = vect_create(cols);
d = vect_create(cols);
/*
* CENTERING
*/
mat_center(X, rows, cols, scale);
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT);
mat_apply_fx(X, rows, cols, fx_div_c, rows);
mat_mult(XT, cols, rows, X, rows, cols, V);
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D);
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU,cols, cols);
vect_delete(scale);
vect_delete(d);
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S) {
mat XT, V, TU, D, X1, _A;
vect scale, d;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
//chu thich voi truong hop 14 kenh, 2s (256mau) du lieu, 14 thanh phan doc lap>>> cols = 14, rows = 256, compc = 14
// matrix creation
XT = mat_create(cols, rows); //14x256
X1 = mat_create(compc, rows); //14x256
V = mat_create(cols, cols); //14x14
D = mat_create(cols, cols); //14x14
TU = mat_create(cols, cols); //14x14
scale = vect_create(cols); //14
d = vect_create(cols); //14
clock1 = clock();
/*
* CENTERING
*/
mat_center(X, rows, cols, scale); //tru di gia tri trung binh cua moi cot
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "wb");
//fprintf(Log, "CENTERING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT); //XT la chuyen vi cua ma tran X[256][14] >>> XT[14][256]
mat_apply_fx(X, rows, cols, fx_div_c, rows); //lay tung gia tri cua X[i][j] chia cho 14
mat_mult(XT, cols, rows, X, rows, cols, V); //V=XT*X >>>V[14][14]
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "WHITENING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "FASTICA %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D); //ham nay tinh mat tran ngich dao
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU, cols, cols);
vect_delete(scale);
vect_delete(d);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "OUTPUT %f \n", time);
//fclose(Log);
}
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
{
//Run Sphere Fit using Levenberg Marquardt LSq Fit
const float lma_damping = 10.0f;
float _samples_collected = size;
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float JTJ[81];
float JTJ2[81];
float JTFI[9];
float residual = 0.0f;
memset(JTJ, 0, sizeof(JTJ));
memset(JTJ2, 0, sizeof(JTJ2));
memset(JTFI, 0, sizeof(JTFI));
float ellipsoid_jacob[9];
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < _samples_collected; k++) {
//Calculate Jacobian
float A = (*diag_x * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z));
float B = (*offdiag_x * (x[k] - *offset_x)) + (*diag_y * (y[k] - *offset_y)) + (*offdiag_z * (z[k] - *offset_z));
float C = (*offdiag_y * (x[k] - *offset_x)) + (*offdiag_z * (y[k] - *offset_y)) + (*diag_z * (z[k] - *offset_z));
float length = sqrtf(A * A + B * B + C * C);
residual = *sphere_radius - length;
fit1 += residual * residual;
// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[0] = 1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C)) / length);
ellipsoid_jacob[1] = 1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C)) / length);
ellipsoid_jacob[2] = 1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C)) / length);
// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[3] = -1.0f * ((x[k] + *offset_x) * A) / length;
ellipsoid_jacob[4] = -1.0f * ((y[k] + *offset_y) * B) / length;
ellipsoid_jacob[5] = -1.0f * ((z[k] + *offset_z) * C) / length;
// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
ellipsoid_jacob[6] = -1.0f * (((y[k] + *offset_y) * A) + ((x[k] + *offset_x) * B)) / length;
ellipsoid_jacob[7] = -1.0f * (((z[k] + *offset_z) * A) + ((x[k] + *offset_x) * C)) / length;
ellipsoid_jacob[8] = -1.0f * (((z[k] + *offset_z) * B) + ((y[k] + *offset_y) * C)) / length;
for (uint8_t i = 0; i < 9; i++) {
// compute JTJ
for (uint8_t j = 0; j < 9; j++) {
JTJ[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; //a backup JTJ for LM
}
JTFI[i] += ellipsoid_jacob[i] * residual;
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
float fit1_params[9] = {*offset_x, *offset_y, *offset_z, *diag_x, *diag_y, *diag_z, *offdiag_x, *offdiag_y, *offdiag_z};
float fit2_params[9];
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
for (uint8_t i = 0; i < 9; i++) {
JTJ[i * 9 + i] += _sphere_lambda;
JTJ2[i * 9 + i] += _sphere_lambda / lma_damping;
}
if (!mat_inverse(JTJ, JTJ, 9)) {
return -1;
}
if (!mat_inverse(JTJ2, JTJ2, 9)) {
return -1;
}
for (uint8_t row = 0; row < 9; row++) {
for (uint8_t col = 0; col < 9; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col];
fit2_params[row] -= JTFI[col] * JTJ2[row * 9 + col];
}
}
//Calculate mean squared residuals
for (uint16_t k = 0; k < _samples_collected; k++) {
float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
(z[k] - fit1_params[2]));
float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] *
(z[k] - fit1_params[2]));
float C = (fit1_params[7] * (x[k] - fit1_params[0])) + (fit1_params[8] * (y[k] - fit1_params[1])) + (fit1_params[5] *
(z[k] - fit1_params[2]));
float length = sqrtf(A * A + B * B + C * C);
residual = *sphere_radius - length;
fit1 += residual * residual;
A = (fit2_params[3] * (x[k] - fit2_params[0])) + (fit2_params[6] * (y[k] - fit2_params[1])) + (fit2_params[7] *
(z[k] - fit2_params[2]));
B = (fit2_params[6] * (x[k] - fit2_params[0])) + (fit2_params[4] * (y[k] - fit2_params[1])) + (fit2_params[8] *
(z[k] - fit2_params[2]));
C = (fit2_params[7] * (x[k] - fit2_params[0])) + (fit2_params[8] * (y[k] - fit2_params[1])) + (fit2_params[5] *
(z[k] - fit2_params[2]));
length = sqrtf(A * A + B * B + C * C);
residual = *sphere_radius - length;
fit2 += residual * residual;
}
fit1 = sqrtf(fit1) / _samples_collected;
fit2 = sqrtf(fit2) / _samples_collected;
if (fit1 > _fitness && fit2 > _fitness) {
_sphere_lambda *= lma_damping;
} else if (fit2 < _fitness && fit2 < fit1) {
_sphere_lambda /= lma_damping;
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
fitness = fit2;
} else if (fit1 < _fitness) {
fitness = fit1;
}
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
if (PX4_ISFINITE(fitness) && fitness < _fitness) {
_fitness = fitness;
*offset_x = fit1_params[0];
*offset_y = fit1_params[1];
*offset_z = fit1_params[2];
*diag_x = fit1_params[3];
*diag_y = fit1_params[4];
*diag_z = fit1_params[5];
*offdiag_x = fit1_params[6];
*offdiag_y = fit1_params[7];
*offdiag_z = fit1_params[8];
return 0;
} else {
return -1;
}
}
