非线性最小二乘法之lm算法的c代码

函数：f=2*jn*(atan((b + o - x)/h) + atan((b - o + x)/h)) - 2*jn*(atan((b + o - x)/(h + 1)) + atan((b - o + x)/(h + 1)))
这是一个实际的工程应用公式。参数为jn，o，b，h。

//main函数,写的比较粗糙，但是可以运行，实际工程应用结果也比较理想。
//如果想提高精度，将float换成double,结果将与matlab库函数的结果一致。
float data_1[N] = {-63.5,-38.1,-12.7,12.7,38.1,63.5};
float obs_1[N] = {0,8,11,1,-1,0};
float pparams[4] = {200,0,15,30};
//lm(data_1,obs_1,4,6,pparams);
//return 0;

float jn_est=200,b_est=15,o_est=0,h_est=30; 
float jn_lm,b_lm,o_lm,h_lm;
int Ndata=N;// 数据个数 
int Nparams=4;// 参数维数 
int n_iters=500;// 迭代最大次数 
float lamda=0.01;// LM算法的阻尼系数初值
float e_delta = 0.00001;

int updateJ=1;
float y_est[N];//函数值
float y_estlm[N];
float d[N]; //误差
float d_lm[N];
float e;
float e_lm;
float dp[4];//步长
float* J = (float *)malloc(Ndata*Nparams*sizeof(float));
float* J_transpose = (float *)malloc(Ndata*Nparams*sizeof(float));
float* H = (float *)malloc(Nparams*Nparams*sizeof(float));
float* H_lm = (float *)malloc(Nparams*Nparams*sizeof(float));
int itern =0;
for(itern=0;itern<n_iters;itern++)
{
if(updateJ == 1)
{       
for (int i=0;i<Ndata;i++)
{
//根据当前估计值，计算雅克比矩阵,顺序jn,o,b,h
*(J+(i*Nparams+0)) = 2*atan((b_est + o_est - data_1[i])/h_est) + 2*atan((b_est - o_est + data_1[i])/h_est) - 2*atan((b_est + o_est - data_1[i])/(h_est + 1)) - 2*atan((b_est - o_est + data_1[i])/(h_est + 1));
*(J+(i*Nparams+1)) = 2*jn_est*(1/(h_est*((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i])/(h_est*h_est) + 1)) - 1/(h_est*((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i])/(h_est*h_est) + 1))) - 2*jn_est*(1/(((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i])/((h_est + 1)*(h_est + 1)) + 1)*(h_est + 1)) - 1/(((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i])/((h_est + 1)*(h_est + 1)) + 1)*(h_est + 1)));
*(J+(i*Nparams+2)) = 2*jn_est*(1/(h_est*((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i])/(h_est*h_est) + 1)) + 1/(h_est*((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i])/(h_est*h_est) + 1))) - 2*jn_est*(1/(((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i])/((h_est + 1)*(h_est + 1)) + 1)*(h_est + 1)) + 1/(((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i])/((h_est + 1)*(h_est + 1)) + 1)*(h_est + 1)));
*(J+(i*Nparams+3)) = 2*jn_est*((b_est + o_est - data_1[i])/((((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i]))/((h_est + 1)*(h_est + 1)) + 1)*((h_est + 1)*(h_est + 1))) + (b_est - o_est + data_1[i])/((((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i]))/((h_est + 1)*(h_est + 1)) + 1)*((h_est + 1)*(h_est + 1)))) - 2*jn_est*((b_est + o_est - data_1[i])/((h_est*h_est)*(((b_est + o_est - data_1[i])*(b_est + o_est - data_1[i]))/(h_est*h_est) + 1)) + (b_est - o_est + data_1[i])/((h_est*h_est)*(((b_est - o_est + data_1[i])*(b_est - o_est + data_1[i]))/(h_est*h_est) + 1)));
//根据当前参数，得到函数值
y_est[i] = 2*jn_est*(atan((b_est + o_est - data_1[i])/h_est) + atan((b_est - o_est + data_1[i])/h_est)) - 2*jn_est*(atan((b_est + o_est - data_1[i])/(h_est + 1)) + atan((b_est - o_est + data_1[i])/(h_est + 1)));
d[i] = obs_1[i] - y_est[i];
}
transpose_multi_self(J,H,Nparams,Ndata);
if(itern==0) 
{
e=dot(d,N);
}
}
// 根据阻尼系数lamda混合得到H矩阵,H_lm=H+(lamda*MatrixXd::Identity(Nparams,Nparams));
diagofmultiadd(lamda,H,H_lm,Nparams);
inv(H_lm,Nparams);
transpose(J,J_transpose,Nparams,Ndata);
float dtemp[N];
multimatrix(J_transpose,d,dtemp,Ndata,Nparams,1,Ndata); 
multimatrix(H_lm,dtemp,dp,Nparams,Nparams,1,Nparams);       
jn_lm=jn_est+dp[0];
o_lm=o_est+dp[1];
b_lm=b_est+dp[2];
h_lm=h_est+dp[3];

for (int i=0;i<Ndata;i++)
{
y_estlm[i] = 2*jn_lm*(atan((b_lm + o_lm - data_1[i])/h_lm) + atan((b_lm - o_lm + data_1[i])/h_lm)) - 2*jn_lm*(atan((b_lm + o_lm - data_1[i])/(h_lm + 1)) + atan((b_lm - o_lm + data_1[i])/(h_lm + 1)));
d_lm[i] = obs_1[i] - y_estlm[i];
}

