因爲實習工作需要使用到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最小二乘法的魯棒性不是很好，很容易受到誤差點的影響，導致擬合不收斂的問題，可以自己調整數據的個數，和對應偏導數，及雅克比矩陣進行擬合，查看擬合結果。