Matlab 和 C++ （3）
上次在“用matlab拟合模型参数和计算参数误差”中给出了用matlab的一般非线性拟合的最小二乘法。具体如下：用matlab拟合模型参数和计算参数误差，原帖由 recbio 于 2007-6-17 21:46 发表 http://www.dolc.de/forum/images/common/back.gif
...
现在，我们给一个c++的一般解决法。程序有两个文件，一个叫“myFIT_FUN.hpp”，里面是LM算法的核心程序（因为matlab有自己的call gate函数，用户c++文件文件最好定义为hpp，这样，可以直接被引用）；另外一个是“myGAUSS_FIT.cpp”，用来定义用户自己的拟合函数。
在这里，我们给了一个高斯分布拟合的例子。
用户自己需要定义两个函数，一个是UserDef_funcs，这个是写在hpp文件里面的方法自己会调用的用户拟合函数；一个是 UserDef_Rage，是一个用户自己定义的边界条件。
关于UserDef_funcs，这里是一个“void myNormal(double x, double *para, double *y, double *dyda, int na)”函数。具体的参数意义如下：
x 输入的自变量
para 参数矩阵首地址
y 函数的输出值 （对于正态分布，这里的值是Y = exp(- (X - A1)*(X - A1)/2/A2/A2 ) / A2 / SQRT_2_PI + A3;）A1-3 的物理意义：A1是数学期望值 A2是标准偏差 A3是水平偏移。
dyda 是每一个参数对应的一介偏导数
na 是拟合参数个数
关于UserDef_Rage，这里是一个“void myNormal_Rage(double *para, int ma, int *ia, int mfit)”函数。具体的参数意义如下：
para 参数矩阵首地址
ma 是拟合参数个数
ia“参数相关矩阵”首地址，如果在参数相关矩阵里面是“1”表示该参数需要拟合，“0”表示不用拟合。
mfit 实际需要拟合的参数的个数。
关于拟合的具体理论问题参见“用matlab拟合模型参数和计算参数误差”，现在我们就简单说说如何应用：
首先，我们可以建立这样的数据：
>> X=;E=50; D=7; O=0.02;
>> Y=1/D/sqrt(2*pi).*exp(-((X-E)/D).^2/2)+O;
这里我们定义了一个数学期望值=50；标准偏差=7；水平偏移=0.02的正态分布函数。
形状如何可以用如下的命令看：plot(X,Y,'r-');
下面，我们给它加上误差：（这个误差我们会在后面看到，通过参数的拟合将同样被拟合出）
>> D_E=((poissrnd(100, 1, 100)-100)./10*0.01 + 1).*E;
>> D_D=((poissrnd(100, 1, 100)-100)./10*0.05 + 1).*D;
>> D_O=((poissrnd(100, 1, 100)-100)./10*0.02 + 1).*O;
这里的误差是随机分布函数poissrnd给出的泊松分布，基本上也是正态的一个例子。
重新计算新的分布点：
>> Y_P=1./D_D./sqrt(2*pi).*exp(-((X-D_E)./D_D).^2/2)+D_O;
和经过误差修饰的参数：
>> A=
>> Y_M = 1/A(2)/sqrt(2*pi)*exp(-((X-A(1))/A(2)).^2/2)+A(3);
看看：
>> plot(X,Y_M,'r-', X, Y_P,'*');
如下图：红线是函数，兰点是我们要输入的拟合点。
下面我们计算模拟参数的误差和Chi Square（方差和）
>> E_P=sqrt(./100)
>> Chi_P = sum((Y_P - Y).^2)
计算结果：
*A= 49.9110 6.9671 0.0199
*E_P = 0.5140 0.3448 0.0004
*Chi_P = 1.4836e-004
到这里，我们建立了一个100个点的拟合模型，模拟数据：X, Y_P
下面是拟合的过程，我们看看会不会得到相同的结果？
首先建立两个拟合用工作矩阵，大小和拟合参数矩阵一样：
>> F = ; E = ;
F里面全是1，表示我们要拟合所有的参数。E将是反回的拟合参数的误差。
下面估算拟合初始值（其实，如果不估算，随便用一些数，也会得到基本相同的值，稍后你会看到）
>> A(3) = Y_P(1);（我们就用最两端的值估计 偏移）
>> A(1) = sum((Y_P - A(3)).* X)/sum(Y_P - A(3));（这是根据定义，求分布函数的数学期望）
>> A(2) = sqrt(sum((Y_P - A(3)).* (X - A(1)).^2))/sum(Y_P - A(3));（这是根据定义求 标准偏差）
看看我们的拟合初始值估计：
A= 49.8889 8.2355 0.0196
>> Y_A = 1/A(2)/sqrt(2*pi)*exp(-((X-A(1))/A(2)).^2/2)+A(3);
>> Chi_A = sum((Y_P - Y_A).^2)
Chi_A =9.0091e-004
和 Chi_P = 1.4836e-004相比，大了一点。好，我们开始拟合：
>> = myGAUSS_FIT (X, Y_P, A, F, E)
Chi = 1.4411e-004
n = 33
拟合函数被调用了33次，拟合后的Chi = 1.4411e-004，比模型的建立值还要小！Chi_P = 1.4836e-004。说明，我们的拟合非常成功，找到了一组参数，比建立模型的参数更加符合图中的蓝色点部分。
我们来看看参数是什么？
A =50.0345 6.9962 0.0198
E =0.0601 0.0490 0.0001
这里，A是返回的参数了，E是误差。我们发现A和前面的*的结果很象，但是为什么E小了许多？
关于这个问题，因为我们的E是参数的数学期望值的无偏估计（计算的时候除了一个自由度），和实际的单次的测量有一个自由度的差别。
如果你不用除自由度
>> E*sqrt(97)
ans =0.5915 0.4830 0.0012（*E_P = 0.5140 0.3448 0.0004
）
怎么样，差不多吧。这个就是拟合参数的误差的意义！
前面我们说了，因为是LM法，有很强的收敛于最佳值的能力。
现在我们看看，随便一个A会怎么样：
>> A=;
>> = myGAUSS_FIT (X, Y_P, A, F, E)
Chi = 1.4411e-004
n = 58
只不过拟合步数多了点，但是一样得到了正确结果！
下面是程序文件：
//---------------------------------------------------------------------------
// myGAUSS_FIT.cpp
//---------------------------------------------------------------------------
#include "mex.h"
#define MY_LCC
#include "myFIT_FUN.hpp"
#define SQRT_2_PI 2.5066282746310005024157652848108
//---------------------------------------------------------------------------
// User Define Function
// Y = exp(- (X - A1)*(X - A1)/2/A2/A2 ) / A2 / SQRT_2_PI + A3;
// dy/dA3 = 1; dy/dA1 = Y / A2 * (X - A1) / A2;
// dy/dA2 = Y / A2 * (( (X - A1) / A2 ) * ( (X - A1) / A2 ) - 1);
//---------------------------------------------------------------------------
void myNormal(double x, double *para, double *y, double *dyda, int na){
double dtmpA, dtmpB, dtmpC, dtmpD, dA2;
dtmpA= (x - para) / ( dA2 = para);
dtmpB=dtmpA * dtmpA;
dtmpD = ( dtmpC=((double) 1 /SQRT_2_PI / dA2)*( dtmpC = exp( - dtmpB/2 ) ) ) / dA2;
*y = dtmpC + para;
dyda = 1;
dyda = dtmpD * dtmpA;
dyda = dtmpD * (dtmpB - 1);
}
//---------------------------------------------------------------------------
void myNormal_Rage(double *para, int ma, int *ia, int mfit){
if(para<=0)para=1e-17;
if(para<=0)para=1e-17;
}
//---------------------------------------------------------------------------
#ifndef MY_LCC
//---------------------------------------------------------------------------
//#pragma argsused
//int WINAPI DllEntryPoint(HINSTANCE hinst, unsigned long reason, void* lpReserved)
//{
// return 1;
//}
//---------------------------------------------------------------------------
void _exportmexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
#else
voidmexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
#endif
{
double *xData, *yData, *sigData;
double pFit, *pFitA, *pFitA_fit, *pFitErr=NULL;
int nCountD , nCountA, nFit_fit, i=0, j;
bool bError=false;
double *ppCovar, *pCovar, *ppAlpha, *pAlpha;
double dChisq, dAlamda;
if (5!=nrhs && 4!=nrhs ) mexErrMsgTxt("Using: chi = myGAUSS_FIT(X, Y, A, F, E); E is error output and can be skip if one do not need");
UserDef_funcs = myNormal; UserDef_Rage = myNormal_Rage; // select our fit and rang-check function
// start to get the matlab data and check the size
nCountD = mxGetN(prhs) * mxGetM(prhs);
xData = (double *) mxGetData (prhs);
if(nCountD != mxGetN(prhs) * mxGetM(prhs))bError=true;
yData = (double *) mxGetData (prhs);
nCountA = mxGetN(prhs) * mxGetM(prhs);
pFitA= (double *) mxGetData (prhs);
if(nCountA != mxGetN(prhs) * mxGetM(prhs))bError=true;
pFitA_fit= (double *) mxGetData (prhs);
if (5==nrhs){
if(nCountA != mxGetN(prhs) * mxGetM(prhs))bError=true;
pFitErr = (double *) mxGetData (prhs);
}
if(bError) mexErrMsgTxt("Using: chi = myGAUSS_FIT(X, Y, A, F [, E]); size X = Y and size A = F [= E]");
sigData = (double *)malloc(nCountD * sizeof (double));
for(i=0;i<nCountD;i++)sigData = 1;// here we use same priority for all the data
i= nCountA * nCountA * sizeof (double);
pCovar = (double *) malloc(i); pAlpha = (double *) malloc(i);
for(i=0; i<nCountA; i++){// generate our working place and copy data
ppCovar=&(pCovar);
ppAlpha=&(pAlpha);
nFit_fit = pFitA_fit;
pFit = pFitA;
}
j= MyMrqmin(xData, yData, sigData, nCountD, pFit, nFit_fit, nCountA,
ppCovar, ppAlpha, &dChisq, &dAlamda);// Doing fit
plhs = mxCreateDoubleMatrix(1 , 1, mxREAL);
plhs = mxCreateDoubleMatrix(1 , 1, mxREAL);
*(mxGetPr(plhs))= j;
if(pFitErr!=NULL)
for(i=0, j = nCountD - nCountA; i<nCountA; i++){ // Save data and error matrix to matlab
pFitA = pFit;
if(nFit_fit!=0 && j>0) pFitErr = sqrt( dChisq * ppCovar/j );
else pFitErr = 0;
}
*(mxGetPr(plhs))= dChisq;// return model error to matlab
free(pAlpha);
free(pCovar);
free(sigData);
}
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
// myFIT_FUN.hpp
// Last Modification:BGB, 2007.03.03
// Any question related to LM fit, please contact guobinbao@sina.com
//---------------------------------------------------------------------------
#ifndef myFIT_FUN_hpp
#define myFIT_FUN_hpp
//---------------------------------------------------------------------------
#include <math.h>
//#include <stdio.h>
//#include <windows.h>
//---------------------------------------------------------------------------
#define MY_MAX 27
#define MY_MAX_LAMDA 1e17
#define MY_MAX_TIMES_A 100
#define MY_MAX_TIMES_B 17
//---------------------------------------------------------------------------
bool DimensionCheck(int n){
if(n>MY_MAX){
//MessageBox(NULL, "MAX dimention is limited!", "Error in MAX dimention",MB_OK | MB_ICONWARNING );
return false;
}
return true;
}
//---------------------------------------------------------------------------
void MyGaussJ(double **a, int n, double **b, int m){
//Linear equation solution by Gauss-Jordan elimination.
//a is the input matrix. b is the m right-hand side vectors.
//On output, a is replaced by its matrix inverse, and b is replaced by the corresponding set
double *fpL,*fpR, *pfTemp;
int nIndex,nIndexT;
int nIndeC,nIndeR;
int i,j,k, l, irow, icol, iTemp;
double fbig, fTemp;
for(i=0;i<n;i++){
fpL=a; fpR=b;
nIndex=i; nIndexT=i;
}
for(i=0;i<n;i++){
fbig=0; irow = icol = i;
for (j=i;j<n;j++)
for (k=i;k<n;k++)
if ((fTemp=fabs(fpL])) >= fbig){
fbig = fTemp;
nIndeR=nIndexT; nIndeC=nIndex;
}
if (irow != i){
pfTemp=fpL; fpL=fpL; fpL= pfTemp;
pfTemp=fpR; fpR=fpR; fpR= pfTemp;
iTemp = nIndexT; nIndexT = nIndexT; nIndexT= iTemp;
}
if (icol != i){
iTemp = nIndex; nIndex = nIndex; nIndex= iTemp;
}
if ( (fTemp = fpL]) == 0.0){
//MessageBox(NULL, "All 0 in array!", "All 0 in Array!",MB_OK | MB_ICONWARNING );
return;
}
fTemp=1.0/fTemp;fpL]=1;
for (j=0;j<n;j++) fpL] *= fTemp;
for (j=0;j<m;j++) fpR *= fTemp;
for (j=0;j<n;j++){
if(j==i)continue;
fTemp = fpL; fpL=0.0;
for (k=0;k<n;k++){
l=nIndex;
fpL -= fpL * fTemp;
}
for (k=0;k<m;k++)
fpR -= fpR * fTemp;
}
}
for(i=0;i<n;i++){
nIndex=i; nIndexT=i;
}
// Until Here, we have all the data, if we donot need an inv, we can quit at this point
// Now, we reshape the matrix to fit the real INV of the input
for (i=0;i<n;i++){
if((k = nIndex])!= (l = nIndeC)){
iTemp = nIndexT;nIndexT=nIndexT;nIndexT=iTemp;
nIndex]=l;
nIndex]=k;
for (j=0;j<n;j++){
fTemp=a; a=a; a= fTemp;
}
for (j=0;j<m;j++){
fTemp=b; b=b; b= fTemp;
}
}
nIndeC=l;
nIndeR=k;
}
for (i=n-1;i>=0;i--){
if((k = nIndeR)!= (l = nIndeC)){
for (j=0;j<n;j++){
fTemp=a; a=a; a= fTemp;
}
}
}
}
//---------------------------------------------------------------------------
void MyGaussJ1(double **a, int n, double **b, int m){
//Help function: Linear equation solution call gate for fortran to c
// or to any Matrix that are not leading with 0 but 1 in the index
double *fpL,*fpR;
int i;
for(i=0;i<n;i++){
fpL=&(a); fpR=&(b);
}
MyGaussJ(fpL, n, fpR, m);
}
//---------------------------------------------------------------------------
void (*UserDef_funcs)(double x, double *para, double *y, double *dyda, int na);
// This is call gate for user define function
// x: incoming data point for x
//para ~ para: fit par
// y: outgoing data from the function
//dyda ~ dyda: outgoing data, the first partical derivative of each fit par
//---------------------------------------------------------------------------
void (*UserDef_Rage)(double *para, int na, int *ia, int nfit);
// This is user define range check function
//---------------------------------------------------------------------------
void MyCovsrt(double **covar, int ma, int *ia, int mfit){
//When end, we need to reshape the error matrix, if mfit != ma
int i,j,k;
float fTemp;
for (i=mfit;i<ma;i++)
for (j=0;j<i;j++) covar=covar=0.0;
k=mfit-1;
for (j=ma-1;j>=0;j--) {
if (ia) {
for (i=0;i<ma;i++){
fTemp=covar; covar= covar; covar=fTemp;
}
for (i=0;i<ma;i++){
fTemp=covar; covar= covar; covar=fTemp;
}
k--;
}
}
}
//---------------------------------------------------------------------------
double MyMrqcof(double *x, double *y, double *sig, int ndata, double *a, int *ia,
int ma, double **alpha, double *beta ){
// help function to calculate the Chi square and H' matrix
// For Newton way, one need to calculate again the 2ed derivative to get a full Hession matrix
int i, j, k, l, m, mfit=0;
double ymod, wt, sig2i, dy;
double fdyda;
double fChisq=0;
for (j=0;j<ma;j++) if (ia) mfit++;
for (j=0;j<mfit;j++) { // Initialize a symmetric alpha, beta.
for (k=0;k<=j;k++) alpha=0.0;
beta=0.0;
}
for (i=0;i<ndata;i++) { //loop over all data and cal Hession' .
(*UserDef_funcs)(x, a , &ymod, fdyda, ma );
sig2i=1.0/(sig*sig);
dy=y-ymod;
for (j=0,l=0;l<ma;l++) {
if (ia) {
wt=fdyda*sig2i;
for (k=0,m=0;m<=l;m++)
if (ia)
alpha += wt*fdyda;
beta += dy*wt;
}
}
fChisq += dy*dy*sig2i; // Cal Chi square.
}
for (j=1;j<mfit;j++) //We suppose a symmetric Hession'
for (k=0;k<j;k++) alpha=alpha;
return fChisq;
}
//---------------------------------------------------------------------------
void MyMrqmin_1(double *x, double *y, double *sig, int ndata, double *a, int *ia,
int ma, double **covar, double **alpha, double *chisq, double *alamda){
int j,k,l;
static int mfit;
static double ochisq, fAtry, fBeta, da, *pOneda;
double dTemp;
if (*alamda < 0.0){ // We use a 0 value to Initialize the starting point
for (mfit=0,j=0;j<ma;j++)if (ia) mfit++;
*alamda= 1;
*chisq = ochisq= MyMrqcof(x,y,sig,ndata,a,ia,ma,alpha, fBeta);
for (j=0;j<ma;j++){ fAtry=a; pOneda=&(da);}
}
// Alter linearized fitting matrix, by augmenting diagonal elements.
for (j=0;j<mfit;j++) {
for (k=0;k<mfit;k++) covar=alpha;
#ifndef MY_LCC
try{
#endif
dTemp = ((double)(alpha))*(1.0+(*alamda));
#ifndef MY_LCC
} catch(...){
dTemp = 0;
}
#endif
covar= dTemp;
da=fBeta;
}
MyGaussJ(covar, mfit, pOneda, 1);
if (*alamda == 0.0) { // Once converged, evaluate covariance matrix.
MyCovsrt(covar, ma, ia, mfit);
if(*chisq > ochisq)*chisq= ochisq;
return;
}
//Did the trial succeed?
for (j=0,l=0;l<ma;l++)
if (ia) fAtry=a + da;
///////////////////////////////////////////////////////////////////////////////
(*UserDef_Rage)(fAtry, ma, ia, mfit);
///////////////////////////////////////////////////////////////////////////////
*chisq = MyMrqcof(x,y,sig,ndata,fAtry,ia,ma,covar,da);
if (*chisq < ochisq) { // Success, accept the new solution.
*alamda *= 0.1;
ochisq = (*chisq);
for (j=0;j<mfit;j++) {
for (k=0;k<mfit;k++) alpha=covar;
fBeta=da;
}
for (l=0;l<ma;l++) a=fAtry;
}else{// Failure, increase alamda and return.
*alamda *= 10;
*chisq = ochisq;
}
}
//---------------------------------------------------------------------------
int MyMrqmin(double *x, double *y, double *sig, int ndata, double *a, int *ia,
int ma, double **covar, double **alpha, double *pdChiq, double *pdAlamdaq ){
double dChi, dAlamda=-1;
int i,j;
MyMrqmin_1(x, y, sig, ndata, a, ia, ma, covar, alpha, &dChi, &dAlamda);
*pdChiq = dChi; *pdAlamdaq = dAlamda; i=j=0;
while(dAlamda < MY_MAX_LAMDA && i<MY_MAX_TIMES_A && j<MY_MAX_TIMES_B){
//printf("%d dAlamda=%lg\t dChi=%lg \n", i, dAlamda, dChi);
MyMrqmin_1(x, y, sig, ndata, a, ia, ma, covar, alpha, &dChi, &dAlamda);
if(*pdAlamdaq < dAlamda){ // On success, dAlamda will reduce 1/10 and we also monitor this
j++;
}else{
j=0;
}
*pdAlamdaq = dAlamda;
i++;
}
//printf("%d dAlamda=%lg\t dChi=%lg \n", i, dAlamda, dChi);
*pdChiq = dChi; *pdAlamdaq = dAlamda; dAlamda = 0;
MyMrqmin_1(x, y, sig, ndata, a, ia, ma, covar, alpha, &dChi, &dAlamda);
return i;
}
//---------------------------------------------------------------------------
#endif
注：和以前一样，默认的是还是LCC，如果是用VC 或者BC编译时请remark "#define MY_LCC"。
另外，我们在这里用了“最大绝对值优先”的Gauss-Jordan法计算矩阵的逆，这个方法的优点是稳定，缺点是速度比QR decomposition慢。考虑到拟合的精度，我们还是推荐这个方法。
时间关系，这里的代码自己只加了一点点解释，假如有任何疑问和问题，欢迎回贴或给我消息:-)
[ 本帖最后由 recbio 于 2007-6-22 11:17 编辑 ] 强人～～～ 目前正在郁闷中。。因为好不容易编的S FUNCTION也要改C 。。。 N多矩阵之间的运算。。。 头都大了。 是原创吗，顶一下。 慢慢看
页:
[1]