c实现LM最小二乘法及矩阵运算函数库的实现

因为实习工作需要使用到LM最小二乘法进行风场反演,所以我用c实现了矩阵相关运算的函数库,并实现了LM最小二乘法。

一,LM最小二乘法的实现理论步骤

二,矩阵运算相关函数的实现

/**************************************************************/
/********************** Matrix_functions.h *******************/
/*
/*                      矩阵运算函数的定义                   */
/***********************************************************/
#pragma once
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include<math.h>
#define N 2


//一,矩阵转置:right:输入矩阵,result:输出矩阵,hight:输入矩阵的高,width:输入矩阵的宽
void Multi_transpose(float * right, float * result, int hight, int width);

//二,矩阵求反:right:输入矩阵,result:输出矩阵,hight:输入矩阵的高,width:输入矩阵的宽
void Multi_negation(float * right, float * result, int hight, int width);

//三,矩阵相乘:left:输入左矩阵,right:输入右矩阵,result:输出矩阵
//hight1:左矩阵的高,width1:左矩阵的宽,hight2:右矩阵的高,width2:右矩阵的宽
void Multi(float * left, float * right, float * result, int hight1, int width1, int hight2, int width2);

//四,矩阵相加:left:输入左矩阵,right:输出右矩阵,result:输出矩阵
//hight:矩阵的高,width:矩阵的宽
void Matrix_addition(float * left, float * right, float * result, int hight, int width);

//五,常数乘矩阵:number:常数,right:输入矩阵,result:输出矩阵
//hight:矩阵的高,width:矩阵的宽
void Multi_constant(float number, float * right, float * result, int hight, int width);

//六,矩阵求逆(待优化)
//arcs:输入矩阵,n:矩阵的阶数,ans:逆矩阵
void Matrix_inversion(float arcs[N][N], int n, float ans[N][N]);

//七,打印矩阵内容
//right:输入矩阵,hight:矩阵高,width:矩阵宽
void Matrix_print(float * right,int hight, int width);
/**************************************************************/
/********************** Matrix_functions.cpp *****************/
/*
/*                      矩阵运算函数的实现                   */
/***********************************************************/
#include "Matrix_functions.h"


//一,矩阵转置算法,number:常数,right:乘矩阵,result:结果矩阵,hight:矩阵高,width:矩阵宽
void Multi_transpose(float * right, float * result, int hight, int width) {

	for (int i = 0; i < hight; i++) {//i表示第i行
		for (int j = 0; j < width; j++) {//j表示第j列
			result[j*hight + i] = right[i*width + j];//在这里 result[i][j] = result[i*f2+j];
		}
	}
}

//二,矩阵求反算法,number:常数,right:乘矩阵,result:结果矩阵,hight:矩阵高,width:矩阵宽
void Multi_negation(float * right, float * result, int hight, int width) {

	for (int i = 0; i < hight; i++) {//i表示第i行
		for (int j = 0; j < width; j++) {//j表示第j列
			result[i*width + j] = -right[i*width + j];//在这里 result[i][j] = result[i*f2+j];
		}
	}
}

//三,矩阵相乘算法,最后四个参数是两个相乘的矩阵的行数和列数
void Multi(float * left, float * right, float * result, int hight1, int width1, int hight2, int width2) {
	for (int i = 0; i < hight1; i++) {//i表示第i行
		for (int j = 0; j < width2; j++) {//j表示第j列
			result[i*width2 + j] = 0;//在这里 result[i][j] = result[i*f2+j];
			for (int p = 0; p < width1; p++) {
		//		printf("%d:%.3lf*%.3lf=%.3lf\n", p, left[i*width1 + p], right[p*width2 + j], left[i*width1 + p] * right[p*width2 + j]);
				result[i*width2 + j] += left[i*width1 + p] * right[p*width2 + j];
			}
		//	printf("%d行,%d列 = %.3lf\n", i, j, result[i*width2 + j]);
		}
	//	printf("\n");
	}
	//for (int i = 0; i<hight1; i++)
	//	for (int j = 0; j<width2; j++)
	//		printf("%d行,%d列 = %.3lf\n", i, j, result[i*width2 + j]);
}


