?? levenbergmarquardt.cpp
字號:
// 驗證使用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;
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -