三个固定点坐标已知A(X1,Y1),B(X2,Y2),C(X3,Y3)。未知点(X,Y)。未知点到A/B距离差为det_ab,到B/C距离差为det_bc,到A/C距离差为det_ac;
求解未知点坐标?
想写成一个函数,输入参数是A/B/C坐标和三个距离差。输出为未知点的坐标。
求写出matlab代码~
另外,假如我这三个距离差有一定的误差,那么求解的时候怎样得到最优解呢?
由于方程个数3>未知数个数2,且涉及到距离的计算,所以原问题是求解一个超定(非线性)方程组.平方和形式这里采用最小二乘法解决(n个定点与未知点的计算,n>=2):
先建立pfun.m文件:
%给出待求点p(x,y)的初值p0
%n个定点的坐标x0,y0
%中间相邻两定点到p的距离差(最后一个点与起点求差)
function [p,norm,res,exit,out]=pfun(p0,x0,y0,det)
%判断det输入的正确性
n=length(det);
for i=1:n-1
if abs(det(i))>sqrt((x0(i)-x0(i+1))^2+(y0(i)-y0(i+1))^2);
error('abs(det) is greater than the disdance of two point')
end
end
if abs(det(n))>sqrt((x0(n)-x0(1))^2+(y0(n)-y0(1))^2)
error('abs(det) is greater than the disdance of two point')
end
%下降算法采用Levenberg-Marquardt法
%函数调用最大次数为1000
%由用户定义目标函数的雅可比矩阵
opt1=optimset('LargeScale','off','MaxFunEvals',1000,'Jacobian','on');
%用非线性最小二乘命令求p(x,y),x=p(1),y=p(2)
[p,norm,res,exit,out]=lsqnonlin(@dfun,p0,[],[],opt1,x0,y0,det);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%建立含雅可比矩阵的pfun.m文件
function [f,g]=dfun(p,x0,y0,det)
n=length(det);
for i=1:n-1
f(i)=sqrt((p(1)-x0(i))^2+(p(2)-y0(i))^2)-...
sqrt((p(1)-x0(i+1))^2+(p(2)-y0(i+1))^2)-det(i);
end
f(n)=sqrt((p(1)-x0(n))^2+(p(2)-y0(n))^2)-...
sqrt((p(1)-x0(1))^2+(p(2)-y0(1))^2)-det(n);
%当函数用两个输出参数调用时
if nargout>1
%计算雅可比矩阵
for i=1:n-1
g(i,1)=(p(1)-x0(i))/sqrt((p(1)-x0(i))^2+(p(2)-y0(i))^2)-...
(p(1)-x0(i+1))/sqrt((p(1)-x0(i+1))^2+(p(2)-y0(i+1))^2);
g(i,2)=(p(2)-y0(i))/sqrt((p(1)-x0(i))^2+(p(2)-y0(i))^2)-...
(p(2)-y0(i+1))/sqrt((p(1)-x0(i+1))^2+(p(2)-y0(i+1))^2);
end
g(n,1)=(p(1)-x0(n))/sqrt((p(1)-x0(n))^2+(p(2)-y0(n))^2)-...
(p(1)-x0(1))/sqrt((p(1)-x0(1))^2+(p(2)-y0(1))^2);
g(n,2)=(p(2)-y0(n))/sqrt((p(1)-x0(n))^2+(p(2)-y0(n))^2)-...
(p(2)-y0(1))/sqrt((p(1)-x0(1))^2+(p(2)-y0(1))^2);
end
例如各点关系如下:
输入:
x0=[0.5,0,0];
y0=[2+sqrt(3)/2,2,0];
det=[sqrt(3)-2,2-2*sqrt(2),2*sqrt(2)-sqrt(3)];
p0=[1,1];
[p,norm,res,exit,out]=pfun(p0,x0,y0,det)
输出:
p =
2.0000 2.0000