#include "stdafx.h"
#include <math.h>
#include <stdio.h>
#define m_col 3
#define m_dim 3
void InvMatrix(const double M[][m_col], const int n, double invM[][m_col]);
void LinearFit(const double Coef[][m_dim], const int n, double *Para);
void main()
{
double A[4][3] = {0,1,7,1,0,6,2,1,11,-1,-1,-1};
double Para[3];
LinearFit(A, 4, Para);
printf("%lf %lf %lf",Para[0],Para[1],Para[2]);
}
void LinearFit(const double Coef[][m_dim], const int n, double *Para)
{
double (*A)[m_col] = new double[m_dim][m_col];
for(int i=0;i<m_dim-1;i++)
{
for(int j=0;j<m_dim-1;j++)
{
A[i][j] = 0;
for(int k=0;k<n;k++)
A[i][j]+= Coef[k][i]*Coef[k][j];
}
}
for(int i=0;i<m_dim-1;i++)
{
A[i][m_dim-1] = A[m_dim-1][i] = 0;
for(int j=0;j<n;j++)
A[i][m_dim-1]+= Coef[j][i];
A[m_dim-1][i] = A[i][m_dim-1];
}
A[m_dim-1][m_dim-1] = n;
double *b = new double[m_dim];
for(int i=0;i<m_dim-1;i++)
{
b[i] = 0;
for(int j=0;j<n;j++)
b[i]+= Coef[j][m_dim-1]*Coef[j][i];
}
b[m_dim-1] = 0;
for(int i=0;i<n;i++)
b[m_dim-1]+= Coef[i][m_dim-1];
double (*invA)[m_col] = new double[m_dim][m_col];
InvMatrix(A, m_dim, invA);
for(int i=0;i<m_dim;i++)
{
Para[i] = 0;
for(int j=0;j<m_dim;j++)
Para[i]+= invA[i][j]*b[j];
}
delete []A;
delete []invA;
delete b;
}
void InvMatrix(const double M[][m_col], const int n, double invM[][m_col])
{
if(n==1)
invM[0][0] = 1/M[0][0];
else
{
double cu, cd, normb;
double (*b)[m_col] = new double[n][m_col];
double (*invv)[m_col] = new double[n][m_col];
for(int j=0;j<n;j++)
{
double *schu = new double[j];
double *schd = new double[j];
if(j>0)
{
for(int k=0;k<j;k++)
{
schu[k] = schd[k] = 0;
for(int i=0;i<n;i++)
{
schu[k]+= b[i][k]*M[i][j];
schd[k]+= b[i][k]*b[i][k];
}
}
}
normb = 0;
for(int i=0;i<n;i++)
{
b[i][j] = M[i][j];
if(j>0)
{
for(int k=0;k<j;k++)
b[i][j]-= b[i][k]*schu[k]/schd[k];
}
normb+= b[i][j]*b[i][j];
}
normb = sqrt(normb);
for(int i=0;i<n;i++)
invv[j][i] = b[i][j]/normb;
delete schu;
delete schd;
}
double (*c)[m_col] = new double[n][m_col];
for(int i=0;i<n;i++)
{
for(int j=0;j<=i;j++)
{
cu = cd = 0;
if(j<i)
{
for(int k=0;k<n;k++)
{
cu+= M[k][i]*b[k][j];
cd+= b[k][j]*b[k][j];
}
c[j][i] = cu/sqrt(cd);
}
else
{
for(int k=0;k<n;k++)
cd+= b[k][j]*b[k][j];
c[j][i] = sqrt(cd);
}
}
}
double (*invc)[m_col] = new double[n][m_col];
for(int j=0;j<n;j++)
{
for(int i=n-1;i>=0;i--)
{
if(i>j)
invc[i][j] = 0;
else if(i==j)
invc[i][j] = 1/c[i][j];
else
{
invc[i][j] = 0;
for (int k=i+1;k<=j;k++)
invc[i][j]-= c[i][k]*invc[k][j];
invc[i][j]/= c[i][i];
}
}
}
for(int i=0;i<n;i++)
{
for(int j=0;j<n;j++)
{
invM[i][j] = 0;
for(int k=0;k<n;k++)
invM[i][j]+= invc[i][k]*invv[k][j];
}
}
delete []b;
delete []c;
delete []invv;
delete []invc;
}
}
最近做项目恰好遇到这个问题,编了个小程序。
linerfit函数为矩阵法的多元线性最小二乘求解函数, 当m_dim设置为3时即可求平面的最小二乘,当然m_dim设置为2可求平面直线最小二乘。注意的是m_col应定义为大于等于m_dim。若目标函数为z=ax+by+c;那么coef传参格式为:
x1 y1 z1
x2 y2 z2
....
求解结果para = [a b c].
当然你也可以用InvMatrix子函数做其他用途,其为QR法快速求逆矩阵。
[
本帖最后由 eastlife0108 于 2014-4-7 15:21 编辑 ]