levenbergmarquardt.cpp

it is a good programe to use in nonlinear optical aspect

// 验证使用Levenberg-Marquardt方法求解非线性方程组 

// OUT:	-0.315639,  	2.95443,	2.13672
//	         	0,  +(-)1.54919,	      0
//----
//----
//			    1,            5,         -4

#include <stdlib.h>					// exit函数
#include <stdio.h>
#include <math.h>


const int DIM_VARS=3;				// 方程组(未知变量)个数
const int DIM_EQUS=4;

const double EPSILON=0.000001;
const unsigned MAXITER = 10000;


void ffOfExample(double x[DIM_VARS],double F[DIM_EQUS]);
void fdOfExample(double x[DIM_VARS],double Fjac[DIM_EQUS][DIM_VARS]);
double chiSquared(double x[DIM_VARS],int equnums);
void Levenberg_Marquardt(double x[DIM_VARS], double lambda, double term_epsilon,int equnums,int varnums);
int gcpelim2( int process, double A[DIM_VARS][DIM_VARS], double xx[DIM_VARS] );
bool CheckSolvs(double x[DIM_VARS]);


int main(int argc, char *argv[])
{
	double x[DIM_VARS]={100,   500,  -100};		// 初始值

	double lambda=1;
	double term_epsilon=1e-12;


	int i = 0, count = 0;

	// 此处必须添加遍历条件
	do {
		printf("\n迭代次数:(%d)\t中间最小值:\t", count++);
		if (count>=MAXITER) exit(1);
		for(i=0; i<DIM_VARS; i++) {
			printf("%12.9f\t",x[i]);
			x[i]+=2.5;				// 类似遗传算法交叉、变异
		}
		Levenberg_Marquardt(x,lambda,term_epsilon,DIM_EQUS,DIM_VARS);
	} while(CheckSolvs(x)==false);	//非线性解发生陷入局部错误情况,继续搜索


	printf("\n全局优化结果\t");
	for(i=0; i<DIM_VARS; i++) printf("%12.9f\t",x[i]);
	printf("\n\n");

	return 0;
}

/*用到求解非线性方程组Levenberg_Marquardt*/
void Levenberg_Marquardt(double x[DIM_VARS], double lambda, double term_epsilon,int equnums,int varnums)
{
/**
* Minimize E = sum {(y[k] - f(x[k],x)) / s[k]}^2
* @param y corresponding array of values
* @param x the parameters/state of the model
* @param vary false to indicate the corresponding x[k] is to be held fixed
* @param s2 sigma^2 for point i
* @param lambda blend between steepest descent (lambda high) and
*	jump to bottom of quadratic (lambda zero).
* 	Start with 0.001.
* @param term_epsilon termination accuracy (0.01)
* @param maxiter	stop and return after this many iterations if not done
	*/
	
	double e1;
    double e0 = chiSquared(x,equnums);		// 定精度,y1^2+...yn^2
	
    bool done = false;
	
    // g = gradient, H = hessian, d = step to minimum
    // H d = -g, solve for d
	double Fjac[DIM_EQUS][DIM_VARS];
    double H[DIM_VARS][DIM_VARS];
    double g[DIM_VARS];
    double F[DIM_EQUS];
	double na[DIM_VARS];
	
	int i,row,col;

    int iter = 0;
    int term = 0;	// termination count test
	
	
	
    do {
		ffOfExample(x,F);		// 求解当前x向量对应的函数
		fdOfExample(x,Fjac);	// -------------------Jacobi矩阵
		
		++iter;
		
		// hessian approximation
		for(row = 0; row < varnums; row++ ) 
		{
			for(col = 0; col < varnums; col++ )
			{
				H[row][col] = 0.;
				for( int i = 0; i < equnums; i++ ) 
				{
					// 此处计算 H=J'J 正确,本来是A[row][k]*B[k][col]
					// 前面的Jacobi矩阵转置了,因此正确
					H[row][col] += Fjac[i][row]*Fjac[i][col];
				}  //equnums
			} //col
		} //row
		
		// boost diagonal towards gradient descent
		for( row = 0; row < varnums; row++ )
			H[row][row] *= (1. + lambda);
		
		// gradient
		for(row = 0; row < varnums; row++ ) 
		{
			g[row] = 0.;
			for( i = 0; i < equnums; i++ )		// 此处修改为方程个数
			{
				g[row] += Fjac[i][row]*F[i];
			}
		} //equnums
		
		
		// solve H d = -g, evaluate error at new location
		// 求解...注意解出的d存放在g的位置,
		// d也就是误差error.
		if(gcpelim2(0,H,g)==1)
		{
			//AfxMessageBox("The x0 isn't good so the matrix Fjac is singular!");
			return ;
		}
		for(i=0;i<varnums;i++)
		{
			na[i]=x[i]-g[i];
		}
		e1 = chiSquared(na,equnums);		// 计算前后两次运算的残差
		
		
		// termination test (slightly different than NR)
		if (fabs(e1-e0) > term_epsilon)
		{
			term = 0;      
		}
		else
		{
			term++;
			if (term == 4)		// 连续4次满足极小情况
				done = true;
		}
		if (iter >= MAXITER)
			done = true;
		
		// in the C++ version, found that changing this to e1 >= e0
		// was not x good idea.  See comment there.
		//
		if (e1 > e0)
		{	
			// new location worse than before
			//调节到新的引导位置,调节步长的方法与算法略有差异,单结果相同
			lambda *= 10.;
		}
		else 
		{
			// new location better, accept new parameters
			lambda *= 0.1;
			e0 = e1;
			// simply assigning x = na will not get results copied back to caller
			for( int i = 0; i < varnums; i++ ) x[i] = na[i];
		}
		
    } while(!done);
	
}

