/*
* iterateSpline()
*
* Perform one cycle of update of all the peaks.
*/
void iterateSpline(void)
{
int i;
fitData *new_peak, *old_peak;
for(i=0;i<n_fit_data;i++){
new_peak = &(new_fit_data[i]);
old_peak = &(old_fit_data[i]);
if (new_peak->status == RUNNING){
/* Add the peak & calculate the error. */
new_peak->error = calcError(new_peak, new_peak->params[CF_BACKGROUND]);
//printf("error: %f %f %f\n", new_peak->error, old_peak->error, new_peak->lambda);
if((fabs(new_peak->error - old_peak->error)/new_peak->error) < tolerance){
new_peak->status = CONVERGED;
}
else{
/*
* Check if the fit is getting worse. If it is then reset to the
* previous parameters & increase the lambda parameter. This will
* cause the search to move downhill with higher probability but
* less speed.
*/
if(new_peak->error > (1.5*old_peak->error)){
subtractPeak(new_peak);
copyFitData(old_peak, new_peak);
new_peak->lambda = 10.0 * new_peak->lambda;
addPeak(new_peak);
}
/*
* If the fit is getting better and lambda is greater than 1.0
* then we slowly decrease lambda.
*/
else if(new_peak->error < old_peak->error){
if(new_peak->lambda > 1.0){
new_peak->lambda = 0.8*new_peak->lambda;
if(new_peak->lambda < 1.0){
new_peak->lambda = 1.0;
}
}
}
/* Calculate updated fit values. */
updateFit(new_peak, new_peak->params[CF_BACKGROUND]);
/*
* Increase lambda in the event of Cholesky failure.
*/
if(new_peak->status == CHOLERROR){
new_peak->lambda = 10.0 * new_peak->lambda;
new_peak->status = RUNNING;
}
else{
subtractPeak(new_peak);
copyFitData(new_peak, old_peak);
updatePeakParameters(new_peak);
if(new_peak->type == F2D){
updateFitValues2D(new_peak);
}
else{
updateFitValues3D(new_peak);
}
if(new_peak->status != BADPEAK){
addPeak(new_peak);
}
}
}
}
}
}
/*
* updateZ()
*
* Update current fits given x, y width determined by z parameter.
*
* This procedure is also responsible for flagging peaks
* that might be bad & that should be removed from fitting.
*/
void updateZ()
{
// Lapack
int n = 5, nrhs = 1, lda = 5, ldb = 5, info;
// Local
int i,j,k,l,m,wx,wy;
double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,a3,a5;
double z0,z1,z2,zt,gx,gy;
double delta[NPEAKPAR];
double jt[5];
double jacobian[5];
double hessian[25];
fitData *cur;
for(i=0;i<NPEAKPAR;i++){
delta[i] = 0.0;
}
for(i=0;i<nfit;i++){
cur = &fit[i];
if(cur->status==RUNNING){
for(j=0;j<5;j++){
jacobian[j] = 0.0;
}
for(j=0;j<25;j++){
hessian[j] = 0.0;
}
l = cur->offset;
wx = cur->wx;
wy = cur->wy;
a1 = cur->params[HEIGHT];
a3 = cur->params[XWIDTH];
a5 = cur->params[YWIDTH];
// dwx/dz calcs
z0 = (cur->params[ZCENTER]-wx_z_params[1])/wx_z_params[2];
z1 = z0*z0;
z2 = z1*z0;
zt = 2.0*z0+3.0*wx_z_params[3]*z1+4.0*wx_z_params[4]*z2;
gx = -2.0*zt/(wx_z_params[0]*cur->wx_term);
// dwy/dz calcs
z0 = (cur->params[ZCENTER]-wy_z_params[1])/wy_z_params[2];
z1 = z0*z0;
z2 = z1*z0;
zt = 2.0*z0+3.0*wy_z_params[3]*z1+4.0*wy_z_params[4]*z2;
gy = -2.0*zt/(wy_z_params[0]*cur->wy_term);
for(j=-wy;j<=wy;j++){
yt = cur->yt[j+wy];
eyt = cur->eyt[j+wy];
for(k=-wx;k<=wx;k++){
m = j*image_size_x+k+l;
fi = f_data[m]+bg_data[m]/((double)bg_counts[m]);
xi = x_data[m];
xt = cur->xt[k+wx];
ext = cur->ext[k+wx];
e_t = ext*eyt;
// first derivatives
jt[0] = e_t;
jt[1] = 2.0*a1*a3*xt*e_t;
jt[2] = 2.0*a1*a5*yt*e_t;
jt[3] = -a1*xt*xt*gx*e_t-a1*yt*yt*gy*e_t;
jt[4] = 1.0;
// calculate jacobian
t1 = 2.0*(1.0 - xi/fi);
jacobian[0] += t1*jt[0];
jacobian[1] += t1*jt[1];
jacobian[2] += t1*jt[2];
jacobian[3] += t1*jt[3];
jacobian[4] += t1*jt[4];
// calculate hessian
t2 = 2.0*xi/(fi*fi);
// calculate hessian without second derivative terms.
hessian[0] += t2*jt[0]*jt[0];
hessian[1] += t2*jt[0]*jt[1];
hessian[2] += t2*jt[0]*jt[2];
hessian[3] += t2*jt[0]*jt[3];
hessian[4] += t2*jt[0]*jt[4];
// hessian[5]
hessian[6] += t2*jt[1]*jt[1];
hessian[7] += t2*jt[1]*jt[2];
hessian[8] += t2*jt[1]*jt[3];
hessian[9] += t2*jt[1]*jt[4];
// hessian[10]
// hessian[11]
hessian[12] += t2*jt[2]*jt[2];
hessian[13] += t2*jt[2]*jt[3];
hessian[14] += t2*jt[2]*jt[4];
// hessian[15]
// hessian[16]
// hessian[17]
hessian[18] += t2*jt[3]*jt[3];
hessian[19] += t2*jt[3]*jt[4];
// hessian[20]
// hessian[21]
// hessian[22]
// hessian[23]
hessian[24] += t2*jt[4]*jt[4];
}
}
// subtract the old peak out of the foreground and background arrays.
subtractPeak(cur);
// Use Lapack to solve AX=B to calculate update vector
dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info );
/*
for(j=0;j<5;j++){
for(k=0;k<5;k++){
printf("%.4f ", hessian[j*5+k]);
}
printf("\n");
}
*/
if(info!=0){
cur->status = ERROR;
if(TESTING){
printf("fitting error! %d %d %d\n", i, info, ERROR);
}
}
else{
// update params
delta[HEIGHT] = jacobian[0];
delta[XCENTER] = jacobian[1];
delta[YCENTER] = jacobian[2];
delta[ZCENTER] = jacobian[3];
delta[BACKGROUND] = jacobian[4];
fitDataUpdate(cur, delta);
// add the new peak to the foreground and background arrays.
if (cur->status != ERROR){
// calculate new x,y width, update fit area.
calcWidthsFromZ(cur);
cur->wx = calcWidth(cur->params[XWIDTH],cur->wx);
cur->wy = calcWidth(cur->params[YWIDTH],cur->wy);
addPeak(cur);
}
}
}
}
if(VERBOSE){
printf("\n");
}
}
/*
* update2D()
*
* Update current fits given equal width in x and y.
*
* This procedure is also responsible for flagging peaks
* that might be bad & that should be removed from fitting.
*/
void update2D()
{
// Lapack
int n = 5, nrhs = 1, lda = 5, ldb = 5, info;
// Local
int i,j,k,l,m,wx,wy;
double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,width;
double delta[NPEAKPAR];
double jt[5];
double jacobian[5];
double hessian[25];
fitData *cur;
for(i=0;i<NPEAKPAR;i++){
delta[i] = 0.0;
}
for(i=0;i<nfit;i++){
cur = &fit[i];
if(cur->status==RUNNING){
for(j=0;j<5;j++){
jacobian[j] = 0.0;
}
for(j=0;j<25;j++){
hessian[j] = 0.0;
}
l = cur->offset;
wx = cur->wx;
wy = cur->wy;
a1 = cur->params[HEIGHT];
width = cur->params[XWIDTH];
for(j=-wy;j<=wy;j++){
yt = cur->yt[j+wy];
eyt = cur->eyt[j+wy];
for(k=-wx;k<=wx;k++){
m = j*image_size_x+k+l;
fi = f_data[m]+bg_data[m]/((double)bg_counts[m]);
xi = x_data[m];
xt = cur->xt[k+wx];
ext = cur->ext[k+wx];
e_t = ext*eyt;
jt[0] = e_t;
jt[1] = 2.0*a1*width*xt*e_t;
jt[2] = 2.0*a1*width*yt*e_t;
jt[3] = -a1*xt*xt*e_t-a1*yt*yt*e_t;
jt[4] = 1.0;
// calculate jacobian
t1 = 2.0*(1.0 - xi/fi);
jacobian[0] += t1*jt[0];
jacobian[1] += t1*jt[1];
jacobian[2] += t1*jt[2];
jacobian[3] += t1*jt[3];
jacobian[4] += t1*jt[4];
// calculate hessian
t2 = 2.0*xi/(fi*fi);
// calculate hessian without second derivative terms.
hessian[0] += t2*jt[0]*jt[0];
hessian[1] += t2*jt[0]*jt[1];
hessian[2] += t2*jt[0]*jt[2];
hessian[3] += t2*jt[0]*jt[3];
hessian[4] += t2*jt[0]*jt[4];
// hessian[5]
hessian[6] += t2*jt[1]*jt[1];
hessian[7] += t2*jt[1]*jt[2];
hessian[8] += t2*jt[1]*jt[3];
hessian[9] += t2*jt[1]*jt[4];
// hessian[10]
// hessian[11]
hessian[12] += t2*jt[2]*jt[2];
hessian[13] += t2*jt[2]*jt[3];
hessian[14] += t2*jt[2]*jt[4];
// hessian[15]
// hessian[16]
// hessian[17]
hessian[18] += t2*jt[3]*jt[3];
hessian[19] += t2*jt[3]*jt[4];
// hessian[20]
// hessian[21]
// hessian[22]
// hessian[23]
hessian[24] += t2*jt[4]*jt[4];
}
}
// subtract the old peak out of the foreground and background arrays.
subtractPeak(cur);
// Use Lapack to solve AX=B to calculate update vector
dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info );
if(info!=0){
cur->status = ERROR;
if(TESTING){
printf("fitting error! %d %d %d\n", i, info, ERROR);
printf(" %f %f %f %f %f\n", delta[HEIGHT], delta[XCENTER], delta[YCENTER], delta[XWIDTH], delta[BACKGROUND]);
}
}
else{
// update params
delta[HEIGHT] = jacobian[0];
delta[XCENTER] = jacobian[1];
delta[YCENTER] = jacobian[2];
delta[XWIDTH] = jacobian[3];
delta[YWIDTH] = jacobian[3];
delta[BACKGROUND] = jacobian[4];
fitDataUpdate(cur, delta);
// add the new peak to the foreground and background arrays.
// recalculate peak fit area as the peak width may have changed.
if (cur->status != ERROR){
cur->wx = calcWidth(cur->params[XWIDTH],cur->wx);
cur->wy = cur->wx;
addPeak(cur);
}
}
}
}
}
/*
* update3D()
*
* Update current fits allowing all parameters to change.
*
* This procedure is also responsible for flagging peaks
* that might be bad & that should be removed from fitting.
*/
void update3D()
{
// Lapack
int n = 6, nrhs = 1, lda = 6, ldb = 6, info;
// Local
int i,j,k,l,m,wx,wy;
double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,a3,a5;
double delta[NPEAKPAR];
double jt[6];
double jacobian[6];
double hessian[36];
fitData *cur;
for(i=0;i<NPEAKPAR;i++){
delta[i] = 0.0;
}
for(i=0;i<nfit;i++){
cur = &fit[i];
if(cur->status==RUNNING){
for(j=0;j<6;j++){
jacobian[j] = 0.0;
}
for(j=0;j<36;j++){
hessian[j] = 0.0;
}
l = cur->offset;
wx = cur->wx;
wy = cur->wy;
a1 = cur->params[HEIGHT];
a3 = cur->params[XWIDTH];
a5 = cur->params[YWIDTH];
for(j=-wy;j<=wy;j++){
yt = cur->yt[j+wy];
eyt = cur->eyt[j+wy];
for(k=-wx;k<=wx;k++){
m = j*image_size_x+k+l;
fi = f_data[m]+bg_data[m]/((double)bg_counts[m]);
xi = x_data[m];
xt = cur->xt[k+wx];
ext = cur->ext[k+wx];
e_t = ext*eyt;
jt[0] = e_t;
jt[1] = 2.0*a1*a3*xt*e_t;
jt[2] = -a1*xt*xt*e_t;
jt[3] = 2.0*a1*a5*yt*e_t;
jt[4] = -a1*yt*yt*e_t;
jt[5] = 1.0;
// calculate jacobian
t1 = 2.0*(1.0 - xi/fi);
jacobian[0] += t1*jt[0];
jacobian[1] += t1*jt[1];
jacobian[2] += t1*jt[2];
jacobian[3] += t1*jt[3];
jacobian[4] += t1*jt[4];
jacobian[5] += t1*jt[5];
// calculate hessian
t2 = 2.0*xi/(fi*fi);
if (0){
// FIXME: not complete
// hessian with second derivative terms.
hessian[0] += t2*jt[0]*jt[0];
hessian[1] += t2*jt[0]*jt[1]+t1*2.0*xt*a3*ext*eyt;
hessian[2] += t2*jt[0]*jt[2];
hessian[3] += t2*jt[0]*jt[3]+t1*2.0*yt*a5*ext*eyt;
hessian[4] += t2*jt[0]*jt[4];
hessian[5] += t2*jt[0]*jt[5];
// hessian[6]
hessian[7] += t2*jt[1]*jt[1]+t1*(-2.0*a1*a3*ext*eyt+4.0*a1*a3*a3*xt*xt*ext*eyt);
hessian[8] += t2*jt[1]*jt[2]+t1*4.0*a1*xt*yt*a3*a3*ext*eyt;
hessian[9] += t2*jt[1]*jt[3];
hessian[10] += t2*jt[1]*jt[4];
hessian[11] += t2*jt[1]*jt[5];
// hessian[12]
// hessian[13]
hessian[14] += t2*jt[2]*jt[2]+t1*(-2.0*a1*a3*ext*eyt+4.0*a1*a3*a3*yt*yt*ext*eyt);
hessian[15] += t2*jt[2]*jt[3];
hessian[16] += t2*jt[2]*jt[4];
hessian[17] += t2*jt[2]*jt[5];
// hessian[18]
// hessian[19]
// hessian[20]
hessian[21] += t2*jt[3]*jt[3];
hessian[22] += t2*jt[3]*jt[4];
hessian[23] += t2*jt[3]*jt[5];
// hessian[24]
// hessian[25]
// hessian[26]
// hessian[27]
hessian[28] += t2*jt[4]*jt[4];
hessian[29] += t2*jt[4]*jt[5];
// hessian[30]
// hessian[31]
// hessian[32]
// hessian[33]
// hessian[34]
hessian[35] += t2*jt[5]*jt[5];
}
else {
// hessian without second derivative terms.
hessian[0] += t2*jt[0]*jt[0];
hessian[1] += t2*jt[0]*jt[1];
hessian[2] += t2*jt[0]*jt[2];
hessian[3] += t2*jt[0]*jt[3];
hessian[4] += t2*jt[0]*jt[4];
hessian[5] += t2*jt[0]*jt[5];
// hessian[6]
hessian[7] += t2*jt[1]*jt[1];
hessian[8] += t2*jt[1]*jt[2];
hessian[9] += t2*jt[1]*jt[3];
hessian[10] += t2*jt[1]*jt[4];
hessian[11] += t2*jt[1]*jt[5];
// hessian[12]
// hessian[13]
hessian[14] += t2*jt[2]*jt[2];
hessian[15] += t2*jt[2]*jt[3];
hessian[16] += t2*jt[2]*jt[4];
hessian[17] += t2*jt[2]*jt[5];
// hessian[18]
// hessian[19]
// hessian[20]
hessian[21] += t2*jt[3]*jt[3];
hessian[22] += t2*jt[3]*jt[4];
hessian[23] += t2*jt[3]*jt[5];
// hessian[24]
// hessian[25]
// hessian[26]
// hessian[27]
hessian[28] += t2*jt[4]*jt[4];
hessian[29] += t2*jt[4]*jt[5];
// hessian[30]
// hessian[31]
// hessian[32]
// hessian[33]
// hessian[34]
hessian[35] += t2*jt[5]*jt[5];
// Ignore off-diagonal terms.
// This approach converges incredibly slowly.
/*
hessian[0] += t2*jt[0]*jt[0];
hessian[7] += t2*jt[1]*jt[1];
hessian[14] += t2*jt[2]*jt[2];
hessian[21] += t2*jt[3]*jt[3];
hessian[28] += t2*jt[4]*jt[4];
hessian[35] += t2*jt[5]*jt[5];
*/
}
}
}
/*
printf("hessian:\n");
for(j=0;j<6;j++){
for(k=0;k<6;k++){
printf("%.4f ", hessian[j*6+k]);
}
printf("\n");
}
printf("\n");
*/
// subtract the old peak out of the foreground and background arrays.
subtractPeak(cur);
// Use Lapack to solve AX=B to calculate update vector
dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info );
if(info!=0){
cur->status = ERROR;
if(TESTING){
printf("fitting error! %d %d %d\n", i, info, ERROR);
}
}
else{
// update params
delta[HEIGHT] = jacobian[0];
delta[XCENTER] = jacobian[1];
delta[XWIDTH] = jacobian[2];
delta[YCENTER] = jacobian[3];
delta[YWIDTH] = jacobian[4];
delta[BACKGROUND] = jacobian[5];
fitDataUpdate(cur, delta);
// add the new peak to the foreground and background arrays.
if (cur->status != ERROR){
cur->wx = calcWidth(cur->params[XWIDTH],cur->wx);
cur->wy = calcWidth(cur->params[YWIDTH],cur->wy);
addPeak(cur);
}
}
}
}
}
