L—M算法 C++源代码

// Term.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include <cmath>
#include <iostream>
using namespace std;

double GetF(double *p,int N)
{
 if(N!=5)
 {
 return 0.0;
 }
 double A = p[0];
 double B = p[1];
 double C = p[2];
 double D = p[3];
 double X = p[4];
 return A-(A-B)/(pow((C/X),D) + 1);
}

int GetA(double *p,int N,double *out)
{
 if(N!=5)
 {
  *out = 0;
  return 1;
 }

 double A = p[0];
 double B = p[1];
 double C = p[2];
 double D = p[3];
 double X = p[4];

 out[0] = pow(C/X,D)/(pow(C/X,D)+1);
 out[1] = 1/pow(C/X,D);
 out[2] = D*(A-B)*pow(C/X,D)/(C*pow(pow(C/X,D)+1,2));
 out[3] = log(C/X)*(A-B)*pow(C/X,D)/pow(pow(C/X,D)+1,2);
 out[4] = 0;

 return 0;
}

//解法方程A*X = B
int Solve_Equation(double *A,double *B,int size,int row,double *X)
{
 int i,j;int T = 0;
 double Large = -1e10;
 int s_index = 0;
 double Temp = 0.0;
 for (i = 0;i<row;i++)
 {
  Large = -1e10;
  for (j = i;j<size;j++)
  {
   if(A[j*row+i]>Large)
   {
    Large = A[j*row+i];
    s_index = j;
   }
  }  
  //如果最大列主元不在子块的第一行，那么进行交换
  if(i!=s_index)
  {
   for (int m = 0;m<row;m++)
   {
    Temp = A[i*size+m];
    A[i*row+m] = A[s_index*size+m];
    A[s_index*size+m] = Temp;
   }
   Temp = B[i];
   B[i] = B[s_index];
   B[s_index] = Temp;
  }
  //进行消元
  double M = 1.0;
  for (j = i+1;j<size;j++)
  {
   M = A[j*row+i]/A[i*row+i];
   for (int k = i;k<size;k++)
   {
    A[j*size+k] = A[j*row+k]-M*A[i*row+k];
   }
   B[j] = B[j]-M*B[i];
  }
 }
 //计算方程组的系数的
 for (i = 0;i<size;i++)
 {
  int Flag = 1;
  for (j = i;j<row && Flag;j++)
  {
   if(B[i]!=0 && A[i*row+j] ==0)
   {
    T = -1;
    break;
   }
   if(A[i*row+j]!=0)
   {
    T++;
    Flag = 0;
   }
  }
 }
 //求取方程组的解
 if(T == row)
 {
  X[size-1] = B[size-1]/A[(size-1)*row+(size-1)];
  for (int m = size-2;m>=0;m--)
  {
   Temp = B[m]-X[m+1]*A[m*row+m+1];
   for (int n = m+2;n<size;n++)
   {
    Temp = Temp-X[n]*A[m*row+n];
   }
   X[m] = Temp/A[m*row+m];
  }
  return 0;
 }
 else
  return 1;
}

//计算Q+U的值     
void ADD_M(double *D_Q,double U_K,double *D_d,int L)
{
 for (int i =0;i<L;i++)
 {
  for (int j = 0;j<L;j++)
  {
   if(i == j)   *(D_d+i*L+j) = *(D_Q+i*L+j)+U_K;
   else         *(D_d+i*L+j) = *(D_Q+i*L+j);
  }
 }
}

//G矩阵取反
//I_G矩阵为G矩阵取反以后的矩阵
void Invert_G(double G[],double I_G[],int l)
{
 for (int i = 0;i<l;i++)
  I_G[i] = -G[i];
}

//H—收敛准则
bool X_Compare(double *FK_1,double *FK,double E,double k)
{
 double s = 0;
 for (int i = 0;i<k;i++)
 {
  s+=pow(FK_1[i]-FK[i],2);
 }

 if(sqrt(s)<E)   return true;
 else            return false;
}

//保存上一次的X值
void X_Save(double *Des,double *Source,double n)
{
 for (int i = 0;i<n;i++)
 {
  Des[i] = Source[i];
 }
}

typedef double (*Tpf)(double *,int);
typedef int (*TpA)(double *,int ,double *);

void ColMutRow(double *l,int M,double *r,int N,double *out)
{

 for(int i =0 ; i < M  ; ++i)
 {
  for( int j = 0 ; j< N; ++j)
  {
   *(out + i*N + j) = l[i] * r[j];
  }
 }
}

void RowMutCol(double *l,double *r,int N,double *out)
{
 *out = 0;
 for(int i =0 ; i < N  ; ++i)
 {
  for( int j = 0 ; j< N; ++j)
  {
   *(out) += l[i] * r[j];
  }
 }
}

void VecMutNum(double *l,double r,int N,double *out)
{
 for(int i =0 ; i < N  ; ++i)
 {
  *(out + i ) = l[i] * r;
 }
}

