最小二乘法拟合n阶曲线

最小二乘法拟合n阶曲线

// An highlighted block

/*********************************************
*        利用公式 ax=y--A'Ax=A'y 拟合曲线     *
*            用最小二乘法进行曲线拟合          *
*********************************************/

#include <math.h>
#include <stdio.h>

#define N 4

void arryans(double a[][N], double b[N], double x[N], int n) //解一个n阶的线性方程组
{
    int i, j, k, mi;
    double mx, tmp;

    for (i = 0; i < n - 1; i++) {
        for (j = i + 1, mi = i, mx = fabs(a[i][i]); j < n; j++) //找主元素
            if (fabs(a[j][i]) > mx) {
                mi = j;
                mx = fabs(a[j][i]);
            }
        if (i < mi) { //交换两行
            tmp = b[i];
            b[i] = b[mi];
            b[mi] = tmp;
            for (j = i; j < n; j++) {
                tmp = a[i][j];
                a[i][j] = a[mi][j];
                a[mi][j] = tmp;
            }
        }
        for (j = i + 1; j < n; j++) { //高斯消元
            tmp = -a[j][i] / a[i][i];
            b[j] += b[i] * tmp;
            for (k = i; k < n; k++)
                a[j][k] += a[i][k] * tmp;
        }
    }
    x[n - 1] = b[n - 1] / a[n - 1][n - 1]; //求解方程
    for (i = n - 2; i >= 0; i--) {
        x[i] = b[i];
        for (j = i + 1; j < n; j++)
            x[i] -= a[i][j] * x[j];
        x[i] /= a[i][i];
    }
}

void arrytrans(double a[][N], double b[][N], int m, int n) //矩阵转置,m行n列
{
    int i, j;

    for (i = 0; i < m; i++)
        for (j = 0; j < n; j++)
            b[j][i] = a[i][j];
}

void arrymul(double a[][N], double b[][N], double c[][N], int m, int l, int n) //两个矩阵的乘法
{
    int i, j, k; //c[m][n]=a[m][l]*b[l][n]

    for (i = 0; i < m; i++)
        for (j = 0; j < n; j++) {
            c[i][j] = 0;
            for (k = 0; k < l; k++)
                c[i][j] += a[i][k] * b[k][j];
        }
}

void arrycon(double a[], double b[][N], int m, int n) //矩阵一维变二维
{
    int i, j, k;

    for (i = 0; i < m; i++)
        for (j = 0; j < n; j++) {
            b[i][j] = 1;
            for (k = 0; k < j; k++)
                b[i][j] *= a[i];
        }
}

void arryzeng(double a[][N], double b[], double c[][N], int m, int n) //增广矩阵
{
    int i, j;

    for (i = 0; i < m; i++)
        for (j = 0; j <= n; j++) {
            if (j == n)
                c[i][j] = b[i];
            else
                c[i][j] = a[i][j];
        }
}

void arrymuls(double a[][N], double b[], double c[], int m, int n) { //由于一维数组按行存放，故乘列向量单独写
    int i, j;

    for (i = 0; i < m; i++) {
        c[i] = 0;
        for (j = 0; j < n; j++)
            c[i] += a[i][j] * b[j];
    }
}

double power(double a[], double x, int i, int j) //用秦九韶算法计算多项式
{
    if (i >= j)
        return a[i] * x;
    else
        return x * power(a, x, i + 1, j) + a[i];
}

/*
 * 最小二乘法拟合函数 y = k0 + k1*x + k2*x^2 + k3*x^3 + ... + kn*x^n
 * x[] 拟合数据的x向量
 * y[] 拟合数据的y向量
 * n 拟合的阶数
 * k[] 拟合后的系数向量 k0=k[0],k1=k[1],...,kn=k[n]
 * */
void CalLS(double x[N], double y[N], int n, double k[N]) {
    int m;
    double a[N][N], b[N][N], c[N][N], d[N];
    n++;                          //n是拟合的阶数
    m = N;                       //m是初始数据的长度
    arrycon(x, a, m, n);          //a[][]是A
    arrytrans(a, b, m, n);        //b[][]是A'
    arrymul(b, a, c, n, m, n);    //c[][]是A'*A
    arrymuls(b, y, d, n, m);      //d[][]是A'*y
    arryans(c, d, k, n);          //k[]是解，即最后的系数，按升幂排序
}