//四,矩阵相加,left:输入矩阵,right:输入矩阵,result:结果矩阵,hight:矩阵高,width:矩阵宽
void Matrix_addition(float * left, float * right, float * result, int hight, int width)
{

	for (int i = 0; i < hight; i++) {//i表示第i行
		for (int j = 0; j < width; j++) {//j表示第j列
			result[i*width + j] = left[i*width + j] + right[i*width + j];//在这里 result[i][j] = result[i*f2+j];
		}
	}
}

//五,常数矩阵相乘算法,number:常数,right:乘矩阵,result:结果矩阵,hight:矩阵高,width:矩阵宽
void Multi_constant(float number, float * right, float * result, int hight, int width) {

	for (int i = 0; i < hight; i++) {//i表示第i行
		for (int j = 0; j < width; j++) {//j表示第j列
			result[i*width + j] = number*right[i*width + j];//在这里 result[i][j] = result[i*f2+j];
		}
	}
}


//六,矩阵求逆算法
//求行列式
int getA(float arcs[N][N], int n)//按第一行展开计算|A|
{
	if (n == 1)
	{
		return arcs[0][0];
	}
	int ans = 0;
	float temp[N][N];
	int i, j, k;
	for (i = 0; i<n; i++)
	{
		for (j = 0; j<n - 1; j++)
		{
			for (k = 0; k<n - 1; k++)
			{
				temp[j][k] = arcs[j + 1][(k >= i) ? k + 1 : k];

			}
		}
		int t = getA(temp, n - 1);
		if (i % 2 == 0)
		{
			ans += arcs[0][i] * t;
		}
		else
		{
			ans -= arcs[0][i] * t;
		}
	}
	return ans;
}
//求代数余子式
void getAStart(float arcs[N][N], int n, float ans[N][N])//计算每一行每一列的每个元素所对应的余子式,组成A*
{
	if (n == 1)
	{
		ans[0][0] = 1;
		return;
	}
	int i, j, k, t;
	float temp[N][N];
	for (i = 0; i<n; i++)
	{
		for (j = 0; j<n; j++)
		{
			for (k = 0; k<n - 1; k++)
			{
				for (t = 0; t<n - 1; t++)
				{
					temp[k][t] = arcs[k >= i ? k + 1 : k][t >= j ? t + 1 : t];
				}
			}


			ans[i][j] = getA(temp, n - 1);
			if ((i + j) % 2 == 1)
			{
				ans[i][j] = -ans[i][j];
			}
		}
	}
}
//矩阵求逆,行列式与代数余子式
void Matrix_inversion(float arcs[N][N], int n, float ans[N][N]) {
	int a = getA(arcs, n);
	if (a == 0)
	{
		printf("can not transform!\n");
	}
	else
	{
		float astar[N][N];
		getAStart(arcs, n, astar);
		for (int i = 0; i<n; i++)
		{
			for (int j = 0; j<n; j++)
			{
				ans[i][j] = (double)astar[i][j] / a;
				//printf("%.3lf ", (double)astar[i][j] / a);
				//printf("ans[i][j]=%.3lf\n", ans[i][j]);
			}
			//printf("\n");
		}
	}

}

//七,打印矩阵内容
//right:输入矩阵,hight:矩阵高,width:矩阵宽
void Matrix_print(float * right, int hight, int width) {
	for (int i = 0; i < hight; i++) {//i表示第i行
		for (int j = 0; j < width; j++) {//j表示第j列
			printf("result[%d][%d]=%f  ",i,j,right[i*width + j]);//在这里 result[i][j] = result[i*f2+j];
		}
		printf("\n");
	}
}

三,利用实现的矩阵函数,实现LM最小二乘法

#include "Matrix_functions.h"

double X[5] = {-2,-1,0,1,2};
double Y[5] = {-22,-5,0,5,22};
float a = 10;
float b = 2;

//拟合函数
float f_function(float x, float y) {
    //return y - (a*cos(b*x) + b*sin(a*x));
//    printf("%f - (%f*%f*%f)+(%f*%f)= %f \n",y,a,x,x,b,x, (y - (a*x*x) + (b*x)));
    return (y - (a*x*x*x) -(b*x));
    
}

//对a参数的偏导
float fa_function(float x, float y) {
//    return -cos(b*x) + a*b*cos(a*x);
//    return -x*x;
    return (-(x*x*x));
}

//对b参数的偏导
float fb_function(float x, float y) {
    //return a*b*sin(b*x) + sin(a*x);
    return -x;
}

