void SMACOF::run( const Matrix<double>& Weights, const Matrix<double>& Distances, const double& tol, const unsigned& maxloops, Matrix<double>& InitialZ ) {
unsigned M = Distances.nrows();
// Calculate V
Matrix<double> V(M,M); double totalWeight=0.;
for(unsigned i=0; i<M; ++i) {
for(unsigned j=0; j<M; ++j) {
if(i==j) continue;
V(i,j)=-Weights(i,j);
if( j<i ) totalWeight+=Weights(i,j);
}
for(unsigned j=0; j<M; ++j) {
if(i==j)continue;
V(i,i)-=V(i,j);
}
}
// And pseudo invert V
Matrix<double> mypseudo(M, M); pseudoInvert(V, mypseudo);
Matrix<double> dists( M, M ); double myfirstsig = calculateSigma( Weights, Distances, InitialZ, dists ) / totalWeight;
// initial sigma is made up of the original distances minus the distances between the projections all squared.
Matrix<double> temp( M, M ), BZ( M, M ), newZ( M, InitialZ.ncols() );
for(unsigned n=0; n<maxloops; ++n) {
if(n==maxloops-1) plumed_merror("ran out of steps in SMACOF algorithm");
// Recompute BZ matrix
for(unsigned i=0; i<M; ++i) {
for(unsigned j=0; j<M; ++j) {
if(i==j) continue; //skips over the diagonal elements
if( dists(i,j)>0 ) BZ(i,j) = -Weights(i,j)*Distances(i,j) / dists(i,j);
else BZ(i,j)=0.;
}
//the diagonal elements are -off diagonal elements BZ(i,i)-=BZ(i,j) (Equation 8.25)
BZ(i,i)=0; //create the space memory for the diagonal elements which are scalars
for(unsigned j=0; j<M; ++j) {
if(i==j) continue;
BZ(i,i)-=BZ(i,j);
}
}
mult( mypseudo, BZ, temp); mult(temp, InitialZ, newZ);
//Compute new sigma
double newsig = calculateSigma( Weights, Distances, newZ, dists ) / totalWeight;
//Computing whether the algorithm has converged (has the mass of the potato changed
//when we put it back in the oven!)
if( fabs( newsig - myfirstsig )<tol ) break;
myfirstsig=newsig;
InitialZ = newZ;
}
}
void Sbs2SourceReconstrucionLoreta::updateAlpha()
{
double tempInvAlpha = 0.0;
double tempEsMean = 0.0;
calculateSigma();
for (int j=0; j<vertices; ++j)
{
for (int i=0; i<vertices; ++i)
{
tempInvAlpha += (*kInv)[i][j]*(*sigmaS)[j][i];
for (int t=0; t<modelUpdateSamplesLength; ++t)
{
tempEsMean += (*currentModelUpdateValuesVertices)[i][t]*(*kInv)[i][j]*(*currentModelUpdateValuesVertices)[j][t];
}
}
}
// std::cout << "ALPHA:"<<std::endl;
// std::cout << "tempInvAlpha: "<<tempInvAlpha <<std::endl;
tempInvAlpha = modelUpdateSamplesLength * tempInvAlpha + tempEsMean;
invAlpha = tempInvAlpha / (double)(vertices*modelUpdateSamplesLength);
// std::cout << "tempInvAlpha: "<<tempInvAlpha <<std::endl;
// std::cout << "tempEsMean: "<<tempEsMean <<std::endl;
// std::cout << "invAlpha: "<<invAlpha <<std::endl;
// std::cout << "----" << std::endl;
// std::cout << invAlpha <<" ";
}
// suggest an action using Bolzman
int suggestBoltz (int state, float QTEMPERATURE ){
double sigma = calculateSigma(state, QTEMPERATURE);
float p = ((double)rand()/(double)RAND_MAX);
cout << "random nb: "<<p<<endl;
float sum = 0.0;
int i = -1;
while ((sum < p)&&(i< actionCount-1)){
i++;
double prob = exp(actionValue[state][i]/QTEMPERATURE)/ sigma;
cout<<"probability is "<<prob<<" for state"<<state<<" action"<<i<<endl;
sum = sum + prob;
}
// just hit p
return i;
}
void Sbs2SourceReconstrucionLoreta::setupModel()
{
toModelUpdateValuesIndex = 0;
modelUpdateSamplesSeen = 0;
paramAScaling = 1000000;
k = new DTU::DtuArray2D<double>(vertices,vertices);
kInv = new DTU::DtuArray2D<double>(vertices,vertices);
a = new DTU::DtuArray2D<double>(channels,vertices);
//toModelUpdateValues = new DTU::DtuArray2D<double>(channels,modelUpdateLength*samples);
//currentModelUpdateValues = new DTU::DtuArray2D<double>(channels,modelUpdateLength*samples);
//currentModelUpdateValuesVertices = new DTU::DtuArray2D<double>(vertices,modelUpdateLength*samples);
toModelUpdateValues = new DTU::DtuArray2D<double>(channels,modelUpdateSamplesLength);
currentModelUpdateValues = new DTU::DtuArray2D<double>(channels,modelUpdateSamplesLength);
currentModelUpdateValuesVertices = new DTU::DtuArray2D<double>(vertices,modelUpdateSamplesLength);
(*k) = 0;
k->toIdentityMatrix();
kInv->toIdentityMatrix();
(*kInv) = 0;
(*a) = 0;
(*toModelUpdateValues) = 0;
(*currentModelUpdateValues) = 0;
(*currentModelUpdateValuesVertices) = 0;
invAlpha = 0.0100;
invBeta = 0.3781;
at = new DTU::DtuArray2D<double>(vertices,channels);
akat = new DTU::DtuArray2D<double>(channels,channels);
kat = new DTU::DtuArray2D<double>(vertices,channels);
ak = new DTU::DtuArray2D<double>(channels,vertices);
akInv = new DTU::DtuArray2D<double>(channels,vertices);
akatInvAlpha = new DTU::DtuArray2D<double>(channels,channels);
sigmaS = new DTU::DtuArray2D<double>(vertices,vertices);
inputMatrix = new DTU::DtuArray2D<double>(channels,channels);
identity = new DTU::DtuArray2D<double>(channels,channels);
identityInvBeta = new DTU::DtuArray2D<double>(channels,channels);
tempW = new DTU::DtuArray2D<double>(vertices,channels);
invSigmaE = new DTU::DtuArray2D<double>(channels,channels);
invSigmaEA = new DTU::DtuArray2D<double>(channels,vertices);
atInvSigmaEA = new DTU::DtuArray2D<double>(vertices,vertices);
//invSigmaEAS = new DTU::DtuArray2D<double>(channels,modelUpdateLength*samples);
invSigmaEAS = new DTU::DtuArray2D<double>(channels,modelUpdateSamplesLength);
katInput = new DTU::DtuArray2D<double>(vertices,channels);
(*kat) = 0;
(*katInput) = 0;
readForwardModel();
readPriorSpatialCoherence();
readPriorSpatialCoherenceInverse();
a->multiply(k,ak);
invSigmaE->toIdentityMatrix();
identity->toIdentityMatrix();
a->transpose(at);
k->multiply(at,kat);
a->multiply(kat,akat);
a->multiply(kInv,akInv);
akat->multiply(invAlpha,akatInvAlpha);
identity->multiply(invBeta,identityInvBeta);
akatInvAlpha->add(identityInvBeta,inputMatrix);
inputMatrix->pinv(inputMatrix);
calculateSigma();
invSigmaE->multiply(a,invSigmaEA);
at->multiply(invSigmaEA,atInvSigmaEA);
kat->multiply(inputMatrix,katInput);
modelUpdateReady = 1;
modelUpdateDeltaCollected = 0;
katInput->multiply(invAlpha,w);
tempModelUpdatedReady = 1;
tempInput = new DTU::DtuArray2D<double>(1,samples);
tempOutput = new DTU::DtuArray2D<double>(1,samples);
}