/**
   * calculate the current sum-squared-error
   * (Chi-squared is the distribution of squared Gaussian errors,
   * thus the name)
   */
/**
   * 计算当前步骤的平方和误差
   * (高斯误差)
   */
double chiSquared(double x[DIM_VARS],int equnums)
{
    double sum = 0.;
	double F[DIM_EQUS];

	ffOfExample(x,F);		// 求解当前x向量对应的函数

    for( int i = 0; i < equnums; i++ ) sum += F[i]*F[i];

    return sum;
} //chiSquared



// 求解结果放在xx[]向量中
int gcpelim2( int process, double A[DIM_VARS][DIM_VARS], double xx[DIM_VARS] )
{
	int k,i,j,i0;
	double pelement;
	if( process == 1 ) printf("The process of elimination\n");
	/* elimination step */
	for(k=0;k<DIM_VARS;k++)
	{
		/* for principal element*/
		// 查找主元
		pelement=fabs(A[k][k]);       i0=k;
		for(i=k;i<DIM_VARS;i++)
			if( fabs(A[i][k]) > pelement ) { pelement=fabs(A[i][k]); i0=i; }
			if( i0 != k )
			{
				for(j=0;j<DIM_VARS;j++)
				{ pelement=A[k][j]; A[k][j]=A[i0][j]; A[i0][j]=pelement; }
				pelement=xx[k]; xx[k]=xx[i0]; xx[i0]=pelement;
			}
			if( fabs(A[k][k]) < EPSILON ) return(1);
			for(i=k+1;i<DIM_VARS;i++)
			{
				A[i][k]=A[i][k]/A[k][k];
				for(j=k+1;j<DIM_VARS;j++) A[i][j]=A[i][j]-A[i][k]*A[k][j];
				xx[i]=xx[i]-A[i][k]*xx[k];
			}
			if(process == 1 )
			{
				for(i=0;i<DIM_VARS;i++)
				{
					for(j=0;j<DIM_VARS;j++) printf("%10.6f",A[i][j]);
					printf("   | %10.6f\n",xx[i]);
				}
				printf("\n");
			}
	}
	/* backward step */
	// 回溯求解
	for(i=DIM_VARS-1;i>=0;i--)
	{
		for(j=i+1;j<DIM_VARS;j++) xx[i]=xx[i]-A[i][j]*xx[j];
		xx[i]=xx[i]/A[i][i];
	}
	return(0);
}

// 此处存放非线性方程组,x为变量,F为函数值
void ffOfExample(double x[3],double F[4])
{
	F[0]=x[0]-5.0*x[1]*x[1]+7.0*x[2]*x[2]+12.0;
	F[1]=3.0*x[0]*x[1]+x[0]*x[2]-11.0*x[0];
	F[2]=2.0*x[1]*x[2]+40.0*x[0];

	F[3]=9.0*x[0]*x[0]-x[1]+x[2];
}


// 残余运算的Jacobi矩阵,手工计算.
void fdOfExample(double x[3],double Fjac[4][3])
{
	// 此处注意和绝大多数Fortran类比,
	// 此处是行先,但存放一致

	// Fjac[i][j]=D(F[i])/D(X[j])


	Fjac[0][0]= 1.0;					Fjac[0][1]=-10.0*x[1];		Fjac[0][2]= 14.0*x[2];
	Fjac[1][0]= 3.0*x[1]+x[2]-11.0;		Fjac[1][1]= 3.0*x[0];		Fjac[1][2]= x[0];
	Fjac[2][0]= 40.0;					Fjac[2][1]= 2.0*x[2];		Fjac[2][2]= 2.0*x[1];

	Fjac[3][0] = 18.0*x[0];				Fjac[3][1] = -1.0;			Fjac[3][2] = 1.0;

}


//------------------------------------------
bool CheckSolvs(double x[DIM_VARS])
{
	bool bval;
	
	register int i;
	double F[DIM_EQUS];


	ffOfExample(x,F);		// 求方程值

	
	bval=true;

	for(i=0; i<DIM_EQUS; i++) {
		if(fabs(F[i]) > 1e-3) {
			bval=false;
			break;
		}
	}

	return bval;
}