float J[5][2];
float J_t[2][5];
float J_t_[2][5];
float F[5][1];
float I[2][2] = { { 1,0 },{ 0,1 } };
float r = 0.01;
float u = 10;
float uI[2][2];
float Q_J[2][2];
float Q[2][2];

float F_[1][5];
float Mul_d[1][1];

float Q_add[2][2];
float Q_add_[2][2];
float Mul_J_f[2][1];
float K[2][1];

int main() {

    int i = 0;
    int j = 0;
    float x = 0;
    float y = 0;
    //
    for (i = 0; i < 5; i++)
        for (j = 0; j < 2; j++)
            J[i][j] = 0;

    for (i = 0; i < 5; i++)
        for (j = 0; j < 1; j++)
            F[i][j] = 0;

    //初始化JCOB矩阵和求误差矩阵
    for (i = 0; i < 5; i++) {
        x = X[i];
        y = Y[i];
        J[i][0] = fa_function(x, y);
        J[i][1] = fb_function(x, y);
        F[i][0] = f_function(x, y);
    }
    
    
    //右矩阵
    Multi_transpose(J[0], J_t[0], 5, 2);
    Multi_negation(J_t[0], J_t_[0], 2, 5);
    Multi(J_t_[0], F[0], Mul_J_f[0], 2, 5, 5, 1);

    //左矩阵
    Multi_constant(r, I[0], uI[0], 2, 2);    
    Multi(J_t[0], J[0],Q[0],2, 5, 5, 2);
    Matrix_addition(Q[0], uI[0],Q_add[0], 2, 2);
    Matrix_inversion(Q_add,2, Q_add_);
    
    //误差
    Multi_transpose(F[0], F_[0], 5,1);
    Multi(F_[0], F[0], Mul_d[0], 1, 5, 5, 1);

    //计算步长
    Multi(Q_add_[0], Mul_J_f[0], K[0], 2, 2, 2, 1);
    
    int count = 0;
    while (true) {

        if (count == 10)
            break;
        
        float d = Mul_d[0][0];
        count++;
        //if (count % 1000 == 0) {
            printf("count=%d\n", count);
            printf("a=%f,b=%f\n", a, b);
            printf("Mul_d=\n");
            Matrix_print(Mul_d[0], 1, 1);
    //    }

        a = a + K[0][0];
        b = b + K[0][1];
        
        //初始化JCOB矩阵和求误差矩阵
        for (i = 0; i < 5; i++) {
            x = X[i];
            y = Y[i];
            J[i][0] = fa_function(x, y);
            J[i][1] = fb_function(x, y);
            F[i][0] = f_function(x, y);
        }
        //右矩阵
        Multi_transpose(J[0], J_t[0], 5, 2);
        Multi_negation(J_t[0], J_t_[0], 2, 5);
        Multi(J_t_[0], F[0], Mul_J_f[0], 2, 5, 5, 1);

        //左矩阵
        Multi_constant(r, I[0], uI[0], 2, 2);
        Multi(J_t[0], J[0], Q[0], 2, 5, 5, 2);
        Matrix_addition(Q[0], uI[0], Q_add[0], 2, 2);
        Matrix_inversion(Q_add, 2, Q_add_);

        //误差
        Multi_transpose(F[0], F_[0], 5, 1);
        Multi(F_[0], F[0], Mul_d[0], 1, 5, 5, 1);

        

        if (Mul_d[0][0] < d)
        {
            K[0][0] = a;
            K[0][1] = b;
            r = r / u;
            if (Mul_d[0][0] < 0.5)
            {
                printf("the final Mul_d=%f\n",Mul_d[0][0]);
                printf("the final a is %f,b is %f,the function is y=%f*x*x+%f*x\n", a,b,a,b);
                break;
            }
                

        }
        else
        {
            r = u*r;
        }

        //计算步长
        Multi(Q_add_[0], Mul_J_f[0], K[0], 2, 2, 2, 1);
    }

    system("pause");
    return 0;

}

四,拟合结果

可以看到,用LM最小二乘法做拟合,如果是对正确的数据做拟合,拟合速度是非常快的,但是,LM最小二乘法的鲁棒性不是很好,很容易受到误差点的影响,导致拟合不收敛的问题,可以自己调整数据的个数,和对应偏导数,及雅克比矩阵进行拟合,查看拟合结果。 

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章