//d_lm=obs_1-y_estlm;
e_lm=dot(d_lm,Ndata);
if(e_lm<e)
{
lamda=lamda/5;
jn_est=jn_lm;
b_est=b_lm;
o_est=o_lm;
h_est=h_lm;         
printf("item=%d:\tjn=%f\tb=%f\to=%f\th=%f\n",itern,jn_est,b_est,o_est,h_est);
if(e-e_lm<e_delta)
break;
e=e_lm;
updateJ=1;
}
else
{
updateJ=0;
lamda=lamda*5;
}
}
cout<<"itern="<<itern<<endl<<"e_lm="<<e_lm<<endl;
cout<<"jn="<<jn_est<<endl;
cout<<"b="<<b_est<<endl;
cout<<"o="<<o_est<<endl;
cout<<"h="<<h_est<<endl;

free(J);
free(J_transpose);
free(H);
free(H_lm);

return 0;

矩阵操作，其中的inv函数来自http://blog.sina.com.cn/s/blog_62450b980100f5ny.html

//方阵m的对角线和lamda相加
void diagofmultiadd(float lamda,float* mi,float* mo,int w)
{
    memcpy(mo,mi,w*w*sizeof(float));
    for(int i=0;i<w;i++)
    {   
        int pos = i*w+i;
        *(mo+pos) = *(mi+pos) +lamda;
    }
}

int transpose(float *mi,float *mo,int w,int h)
{
    for(int i=0;i<h;i++)
    {
        for(int j=0;j<w;j++)
        {
            *(mo+j*h+i) = *(mi+i*w+j);
            //swap(p+i*w+j,p+j*h+i);//交换指针
        }
    }
    return 0;
}
//转置结果保存到p
int transpose(float *p,int w,int h)
{
    float *mt = (float *)malloc(w*h*sizeof(float));
    transpose(p,mt,w,h);
    memcpy(p,mt,w*h*sizeof(float));
    free(mt);

    return 0;
}
//m1*m2=mo
int multimatrix(float *m1,float *m2,float *mo,int w1,int h1,int w2,int h2)
{
    //print(m1,w1,h1);
    //print(m2,w2,h2);
    if(w1!=h2)return -1;
    for(int i=0;i<h1;i++)
    {
        for(int j=0;j<w2;j++)
        {
            float temp =0;
            for(int k=0;k<w1;k++)
                temp+= *(m1+i*w1+k) * *(m2+k*w2+j);
            *(mo+i*w2+j) = temp;
        }
    }
    return 0;
}
int multimatrix(float rate,float *m,int len)
{
    int i=0;
    while(i<len)
    {
        *(m+i) *= rate;
        i++;
    }
    return 0;

}
void transpose_multi_self(float* mi,float* mo,int w,int h)
{
    float *mt = (float *)malloc(w*h*sizeof(float));
    transpose(mi,mt,w,h);
    for(int i=0;i<w;i++)
    {
        for(int j=0;j<w;j++)
        {
            *(mo+(i*w+j)) = dot(mt+(i*h),mt+(j*h),h);
        }
    }


    free(mt);
}

float dot(float *v1,float *v2,int len)
{
    float ret = 0;
    for (int i=0;i<len;i++)
    {
        ret += *(v1+i) * *(v2+i);
    }
    return ret;
}
float dot(float *v,int len)
{
    float ret = 0;
    for (int i=0;i<len;i++)
    {
        ret += *(v+i) * *(v+i);
    }
    return ret;
}

int inv(float *p,int n)
{
    void swap(float *a,float *b);
    int *is,*js,i,j,k,l;

    float temp,fmax;
    is=(int *)malloc(n*sizeof(int));
    js=(int *)malloc(n*sizeof(int));
    for(k=0;k<n;k++)
    {
        fmax=0.0;
        for(i=k;i<n;i++)
            for(j=k;j<n;j++)
            { temp=fabs(*(p+i*n+j));//找最大值
        if(temp>fmax)
        { fmax=temp;
        is[k]=i;js[k]=j;
        }
        }
        if((fmax+1.0)==1.0)
        {  free(is);free(js);
        printf("no inv");
        return(0);
        }
        if((i=is[k])!=k)
            for(j=0;j<n;j++)
                swap(p+k*n+j,p+i*n+j);//交换指针
        if((j=js[k])!=k)
            for(i=0;i<n;i++)
                swap(p+i*n+k,p+i*n+j);  //交换指针
        p[k*n+k]=1.0/p[k*n+k];
        for(j=0;j<n;j++)
            if(j!=k)
                p[k*n+j]*=p[k*n+k];
        for(i=0;i<n;i++)
            if(i!=k)
                for(j=0;j<n;j++)
                    if(j!=k)
                        p[i*n+j]=p[i*n+j]-p[i*n+k]*p[k*n+j];
        for(i=0;i<n;i++)
            if(i!=k)
                p[i*n+k]*=-p[k*n+k];
    }
    for(k=n-1;k>=0;k--)
    {
        if((j=js[k])!=k)
            for(i=0;i<n;i++)
                swap(p+(j*n+i),p+(k*n+i));
        if((i=is[k])!=k)
            for(j=0;j<n;j++)
                swap(p+(j*n+i),p+(j*n+k));
    }
    free(is);
    free(js);
    return 1;

}
void swap(float *a,float *b)
{  
    float c;
    c=*a;
    *a=*b;
    *b=c;
}

欢迎交流