void VecAddVec(double *l,double *r,int N,double *out)
{
 for(int i =0 ; i < N  ; ++i)
 {
  *(out + i ) = l[i] + r[i];
 }
}

void VecAssignment(double *dsc,double *src,int n)
{
 memcpy(dsc,src,n*sizeof(double)); //x = x0 ;
}

//getf-目标函数
//getA-目标函数的雅克布矩阵
//beta0,u0,v0 迭代参数
//x0 初始值
//N x0的维数
//e 迭代精度
//KMAX 最大迭代次数
int LMA(Tpf getf,TpA getA,double beta0,double u0,double v0,double *x0,int N,double e ,int KMax , double * out,double * Sout )
{
 double *x = new double[N];
 double f  = 0.0 ;
 double S  = 0.0;
 double *A = new double[N];
 double *Q = new double[N*N];
 double *Q_I = new double[N*N];
 double *G = new double[N];
 double *P = new double[N];
 double *I_G = new double[N];
 double fk_1 = 0.0;
 double Sk_1 = 0.0;
 int KMAX = KMax;
 int k;

 //第一步:初始参数
 double beta = beta0;
 double u = u0;
 double v = v0;

 //第二步:给定初始点X0，置K=0
 VecAssignment(x,x0,N); // X = X0
 k = 0;

 //零时变量
 double gp,s_bgp;
 bool Finished  = false;
 double *xk_1 = new double[N];

 //迭代开始
 do
 {

  //第三步:计算F，S
  f = getf(x,N); //fk=f(xk)
  S= f * f;
  //ColMutRow(f,1,f,1,S); //Sk=f*f

  //第四步:计算A，Q
  getA(x,N,A);
  ColMutRow(A,N,A,N,Q);

  //第五步:计算G
  ColMutRow(A,N,&f,1,G);

  do
  {
   //第六步:解法方程
   ADD_M(Q,u,Q_I,N);
   Invert_G(G,I_G,N);
   Solve_Equation(Q_I,I_G,N,N,P);

   //第七步:计算XK_1
   VecAddVec(x,P,N,xk_1);  //xk+1 = x + P;
   fk_1 = getf(xk_1,N);  //fk=f(xk)
   Sk_1= fk_1 *fk_1;

   //第八步:H—收敛准则
   Finished = X_Compare(&fk_1,&f,e,1);
   if(Finished)
   {
    VecAssignment(out,xk_1,N); //x = xk+1;
    *Sout = S;
    break;
   }

   //第九步:检验S
   RowMutCol(G,P,N,&gp);
   s_bgp = S + beta * gp ;
   if(Sk_1 < s_bgp)
   {
    u = u/v;
    break;
   }
   else
   {
    u = v*u;
   }

  }while(true);
  //第十步：置K=K+1
  k = k + 1; 
  VecAssignment(x,xk_1,N); //xk = xk+1;
 }while( k<KMAX && !Finished );

 VecAssignment(out,xk_1,N); //x = xk+1;
 *Sout = S;

 delete []G;
 delete []I_G;
 delete []P;
 delete []Q;
 delete []Q_I;
 delete []A;
 delete []x;
 return 1;
}

int _tmain(int argc, _TCHAR* argv[])
{
 double u0,v0,e,Beta;
 int N,Max;
/*
 cout<<"请输入初始u0的值：";
 cin>>u0;
 cout<<endl;
 cout<<"请输入初始v0的值：";
 cin>>v0;
 cout<<endl;
 //cout<<"请输入初始N的值：";
 //cin>>N;
 //cout<<endl;
 cout<<"请输入初始e的值：";
 cin>>e;
 cout<<endl;
 cout<<"请输入最大循环次数：";
 cin>>Max;
 cout<<endl;
 cout<<"请输入初始Beta的值：";
 cin>>Beta;
 cout<<endl;
 for (int i = 0;i<N;i++)
 {
  cout<<"请输入X【"<<i<<"】的初始值：";
  cin>>I_X[i];
  cout<<endl;
 }
*/
 N = 5;

 double *I_X = new double[N];
 double *Xout =  new double [N];

 u0 = 0.3;
 Beta = 0.5;
 v0 = 2;
 I_X[0] = 2.3;
 I_X[1] = 4.4;
 I_X[2] = 3.7;
 I_X[3] = 1.6;
 I_X[4] = 5.1;

 e= 0.0000001;
 Max = 100;
 double sout;

 cout <<GetF(I_X,N) << endl;
 LMA(GetF,GetA,Beta,u0,v0,I_X,N,e ,Max, Xout,&sout);
 for (int i = 0;i<N;i++)
 {
    cout << Xout[i] << "      ";
 }
 cout<<sout<<endl;
 cout <<GetF(Xout,N) << endl;

 delete []Xout;
 delete []I_X;

 system("pause");
 return 0;
}