matlab 实现：
% 计算函数f的雅克比矩阵，是解析式
syms c1 c2 v psi x1 x2 y1 y2 del_t
f = c1+c2+v/psi*del_t+cos(psi)*(x1+x2-c1-c2-v/psi*del_t)+sin(psi)*(x2-x1+c1-c2+v/psi*del_t);
Jsym = jacobian(f,[v psi])
x1 = [ 261.32 149.24 149.24 42.08 485.2 611.44 365.88 485.2 254.76 365.88 381.8 261.32 156.28 254.76];
x2 = [337.44 290.2 290.2 245.52 132.52 173.56 89.48 132.52 57.8 89.48 384.48 337.44 21.92 57.8];
y1 = [ 264.6 155.24 155.24 50.88 498.96 624.76 382. 498.96 271.12 382. 377.8 264.6 174.84 271.12 ];
y2 = [352.96 295.6 295.6 247.04 157.4 207.8 108.84 157.4 67.84 108.84 405.56 352.96 29.48 67.84];
c1 = 320;
c2 = 389.33;
del_t =1;
% 2. LM算法
% 初始猜测s
v0=1; psi0=1;
y_init = c1+c2+v0/psi0*del_t+cos(psi0)*(x1+x2-c1-c2-v0/psi0*del_t)+sin(psi0)*(x2-x1+c1-c2+v0/psi0*del_t);
% 数据个数
Ndata=length(x1);
% 参数维数
Nparams=2;
% 迭代最大次数
n_iters=50;
% LM算法的阻尼系数初值
lamda=0.01;
% step1: 变量赋值
updateJ=1;
v_est=v0;
psi_est=psi0;
% step2: 迭代
for it=1:n_iters
if updateJ==1
% 根据当前估计值，计算雅克比矩阵
J=zeros(Ndata,Nparams);
for i=1:length(x1)
J(i,:)=[del_t/psi_est - (del_t*cos(psi_est))/psi_est + (del_t*sin(psi_est))/psi_est cos(psi_est)*(c1 - c2 - x1(i) + x2(i) + (del_t*v_est)/psi_est) + sin(psi_est)*(c1 + c2 - x1(i) - x2(i) + (del_t*v_est)/psi_est) + sin(psi_est)*(c1 + c2 - x1(i) - x2(i) + (del_t*v_est)/psi_est) - (del_t*v_est)/psi_est^2 + (del_t*v_est*cos(psi_est))/psi_est^2 - (del_t*v_est*sin(psi_est))/psi_est^2];
end
disp(J)
% 根据当前参数，得到函数值
y_est = c1+c2+v_est/psi_est*del_t+cos(psi_est)*(x1+x2-c1-c2-v_est/psi_est*del_t)+sin(psi_est)*(x2-x1+c1-c2+v_est/psi_est*del_t);
% 计算误差
d=y1+y2-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(:);
v_lm=v_est+dp(1);
psi_lm=psi_est+dp(2);
% 计算新的可能估计值对应的y和计算残差e
y_est_lm = c1+c2+v_lm/psi_lm*del_t+cos(psi_lm)*(x1+x2-c1-c2-v_lm/psi_lm*del_t)+sin(psi_lm)*(x2-x1+c1-c2+v_lm/psi_lm*del_t);
d_lm=y1+y2-y_est_lm;
e_lm=dot(d_lm,d_lm);
% 根据误差，决定如何更新参数和阻尼系数
if e_lm<e
lamda=lamda/10;
v_est=v_lm;
psi_est=psi_lm;
e=e_lm;
% disp(e);
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
%显示优化的结果
v_est
psi_est
C++ 实现：
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
Eigen::VectorXf computeRelatedValues2(int N, float c1, float c2, float del_t, Eigen::VectorXf x1, Eigen::VectorXf x2, Eigen::VectorXf y1, Eigen::VectorXf y2, float v,float psi)
{
//c1+c2+v0/psi0*del_t+cos(psi0)*(x1+x2-c1-c2-v0/psi0*del_t)+sin(psi0)*(x2-x1+c1-c2+v0/psi0*del_t);
Eigen::VectorXf value(N);
value = Eigen::VectorXf::Ones(N)*(c1 + c2 + v / psi*del_t) +
cos(psi)*(x1 + x2 - Eigen::VectorXf::Ones(N)*(c1 + c2 + v / psi*del_t))
+ sin(psi)*(x2 - x1 + Eigen::VectorXf::Ones(N)*(c1 - c2 + v / psi*del_t));
return value;
}
void LM()
{
float c1 = 320;
float c2 = 389.33;
float del_t = 1;
//数据个数
int N = 14;
// 参数维数
int Nparams = 2;
Eigen::VectorXf x1(N);
x1 << 261.32, 149.24, 149.24, 42.08, 485.2, 611.44, 365.88, 485.2, 254.76, 365.88, 381.8, 261.32, 156.28, 254.76;
Eigen::VectorXf x2(N);
x2 << 337.44, 290.2, 290.2, 245.52, 132.52, 173.56, 89.48, 132.52, 57.8, 89.48, 384.48, 337.44, 21.92, 57.8;
Eigen::VectorXf y1(N);
y1 << 264.6, 155.24, 155.24, 50.88, 498.96, 624.76, 382., 498.96, 271.12, 382., 377.8, 264.6, 174.84, 271.12;
Eigen::VectorXf y2(N);
y2 << 352.96, 295.6, 295.6, 247.04, 157.4, 207.8, 108.84, 157.4, 67.84, 108.84, 405.56, 352.96, 29.48, 67.84;
//迭代最大次数
int max_iter = 20;
float func_tol = pow(10.0, -5);
int iter = 0;
float fval = INT_MAX;
//阻尼系数初值
float lamda = 0.01;
//初始猜测
float v0 = 1;
float psi0 = 1;
Eigen::VectorXf y_init = computeRelatedValues2(N, c1, c2, del_t, x1, x2, y1, y2, v0, psi0);
//cout <<"y_init: "<< y_init.transpose()<< endl;
float v_est = v0;
float psi_est = psi0;
//雅克比矩阵
Eigen::MatrixXf J = Eigen::MatrixXf::Zero(N, Nparams);
Eigen::VectorXf y_est = Eigen::VectorXf(N);
Eigen::VectorXf d = Eigen::VectorXf(N);
Eigen::MatrixXf H = Eigen::VectorXf(Nparams, Nparams);
Eigen::VectorXf dp = Eigen::VectorXf(N);
Eigen::VectorXf g = Eigen::VectorXf(N);
float v_lm = 0;
float psi_lm = 0;
float e = 0;
Eigen::VectorXf y_est_lm = Eigen::VectorXf(N);
Eigen::VectorXf d_lm = Eigen::VectorXf(N);
Eigen::MatrixXf H_lm = Eigen::MatrixXf(Nparams, Nparams);
float e_lm = 0;
int updateJ = 1;
while (iter < max_iter&&updateJ>0)
{
++iter;
//if (updateJ==1)
//{
for (int i = 0; i<N; ++i)
{
J.row(i) << del_t / psi_est - (del_t*cos(psi_est)) / psi_est + (del_t*sin(psi_est)) / psi_est,
cos(psi_est)*(c1 - c2 - x1(i) + x2(i) + (del_t*v_est) / psi_est) + sin(psi_est)*(c1 + c2 - x1(i) - x2(i) +
(del_t*v_est) / psi_est) + sin(psi_est)*(c1 + c2 - x1(i) - x2(i) + (del_t*v_est) / psi_est) -
(del_t*v_est) / (psi_est*psi_est) + (del_t*v_est*cos(psi_est)) / (psi_est *psi_est) - (del_t*v_est*sin(psi_est)) / (psi_est *psi_est);
//cout << J.row(i) << endl;
}
//cout << endl;
//根据当前参数，得到函数值
y_est = computeRelatedValues2(N, c1, c2, del_t, x1, x2, y1, y2, v_est, psi_est);
//cout << "y_est: " << y_est.transpose() << endl;
//计算误差
d = y1 + y2 - y_est;
//计算（拟）海塞矩阵
H = J.transpose()*J;
// 若是第一次迭代，计算误差
if (iter == 1)
{
e = d.transpose() * d;
}
//}
//根据阻尼系数lamda混合得到H矩阵
//H_lm=H+(lamda*eye(Nparams,Nparams));
H_lm = H + (lamda*Eigen::MatrixXf::Identity(Nparams, Nparams));
//计算步长dp，并根据步长计算新的可能的\参数估计值
dp = H_lm.inverse()*(J.transpose()*d);
g = J.transpose()*d;
v_lm = v_est + dp(0,0);
psi_lm = psi_est + dp(1,0);
//计算新的可能估计值对应的y和计算残差e
y_est_lm = computeRelatedValues2(N, c1, c2, del_t, x1, x2, y1, y2, v_lm, psi_lm);
d_lm = y1 + y2 - y_est_lm;
e_lm = d_lm.transpose() * d_lm;
//根据误差，决定如何更新参数和阻尼系数
if (e_lm < e)
{
lamda = lamda / 10;
v_est = v_lm;
psi_est = psi_lm;
e = e_lm;
//cout << e << endl;
updateJ = 1;
}
else
{
lamda = lamda * 10;
updateJ = 0;
}
}
cout << iter<<endl;
cout << v_est << " " << psi_est << endl;
}
转载：https://blog.csdn.net/shanglianlm/article/details/85342015
查看评论