最小二乘法

最小二乘法，是解方程中用到比较常用的方法。以下代码是本人在vs2005上进行调试编译过的。属于原创——后续继续推出递推最小二乘法的代码。

// least_squares.cpp : Defines the entry point for the console application.
/***************************************************************************
*  least_squares.cpp
*  Author  tcg
*  Copyright  2015.3.20  
*  Email  [e-mail]
****************************************************************************/


#include "stdafx.h"
#include "stdio.h"
#include <string>
#include "math.h"


// 接口
int least_squares(double *mtrx_data,int col_num,int row_num,int cal_col_num,double *solution);//最小二乘法接口
int solve_linear_eqts(double *in_mtrx,int col_num,int row_num,int colNum,double *solution);//得到最优解 
int reduce_unknowns(double *mtrx_tmp,int col_num,int rowNo,int colNo,int row_num,int colNum);//参数优化,较少未知参数


// 返回1 -succes;0-fail
int least_squares(double *mtrx_data,int col_num,int row_num,int cal_col_num,double *solution)
{
int k,l,j;  
double *eqt;
int *q;
q=&cal_col_num;
eqt=(double *)malloc(cal_col_num*cal_col_num*sizeof(double));


for(k=0;k<*q-1;k++)
{
for(l=0;l<*q-1;l++)
{
eqt[k*col_num+l]=0;
for(j=0;j<row_num;j++)
eqt[k*col_num+l]+=*(mtrx_data+col_num*j+k)*(*(mtrx_data+col_num*j+l));
}
}
for(k=0;k<*q-1;k++)
{
eqt[k*col_num+*q-1]=0;
for(j=0;j<row_num;j++)
eqt[k*col_num+*q-1]+=*(mtrx_data+col_num*j+*q-1)*(*(mtrx_data+col_num*j+k));
}


if(!solve_linear_eqts(eqt,col_num,(*q-1),*q,solution)) 
{
if (eqt)
free(eqt);
return 0;
}
if(eqt)
free(eqt);


return 1;
}




int solve_linear_eqts(double *in_mtrx,int col_num,int row_num,int cal_col_num,double *solution)
{
int i,j;
double tmp_sum;
if(row_num!=(cal_col_num-1)) 
{
return 0;
}

for(i=0;i<cal_col_num-2;i++)
{  
if(!reduce_unknowns(in_mtrx,col_num,i,i,row_num,cal_col_num)) 
{
return 0;
}
}


for(i=row_num-1;i>=0;i--)
{
tmp_sum=0;
for(j=i+1;j<row_num;j++)
tmp_sum+=*(solution+j)*(*(in_mtrx+i*col_num+j));


if(fabs(in_mtrx[i*col_num+i])<0.000001) 
{
return 0;      
}
*(solution+i)=(*(in_mtrx+i*col_num+cal_col_num-1)-tmp_sum)/(*(in_mtrx+i*col_num+i));
}
return 1;
}




int reduce_unknowns(double *mtrx_tmp,int col_num,int row_no,int col_no,int row_num,int colnum)
{
int i,j;
double tmp_db_l1,tmp_db_l2;
double *p,*mp;




p=mtrx_tmp+row_no*col_num+col_no;
mp=mtrx_tmp+row_no*col_num+col_no;

tmp_db_l1=*p;
p+=col_num;
for(i=row_no+1;i<row_num;i++)
{
if(fabs(*p)<0.000001) continue;
tmp_db_l2=*p;
for(j=col_no;j<colnum;j++)
{
*p=*p-*mp*tmp_db_l2/tmp_db_l1;
p++;
mp++;
}
p+=col_num-(colnum-col_no);
mp-=colnum-col_no;
}
return 1;
}


// 校准公式
// λ = m_aFit[0] + m_aFit[1] *n + m_aFit[2] *n2 + m_aFit[3] *pixnr3 + m_aFit[4] *pixnr4


#define COL_NUM 6//5
#define ROW_NUM 20//5//6
int main()
{
int i,j;
double solution[COL_NUM-1],eqt[COL_NUM][COL_NUM]={0};


// 测试程序
// 用实际波长代入计算
double a[ROW_NUM][COL_NUM] = 
{
{ 1,51.2,51.2*51.2,51.2*51.2*51.2,51.2*51.2*51.2*51.2,312.75},
{ 1,137.9,137.9*137.9,137.9*137.9*137.9,137.9*137.9*137.9*137.9,365.02},
{ 1,203.6,203.6*203.6,203.6*203.6*203.6,203.6*203.6*203.6*203.6,404.44},
{ 1,209.1,209.1*209.1,209.1*209.1*209.1,209.1*209.1*209.1*209.1,407.74},
{ 1,441.9,441.9*441.9,441.9*441.9*441.9,441.9*441.9*441.9*441.9,546.12},
{ 1,698.7,698.7*698.7,698.7*698.7*698.7,698.7*698.7*698.7*698.7,696.47},
{ 1,716.3,716.3*716.3,716.3*716.3*716.3,716.3*716.3*716.3*716.3,706.68},
{ 1,730.1,730.1*730.1,730.1*730.1*730.1,730.1*730.1*730.1*730.1,714.69},
{ 1,751.7,751.7*751.7,751.7*751.7*751.7,751.7*751.7*751.7*751.7,727.20},
{ 1,770.9,770.9*770.9,770.9*770.9*770.9,770.9*770.9*770.9*770.9,738.31},
{ 1,792.3,792.3*792.3,792.3*792.3*792.3,792.3*792.3*792.3*792.3,750.67},
{ 1,814.4,814.4*814.4,814.4*814.4*814.4,814.4*814.4*814.4*814.4,763.42},
{ 1,829.8,829.8*829.8,829.8*829.8*829.8,829.8*829.8*829.8*829.8,772.30},
{ 1,868.7,868.7*868.7,868.7*868.7*868.7,868.7*868.7*868.7*868.7,794.68},
{ 1,897.4,897.4*897.4,897.4*897.4*897.4,897.4*897.4*897.4*897.4,811.15},
{ 1,923.9,923.9*923.9,923.9*923.9*923.9,923.9*923.9*923.9*923.9,826.34},
{ 1,968.8,968.8*968.8,968.8*968.8*968.8,968.8*968.8*968.8*968.8,852.01},
{ 1,1074.7,1074.7*1074.7,1074.7*1074.7*1074.7,1074.7*1074.7*1074.7*1074.7,912.27},
{ 1,1092.5, 1092.5*1092.5,1092.5*1092.5*1092.5,1092.5*1092.5*1092.5*1092.5,922.36},
{ 1,1169.3,1169.3*1169.3,1169.3*1169.3*1169.3,1169.3*1169.3*1169.3*1169.3,965.73}


};


for ( j = 5; j <= ROW_NUM; j++ )
{
least_squares(&a[0][0],COL_NUM,j,COL_NUM,&solution[0]);


for(i=0;i<COL_NUM-1;i++) 
{
printf("%.14lf ",solution[i]);

}
printf("\n");
printf("\n");
}






while(1); // 程序停住，方便观察结果。
}