根据已学习内容，解决现在遇到的问题
1.这是一个凸优化问题吗：
https://blog.csdn.net/xmu_jupiter/article/details/47400411
经过一轮推导，对于变量为传感器位置，x,y,z。并不是凸函数
2.需要多少个等式对应多少个未知数
至少达到变量数，越多越好
3.如果使用LM方法，怎么加入约束条件
涉及到的约束：
手臂长度约束，不等式，20=<l1,l2<=30
传感器位置约束，不等式，-4<=x,y,z<4
面朝向约束，不等式，0<=theta<=6.28
https://www.cnblogs.com/punkcure/p/7825735.html
介绍了拉格朗日对偶问题，将不等式约束加入目标函数，生成一个性质类似的新目标函数。但是这种解决方法，和前面学的牛顿法等迭代求解方法好像并不能结合。
新的目标函数，其导数在可行解范围内还是一样的，一旦迭代过程里跳出了可行域，那就不知道怎么办了。跳出可行域，身边都是无穷大，导数
https://blog.csdn.net/qq_41679006/article/details/81198411
外罚点函数法：把不等式或者等式的平方项加入目标函数。
内罚点函数法：把基于不等式的分式加入目标函数，构成搜索过程里的壁垒，即搜索不会进入不可行域。
加入惩罚函数对原先搜索过程的影响：网上资料就是没有展示一点，对这个新的目标函数进行无约束优化求解的过程。
因为在可行域内，算法相当于不变。例如外点法，当我们使用梯度下降也就是导数来进行迭代的时候，
噢，我分别用外点法和内点法做了一组数据模拟，用的梯度下降，虽然我还不太清楚收敛条件，但是算法确实是收敛路上的，而且内点法的壁垒作用很明显。
也就是，增加的罚函数的导数，确实影响了搜索过程，
在搜索方向上，步长的话，一维步长搜索，确定了搜索方向D后，以步长为变量进行一次搜索。
https://blog.csdn.net/ice110956/article/details/17631765
这里介绍了一个罚函数法的迭代版，惩罚因子也可以根据每次达到的精度进行迭代，感觉意义不大
目标函数：
/////////$f(x,y,z) = sum(sqrt{(x-a)^2 + (y-b)^2 + (z-c)^2} - D)^2$
目标函数：
$r_i(x,y,z) = sqrt{(x-a)^2 + (y-b)^2 + (z-c)^2} - D$
$f(x,y,z) = sum r_i^2 $
$s.t. -4=< x <= 4, -4=<y<=4, -4<=z<=4$
加入内点法后，罚函数：
$P(x,y,z) = (frac{1}{x-4})^2 + (frac{1}{x+4})^2 + (frac{1}{y-4})^2 + (frac{1}{y+4})^2 + (frac{1}{z-4})^2 + (frac{1}{z+4})^2$
$F(x,y,z) = f(x,y,z) + P(x,y,z)$
初始预测点：x0 = 1, y0 =1, z0 = 1.
LM算法公式：
$a = [x,y,z]^T$
g = J^T [ r_1, r_2,..... frac{1}{x-4},....frac{1}{z+4}]
$x = x_0 - (J^TJ + mu I)^{-1}g(x,z,y)$
$J_{ij} = frac{partial F_i}{partial a_j}$
$mu$的初值选取：和$J^TJ$矩阵元素个数有关？？$mu_0 = au * max(J^TJ_{ii}^0)$ $mu$等于$ au$乘以$J^TJ$也就是海森阵近似阵，该矩阵主对角线元素中最大的一个。。。$ au$的取值还没资料，网上代码有给e-10的。
$mu$的控制公式，$ ho = frac{F(x) - F(x+h)}{L(0) - L(h)}$ $ L(0) - L(h) = -h^TJ^Tf - frac{1}{2}h^TJ^TJh$
$if ho > 0$ $mu = mu * max{frac{1}{3},1 - (2 ho - 1)^3}$ $else$ $mu = mu * 2$
收敛条件：
对于LM法：
1.g过小
2.x的变化过小
3.达到最大迭代次数
milestone!!!!
理论部分就这样了，下面matlab的函数测一遍，，自己写一个再测一遍