CMatrix
CMatrix::getInverse(
) const
{
double det;
det = getDeterminant();
if(fabs(det) < _EPSILON)
throw;
return CMatrix(
getSubDeterminant(0,0) / det,
-getSubDeterminant(1,0) / det,
getSubDeterminant(2,0) / det,
-getSubDeterminant(3,0) / det,
-getSubDeterminant(0,1) / det,
getSubDeterminant(1,1) / det,
-getSubDeterminant(2,1) / det,
getSubDeterminant(3,1) / det,
getSubDeterminant(0,2) / det,
-getSubDeterminant(1,2) / det,
getSubDeterminant(2,2) / det,
-getSubDeterminant(3,2) / det,
-getSubDeterminant(0,3) / det,
getSubDeterminant(1,3) / det,
-getSubDeterminant(2,3) / det,
getSubDeterminant(3,3) / det
);
}
Mat3< T > Mat3< T >::getInverse() const {
const T invDet = static_cast< T >(1.0) / getDeterminant();
return Mat3(
e_[Y1] * e_[W2] - e_[Y2] * e_[W1],
e_[Y2] * e_[W0] - e_[Y0] * e_[W2],
e_[Y0] * e_[W1] - e_[Y1] * e_[W0],
e_[X2] * e_[W1] - e_[X1] * e_[W2],
e_[X0] * e_[W2] - e_[X2] * e_[W0],
e_[X1] * e_[W0] - e_[X0] * e_[W1],
e_[X1] * e_[Y2] - e_[X2] * e_[Y1],
e_[X2] * e_[Y0] - e_[X0] * e_[Y2],
e_[X0] * e_[Y1] - e_[X1] * e_[Y0]
) * invDet;
}
void Matrix2::invert()
{
/*
2x2 Matrix inversion is done with the formula
[ a b ]
[ c d ]
1 / det * [d -b][-c a]
*/
assert(fabs(getDeterminant()) > 0.000001f);
float invDet = 1 / getDeterminant();
float a = components[0];
float b = components[1];
float c = components[2];
float d = components[3];
set( d, -b,
-c, a);
componentMultiplyAndAssign(invDet);
}
///////////////////////////////////////////////////////////////////////////////
// inverse of 2x2 matrix
// If cannot find inverse, set identity matrix
///////////////////////////////////////////////////////////////////////////////
Matrix2& Matrix2::invert()
{
float determinant = getDeterminant();
/* if(fabs(determinant) ==0)
determinant=EPSILON;*/
float tmp = m[0]; // copy the first element
float invDeterminant = 1.0f / determinant;
m[0] = invDeterminant * m[3];
m[1] = -invDeterminant * m[1];
m[2] = -invDeterminant * m[2];
m[3] = invDeterminant * tmp;
return *this;
}
void Matrix3::invert()
{
//See http://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3.C3.973_matrices
//for the algorithm used here to invert 3x3 matrices.
float det = getDeterminant();
assert(fabs(det) > 0.000001f);
float invdet = 1 / det;
//With
//a=0 b=1 c=2
//d=3 e=4 f=5
//g=6 h=7 k=8
//ek - fh
float a = components[4] * components[8] - components[5] * components[7];
//ch - bk
float b = components[2] * components[7] - components[1] * components[8];
//bf - ce
float c = components[1] * components[5] - components[2] * components[4];
//fg - dk
float d = components[5] * components[6] - components[3] * components[8];
//ak - cg
float e = components[0] * components[8] - components[2] * components[6];
//cd - af
float f = components[2] * components[3] - components[0] * components[5];
//dh - eg
float g = components[3] * components[7] - components[4] * components[6];
//bg - ah
float h = components[1] * components[6] - components[0] * components[7];
//ae - bd
float k = components[0] * components[4] - components[1] * components[3];
set(a, b, c,
d, e, f,
g, h, k);
componentMultiplyAndAssign(invdet);
}
// -------------------------------------------------------------------------- //
// inverse of 2x2 matrix
// If cannot find inverse, set identity matrix
// -------------------------------------------------------------------------- //
Matrix2& Matrix2::invert()
{
float determinant = getDeterminant();
if(fabs(determinant) <= EPSILON)
{
return identity();
}
float tmp = m[0]; // copy the first element
float invDeterminant = 1.0f / determinant;
m[0] = invDeterminant * m[3];
m[1] = -invDeterminant * m[1];
m[2] = -invDeterminant * m[2];
m[3] = invDeterminant * tmp;
return *this;
}
Transform Transform::getInverse()
{
Transform inverse;
const double det = getDeterminant();
inverse[0] = (m_values[4] * m_values[8] - m_values[7] * m_values[5]) / det;
inverse[1] = -(m_values[1] * m_values[8] - m_values[7] * m_values[2]) / det;
inverse[2] = (m_values[1] * m_values[5] - m_values[4] * m_values[2]) / det;
inverse[3] = -(m_values[3] * m_values[8] - m_values[6] * m_values[5]) / det;
inverse[4] = (m_values[0] * m_values[8] - m_values[6] * m_values[2]) / det;
inverse[5] = -(m_values[0] * m_values[5] - m_values[3] * m_values[2]) / det;
inverse[6] = (m_values[3] * m_values[7] - m_values[6] * m_values[4]) / det;
inverse[7] = -(m_values[0] * m_values[7] - m_values[6] * m_values[1]) / det;
inverse[8] = (m_values[0] * m_values[4] - m_values[3] * m_values[1]) / det;
return inverse;
}
void Matrix4x4::invert()
{
float x = getDeterminant();
std::vector<float> i(16, 0.0);
i[0]= (-elements[13]*elements[10]*elements[7] +elements[9]*elements[14]*elements[7] +elements[13]*elements[6]*elements[11]
-elements[5]*elements[14]*elements[11] -elements[9]*elements[6]*elements[15] +elements[5]*elements[10]*elements[15])/x;
i[4]= ( elements[12]*elements[10]*elements[7] -elements[8]*elements[14]*elements[7] -elements[12]*elements[6]*elements[11]
+elements[4]*elements[14]*elements[11] +elements[8]*elements[6]*elements[15] -elements[4]*elements[10]*elements[15])/x;
i[8]= (-elements[12]*elements[9]* elements[7] +elements[8]*elements[13]*elements[7] +elements[12]*elements[5]*elements[11]
-elements[4]*elements[13]*elements[11] -elements[8]*elements[5]*elements[15] +elements[4]*elements[9]* elements[15])/x;
i[12]=( elements[12]*elements[9]* elements[6] -elements[8]*elements[13]*elements[6] -elements[12]*elements[5]*elements[10]
+elements[4]*elements[13]*elements[10] +elements[8]*elements[5]*elements[14] -elements[4]*elements[9]* elements[14])/x;
i[1]= ( elements[13]*elements[10]*elements[3] -elements[9]*elements[14]*elements[3] -elements[13]*elements[2]*elements[11]
+elements[1]*elements[14]*elements[11] +elements[9]*elements[2]*elements[15] -elements[1]*elements[10]*elements[15])/x;
i[5]= (-elements[12]*elements[10]*elements[3] +elements[8]*elements[14]*elements[3] +elements[12]*elements[2]*elements[11]
-elements[0]*elements[14]*elements[11] -elements[8]*elements[2]*elements[15] +elements[0]*elements[10]*elements[15])/x;
i[9]= ( elements[12]*elements[9]* elements[3] -elements[8]*elements[13]*elements[3] -elements[12]*elements[1]*elements[11]
+elements[0]*elements[13]*elements[11] +elements[8]*elements[1]*elements[15] -elements[0]*elements[9]* elements[15])/x;
i[13]=(-elements[12]*elements[9]* elements[2] +elements[8]*elements[13]*elements[2] +elements[12]*elements[1]*elements[10]
-elements[0]*elements[13]*elements[10] -elements[8]*elements[1]*elements[14] +elements[0]*elements[9]* elements[14])/x;
i[2]= (-elements[13]*elements[6]* elements[3] +elements[5]*elements[14]*elements[3] +elements[13]*elements[2]*elements[7]
-elements[1]*elements[14]*elements[7] -elements[5]*elements[2]*elements[15] +elements[1]*elements[6]* elements[15])/x;
i[6]= ( elements[12]*elements[6]* elements[3] -elements[4]*elements[14]*elements[3] -elements[12]*elements[2]*elements[7]
+elements[0]*elements[14]*elements[7] +elements[4]*elements[2]*elements[15] -elements[0]*elements[6]* elements[15])/x;
i[10]=(-elements[12]*elements[5]* elements[3] +elements[4]*elements[13]*elements[3] +elements[12]*elements[1]*elements[7]
-elements[0]*elements[13]*elements[7] -elements[4]*elements[1]*elements[15] +elements[0]*elements[5]* elements[15])/x;
i[14]=( elements[12]*elements[5]* elements[2] -elements[4]*elements[13]*elements[2] -elements[12]*elements[1]*elements[6]
+elements[0]*elements[13]*elements[6] +elements[4]*elements[1]*elements[14] -elements[0]*elements[5]* elements[14])/x;
i[3]= ( elements[9]* elements[6]* elements[3] -elements[5]*elements[10]*elements[3] -elements[9]* elements[2]*elements[7]
+elements[1]*elements[10]*elements[7] +elements[5]*elements[2]*elements[11] -elements[1]*elements[6]* elements[11])/x;
i[7]= (-elements[8]* elements[6]* elements[3] +elements[4]*elements[10]*elements[3] +elements[8]* elements[2]*elements[7]
-elements[0]*elements[10]*elements[7] -elements[4]*elements[2]*elements[11] +elements[0]*elements[6]* elements[11])/x;
i[11]=( elements[8]* elements[5]* elements[3] -elements[4]*elements[9]* elements[3] -elements[8]* elements[1]*elements[7]
+elements[0]*elements[9]* elements[7] +elements[4]*elements[1]*elements[11] -elements[0]*elements[5]* elements[11])/x;
i[15]=(-elements[8]* elements[5]* elements[2] +elements[4]*elements[9]* elements[2] +elements[8]* elements[1]*elements[6]
-elements[0]*elements[9]* elements[6] -elements[4]*elements[1]*elements[10] +elements[0]*elements[5]* elements[10])/x;
for(size_t j = 0; j < 16; ++j)
{
elements[j] = i[j];
}
}
void newtonMethod(double *guess, double *a_final, double *Vds, double *Vgs,
double *Smeas, int paramSize, int measSize, int task)
{
if(guess == NULL || Vds == NULL || Vgs == NULL || Smeas == NULL)
return;
double da[paramSize];
double a_temp[paramSize], a_temp2[paramSize];
for(int i = 0; i < paramSize; i++)
{
a_final[i] = guess[i];
a_temp[i] = guess[i];
a_temp2[i] = guess[i];
}
int stop = 0, count;
//double dV1, dV2;
//double H[SIZE_PARAM][SIZE_PARAM], invH[SIZE_PARAM][SIZE_PARAM]; //Hessian
double delt_a[paramSize];
int topCnt = 0;
double **H = new double*[paramSize];
double **invH = new double*[paramSize];
double J[paramSize];
for(int i = 0; i < paramSize; i++)
{
H[i] = new double[paramSize];
invH[i] = new double[paramSize];
for(int j = 0; j < paramSize; j++)
{
H[i][j] = 0;
invH[i][j] = 0;
}
}
while(stop == 0)
{
cout << a_final[0] << " " << a_final[1] << " " << a_final[2] << endl;
// estimating new perturb values to calculate Hessian
for(int i = 0; i < paramSize; i++)
{
da[i] = a_final[i] * PERTURB_NUM;
}
// calculating the Hessian using central difference (finite difference)
// http://www.math.unl.edu/~s-bbockel1/833-notes/node23.html
for(int i = 0; i < paramSize; i++)
{
// for the diagonal elements
a_temp[i] += da[i];
a_temp2[i] -= da[i];
H[i][i] = (generateV(a_temp2, Vds, Vgs, Smeas, paramSize, measSize, task)
- 2.0*generateV(a_final, Vds, Vgs, Smeas, paramSize, measSize, task)
+ generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task))
/ (pow(da[i],2.0));
J[i] = (generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task)
- generateV(a_temp2, Vds, Vgs, Smeas, paramSize, measSize, task)) / (2.0*da[i]);
a_temp[i] = a_final[i];
a_temp2[i] = a_final[i];
// for non-diagonal elements
for(int j = 0; j < paramSize; j++)
{
if(i != j)
{
// f(x+h, y+h)
a_temp[i] += da[i];
a_temp[j] += da[j];
H[i][j] = generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task);
// f(x+h, y-h)
a_temp[j] -= da[j] * 2.0;
H[i][j] -= generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task);
// f(x-h, y-h)
a_temp[i] -= da[i] * 2.0;
H[i][j] += generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task);
// f(x-h, y+h)
a_temp[j] += da[j] * 2.0;
H[i][j] -= generateV(a_temp, Vds, Vgs, Smeas, paramSize, measSize, task);
// 4h^2
H[i][j] /= (4.0 * da[i] * da[j]);
// resetting parameters for next iteration
a_temp[i] = a_final[i];
a_temp[j] = a_final[j];
}
}
}
cout << "HESSIAN: ";
cout << H[0][0] << " " << H[0][1] << " " << H[0][2] << endl
<< " " << H[1][0] << " " << H[1][1] << " " << H[1][2] << endl
<< " " << H[2][0] << " " << H[2][1] << " " << H[2][2] << endl;
// calculating the new parameters
//calculate the inverse of Hessian
if(getDeterminant(H, paramSize) == 0 || isnan(H[0][0]))
{
cout << "newton: det(H) is invalid" << endl;
stop = 1;
}
else{
inverse(H, paramSize, invH);
cout << "INVERSE: ";
cout << invH[0][0] << " " << invH[0][1] << " " << invH[0][2] << endl
<< " " << invH[1][0] << " " << invH[1][1] << " " << invH[1][2] << endl
<< " " << invH[2][0] << " " << invH[2][1] << " " << invH[2][2] << endl;
cout << "JACOBIAN: ";
cout << J[0] << " " << J[1] << " " << J[2] << endl;
//matrix multiplication
count = 0;
for(int i = 0; i < paramSize; i++)
{
delt_a[i] = 0;
for(int j = 0; j < paramSize; j++)
{
delt_a[i] += invH[i][j] * J[j];
}
a_final[i] -= delt_a[i];
a_temp[i] = a_final[i];
a_temp2[i] = a_final[i];
//a_temp3[i] = a_final[i];
//linear search
/**
min = 10000;
for(double j = 1.0; j >= 0.25; j -= 0.25)
{
a_temp1[i] -= j * delt_a[i];
V_temp = generateV(a_temp1, x, Smeas, paramSize, measSize);
a_temp1[i] = a_final[i];
cout << V_temp << " ";
if(abs(V_temp) < abs(min))
{
min = V_temp;
delt_a_final[i] = j * delt_a[i];
}
}*/
if(abs(delt_a[i]) < THRESHOLD)
{
count++;
}
}
cout << "delt_a: " << delt_a[0] << " " << delt_a[1] << " " << delt_a[2] << endl;
cout << "PARAMS: " << a_final[0] << " " << a_final[1] << " " << a_final[2];
cout << endl << endl;
if(count >= paramSize)
{
stop = 1;
}
}
topCnt++;
}
cout << "newton: number of iterations = " << topCnt << endl;
// deallocating assigned memory
for(int i = 0; i < paramSize; i++)
{
delete [] H[i];
delete [] invH[i];
}
delete [] H;
delete [] invH;
H = NULL;
invH = NULL;
}
double ObjectiveFunction::getDeterminant(Matrix<double> matrix)
{
double determinant = 0.0;
int numberOfRows = matrix.getNumberOfRows();
int numberOfColumns = matrix.getNumberOfColumns();
if(numberOfRows != numberOfColumns)
{
std::cout << "Error: NewtonMethod class. "
<< "getDeterminant(Matrix<double>) method." << std::endl
<< "Matrix must be square" << std::endl
<< std::endl;
exit(1);
}
if(numberOfRows == 0)
{
std::cout << "Error: NewtonMethod class. "
<< "getDeterminant(Matrix<double>) method." << std::endl
<< "Size of matrix is zero." << std::endl
<< std::endl;
exit(1);
}
else if(numberOfRows == 1)
{
// std::cout << "Warning: NewtonMethod class. "
// << "getDeterminant(Matrix<double>) method." << std::endl
// << "Size of matrix is one." << std::endl;
determinant = matrix[0][0];
}
else if(numberOfRows == 2)
{
determinant = matrix[0][0]*matrix[1][1] - matrix[1][0]*matrix[0][1];
}
else
{
for(int j1 = 0; j1 < numberOfRows; j1++)
{
Matrix<double> subMatrix(numberOfRows-1, numberOfColumns-1, 0.0);
for(int i = 1; i < numberOfRows; i++)
{
int j2 = 0;
for (int j = 0; j < numberOfColumns; j++)
{
if (j == j1)
{
continue;
}
subMatrix[i-1][j2] = matrix[i][j];
j2++;
}
}
determinant += pow(-1.0, j1+2.0)*matrix[0][j1]*getDeterminant(subMatrix);
}
}
return(determinant);
}
Matrix<double> ObjectiveFunction::getInverseHessian(Vector<double> argument)
{
Matrix<double> inverseHessian(numberOfVariables, numberOfVariables, 0.0);
Matrix<double> hessian = getHessian(argument);
double hessianDeterminant = getDeterminant(hessian);
if(hessianDeterminant == 0.0)
{
std::cout << "Error: ObjectiveFunction class. "
<< "Matrix<double> getInverseHessian(Vector<double>) method."
<< std::endl
<< "Hessian matrix is singular." << std::endl
<< std::endl;
exit(1);
}
// Get cofactor matrix
Matrix<double> cofactor(numberOfVariables, numberOfVariables, 0.0);
Matrix<double> c(numberOfVariables-1, numberOfVariables-1, 0.0);
for(int j = 0; j < numberOfVariables; j++)
{
for (int i = 0; i < numberOfVariables; i++)
{
// Form the adjoint a_ij
int i1 = 0;
for(int ii = 0; ii < numberOfVariables; ii++)
{
if(ii == i)
{
continue;
}
int j1 = 0;
for(int jj = 0; jj < numberOfVariables; jj++)
{
if (jj == j)
{
continue;
}
c[i1][j1] = hessian[ii][jj];
j1++;
}
i1++;
}
double determinant = getDeterminant(c);
cofactor[i][j] = pow(-1.0, i+j+2.0)*determinant;
}
}
// Adjoint matrix is the transpose of cofactor matrix
Matrix<double> adjoint(numberOfVariables, numberOfVariables, 0.0);
double temp = 0.0;
for(int i = 0; i < numberOfVariables; i++)
{
for (int j = 0; j < numberOfVariables; j++)
{
adjoint[i][j] = cofactor[j][i];
}
}
// Inverse matrix is adjoint matrix divided by matrix determinant
for(int i = 0; i < numberOfVariables; i++)
{
for(int j = 0; j < numberOfVariables; j++)
{
inverseHessian[i][j] = adjoint[i][j]/hessianDeterminant;
}
}
return(inverseHessian);
}
