LM在最优化中占据着极其重要的地位，将非线性问题转为为线性问题来求解，求出最优解。
最近在搞相机标定，而不得不面对LM，首先的想法百度，（个人不喜欢去看e文，总觉得效率不高，事实上错了）找到了一些相关的文章，好难啊，这是第一想法。不过还是找到了几篇大牛的文章，以及相关的MATLAB实现代码，本文的c实现便是基于MATLAB代码。感谢原作者
http://www.codelast.com/?p=29，这是大牛对之及其通俗的解释，相信看了能明白个大概。
http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
讲解的非常透彻的一篇文档，以及实现。E文的，应该看懂都没问题。
% 计算函数f的雅克比矩阵，是解析式
syms a b y x real;
f=a*exp(-b*x);
Jsym=jacobian(f,[a b])
% 拟合用数据。参见《数学试验》，p190，例2
data_1=[0.25 0.5 1 1.5 2 3 4 6 8];
obs_1=[19.21 18.15 15.36 14.10 12.89 9.32 7.45 5.24 3.01];
% 2. LM算法
% 初始猜测s
a0=10; b0=0.5;
y_init = a0*exp(-b0*data_1);
% 数据个数
Ndata=length(obs_1);
% 参数维数
Nparams=2;
% 迭代最大次数
n_iters=50;
% LM算法的阻尼系数初值
lamda=0.01;
% step1: 变量赋值
updateJ=1;
a_est=a0;
b_est=b0;
% step2: 迭代
for it=1:n_iters
if updateJ==1
% 根据当前估计值，计算雅克比矩阵
J=zeros(Ndata,Nparams);
for i=1:length(data_1)
J(i,:)=[exp(-b_est*data_1(i)) -a_est*data_1(i)*exp(-b_est*data_1(i))];
end
% 根据当前参数，得到函数值
y_est = a_est*exp(-b_est*data_1);
% 计算误差
d=obs_1-y_est;
% 计算（拟）海塞矩阵
H=J'*J;
% 若是第一次迭代，计算误差
if it==1
e=dot(d,d);
end
end
% 根据阻尼系数lamda混合得到H矩阵
H_lm=H+(lamda*eye(Nparams,Nparams));
% 计算步长dp，并根据步长计算新的可能的\参数估计值
dp=inv(H_lm)*(J'*d(:));
g = J'*d(:);
a_lm=a_est+dp(1);
b_lm=b_est+dp(2);
% 计算新的可能估计值对应的y和计算残差e
y_est_lm = a_lm*exp(-b_lm*data_1);
d_lm=obs_1-y_est_lm;
e_lm=dot(d_lm,d_lm);
% 根据误差，决定如何更新参数和阻尼系数
if e_lm<e
lamda=lamda/10;
a_est=a_lm;
b_est=b_lm;
e=e_lm;
disp(e);
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
%显示优化的结果
a_est
b_est
以上是MATLAB实现，据说原作者是http://www.shenlejun.cn/my/article/show.asp?id=17&page=1但是好像打不开。
于是我自己想用c语言来实现，写了很久发现还是有点写不出来啊，看来代码实现能力还是不行。最后不得已写了一个以上MATLAB的c语言版本。
非常感谢那些把经验和知识分享出来的牛人。
#include <stdio.h>
#include <math.h>
#include <string.h>
/*
函数表达式为 f(x)=a*exp(-b*x),a,b为待估参数
*/
double data_1[9]={0.25 ,0.5, 1 ,1.5 ,2, 3, 4, 6 ,8};
double obs_1[9]={19.21 ,18.15 ,15.36 ,14.10 ,12.89, 9.32 ,7.45 ,5.24 ,3.01};
/*
void cal_jaco(double **J, double a_bst, double b_bst)
{
int i,j;
j=0;
for(i=0;i<9;i++)
{
J[i][j] = exp(-b_bst*data_1[i]);
J[i][j+1] = -a_bst*data_1[i]*exp(-b_bst*data_1[i]);
}
}
*/
//计算函数的值，目前只计算已知函数，即无法对输入的函数求解
void cal_fun_val(double *val,double a_bst, double b_bst)
{
int i;
for(i=0;i<9;i++)
{
val[i]=a_bst*exp(-b_bst*data_1[i]);
}
}
//一位向量的减法
void matrix_sub(double *result, double *first_m, double *second_m)
{
int i;
for(i=8;i>=0;i--)
{
result[i]=first_m[i]-second_m[i];
}
}
//一维向量的加法
void matrix_add(double *result, const double *first_m, const double *second_m)
{
int i;
for (i = 8; i >= 0; i--)
{
result[i] = first_m[i] + second_m[i];
}
}
//二维向量的转置
void tran_matrix(double **tran_mat, double **matrix, int row,int col)
{
int i,j;
for (i = 0; i < row; i++)
{
for (j = 0; j < col; j++)
{
*((double *)tran_mat + row*j +i) = *((double *)matrix + col*i + j);
}
}
}
//打印出n*m的矩阵
void print_matrix(double **matrix, int row, int col)
{
int i,j;
for (i = 0; i < row; i++)
{
for (j = 0; j < col; j++)
{
printf("%f ", *((double *)matrix + col*i + j));
}
printf("\n");
}
}
//二维矩阵的乘法
/*
outa=inb*inc
*/
void matrix_mut(double ** outa, double** inb, int b_row, int b_col, double** inc, int c_col)
{
int i, j, k,num;
double tempdata=0;
num = 0;
double *tempa = (double *)outa;
double t1, t2;
for (i = 0; i < b_row; i++)
{
for (k = 0; k < c_col; k++)
{
for (j = 0; j < b_col; j++)
{
t1 = (*((double *)inb + b_col*i + j));
t2 = (*((double *)inc + c_col*j + k));
tempdata += t1*t2;
}
tempa[num]=tempdata;
num++;
tempdata = 0;
}
}
}
//矩阵与常数相乘
void cost_mat_mul(double **res_mat, int ratio, double **mat, int row, int col)
{
int i, j;
for (i = 0; i < row; i++)
{
for (j = 0; j < col; j++)
{
(*((double *)res_mat + col*i + j)) = ratio * (*((double *)mat + col*i + j));
}
}
}
//向量点积
double dot(double * mat1, double * mat2,int nofelem)
{
double *temp_mat1 = mat1;
double *temp_mat2 = mat2;
double result = 0;
int i;
for (i = 0; i<nofelem;i++)
result += (*temp_mat1++)*(*temp_mat2++);
return result;
}
#define MAX 2
#define E 0.000000001
/**
* 计算矩阵行列式src的模值
*/
double calculate_A(double src[][MAX], int n)
{
int i, j, k, x, y;
double tmp[MAX][MAX], t;
double result = 0.0;
if (n == 1)
{
return src[0][0];
}
for (i = 0; i < n; ++i)
{
for (j = 0; j < n - 1; ++j)
{
for (k = 0; k < n - 1; ++k)
{
x = j + 1;
y = k >= i ? k + 1 : k;
tmp[j][k] = src[x][y];
}
}
t = calculate_A(tmp, n - 1);
if (i % 2 == 0)
{
result += src[0][i] * t;
}
else
{
result -= src[0][i] * t;
}
}
return result;
}
/**
* 计算伴随矩阵
*/
void calculate_A_adjoint(double src[MAX][MAX], double dst[MAX][MAX], int n)
{
int i, j, k, t, x, y;
double tmp[MAX][MAX];
if (n == 1)
{
dst[0][0] = 1;
return;
}
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)
{
x = k >= i ? k + 1 : k;
y = t >= j ? t + 1 : t;
tmp[k][t] = src[x][y];
}
}
dst[j][i] = calculate_A(tmp, n - 1);
if ((i + j) % 2 == 1)
{
dst[j][i] = -1 * dst[j][i];
}
}
}
}
/**
* 得到逆矩阵
*/
int inv(const double src[MAX][MAX], double dst[MAX][MAX], int n)
{
double A = calculate_A(src, n);
double tmp[MAX][MAX];
int i, j;
if (fabs(A - 0) <= E)
{
// printf("不可能有逆矩阵！\n");
return 0;
}
calculate_A_adjoint(src, tmp, n);
for (i = 0; i < n; ++i)
{
for (j = 0; j < n; ++j)
{
dst[i][j] = (double)(tmp[i][j] / A);
}
}
return 1;
}
//LM算法 核心代码
int LM()
{
// 迭代最大次数
int n_iters=400;
// LM算法的阻尼系数初值
double lamda=0.01;
//初始值
double a0=20;
double b0=0.5;
// step1: 变量赋值
int updateJ=1;
double a_est=a0;
double b_est=b0;
int it=0;
double J[9][2];
double Jt[2][9];
double y_est[9];
double d[9];
double H[2][2] = {0};
double H_1m[2][2];
double e;
double tempeye[2][2] = { { 1, 0 }, { 0, 1 } };
double eye[2][2] = { { 1, 0 }, { 0, 1 } };
double g[2] = {0};
int i, j;
double invH_1m[2][2] = {0};
double dp[2] = {0};
double a_1m, b_1m;
double y_est_lm[9];
double d_1m[9], e_1m;
double temp = 0;
for ( it =0;it<n_iters; it++)
{
if(updateJ == 1)
{
// cal_jaco(J,a_est,b_est);
for (i = 0; i<9; i++)
{//计算雅克比矩阵
J[i][0] = exp(-b_est*data_1[i]);
J[i][1] = -a_est*data_1[i] * exp(-b_est*data_1[i]);
}
cal_fun_val(y_est, a_est, b_est);
matrix_sub(d, obs_1, y_est);//计算当前的误差
tran_matrix(Jt, J, 9, 2);
matrix_mut(H, Jt, 2, 9, J, 2);//得到Hessian 矩阵
if (it == 0)
{
e = dot(d, d, 9);//第一次时计算误差作为误差初值
}
}
cost_mat_mul(tempeye, lamda, eye, 2, 2);
/* for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
tempeye[i][j] = eye[i][j] * lamda;
}
}*/
// matrix_add(H_1m, H, tempeye);
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
H_1m[i][j] = H[i][j] + tempeye[i][j];
}
}
for (i = 0; i < 2; i++)
{
for (j = 0; j < 9; j++)
{
temp += Jt[i][j] * d[j];
}
g[i] = temp;
}
inv(H_1m, invH_1m, 2);
temp = 0;
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
temp += invH_1m[i][j] * g[j];
}
dp[i] = temp;//求解步长
}
a_1m = a_est + dp[0];
b_1m = b_est + dp[1];
for (i = 0; i < 9; i++)
{
y_est_lm[i] = a_1m*exp(-b_1m*data_1[i]);
d_1m[i] = obs_1[i] - y_est_lm[i];
}
e_1m = dot(d_1m, d_1m, 9);//计算新的 a,b时的误差
if (e_1m < e)//如果误差下降了，这是我们想要的
{
lamda = lamda / 10;
a_est = a_1m;
b_est = b_1m;
e = e_1m;
printf("%f \n", e);
updateJ = 1;
}
else
{
updateJ = 0;
lamda = lamda * 10;
}
}
printf("%f,%f ", a_est, b_est);
return 1;
}
int main()
{
LM();
return 0;
}
版权声明：本文为wejoncy原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。