因为实习工作需要使用到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最小二乘法的鲁棒性不是很好,很容易受到误差点的影响,导致拟合不收敛的问题,可以自己调整数据的个数,和对应偏导数,及雅克比矩阵进行拟合,查看拟合结果。