Matlab最小二乘法实现轨迹定位方法（原创：呵呵哒！(￣▽￣)"）

轨迹定位方法——Matlab 仿真方法

已知平面四点坐标(x1,y1)=(0,0), (x2,y2) =(1,0), (x3,y3) =(0,1)， (x4,y4) =(0,2)，某人沿一条直线行走，行走过程中经过待定位点的坐标分别为 (a1,b1),…… (a5,b5) ，待定位各个坐标点到已知四点距离分别测得为（ 0.71, 0.78, 0.64 ，1.52 ），（ 1.4, 0.95, 1.05 ，1.5 ），（ 2.12, 1.61, 1.55 ，1.52），（ 2.87, 2.24, 2.31 ，2.1 ），（ 3.61, 3, 2.97 ，2.57 ）求此人行走的直线方程。

定位方法——多点定位

解题思路：

（1）线性回归方程

（2）最小二乘法

程序实现：

Location.m

function kk=Location(xx,yy,ll)

xxx = xx(1:end-1);

yyy = yy(1:end-1);

lll = ll(1:end-1);



A = [2*(xxx-xx(end)) 2*(yyy-yy(end))];

b = (xxx.^2-xx(end)^2) + (yyy.^2-yy(end)^2) - (lll.^2-ll(end)^2);

kk=(A.'*A)^(-1)*A.'*b;

drawline.m（线性回归方程matlab实现画直线）

function drawline(x,y)

n=length(x);

xb=mean(x);

yb=mean(y);

x2b=sum(x.^2)/n;

xyb=x*y'/n;

b=(xb*yb-xyb)/(xb^2-x2b);

a=yb-b*xb;

y1=a+b.*x;

plot(x,y,'*',x,y1);

serror=sum((y-y1).^2)

第一种常规程序：（缺点：每次求一个定位点都得调用Location()函数，代码很多冗余）

heheda_matlab_changui.m

tic

xx=[0 1 0 0].';

yy=[0 0 1 2].';

l1=[0.71 0.78 0.64 1.52].';

l2=[1.4 0.95 1.05 1.5].';

l3=[2.12 1.61 1.55 1.52].';

l4=[2.87 2.24 2.31 2.1].';

l5=[3.61 3 2.97 2.57].';

k1=Location(xx,yy,l1)

k2=Location(xx,yy,l2)

k3=Location(xx,yy,l3)

k4=Location(xx,yy,l4)

k5=Location(xx,yy,l5)



X=[k1(1) 1

    k2(1) 1

    k3(1) 1

    k4(1) 1

    k5(1) 1]

Y=[k1(2)

    k2(2)

    k3(2)

    k4(2)

    k5(2)]

X1=[k1(1) k2(1) k3(1) k4(1) k5(1)]

Y1=[k1(2) k2(2) k3(2) k4(2) k5(2)]



drawline(X1,Y1)

toc

第二种：简化程序

heheda_chengxu.m

tic

xx=[0 1 0 0].';

yy=[0 0 1 2].';

l1=[0.71 0.78 0.64 1.52].';

l2=[1.4 0.95 1.05 1.5].';

l3=[2.12 1.61 1.55 1.52].';

l4=[2.87 2.24 2.31 2.1].';

l5=[3.61 3 2.97 2.57].';

sum=[l1 l2 l3 l4 l5];

kk=[]

i=1

for i=1:5

    kk(:,i)=Location(xx,yy,sum(:,i));

end;

k_x=kk(1,:)

k_y=kk(2,:)



X=[]

Y=[]

for i=1:5

    X(:,i)=[k_x(i),1]

    Y(:,i)=k_y(i)

end

X=X.'

Y=Y.'

a = -5:0.1:2;

b = 0:0.1:10;

drawline(k_x,k_y)

toc

第三种：实现代码优化

code_fast.m

tic

xx=[0 1 0 0].';

yy=[0 0 1 2].';

l1=[0.71 0.78 0.64 1.52].';

l2=[1.4 0.95 1.05 1.5].';

l3=[2.12 1.61 1.55 1.52].';

l4=[2.87 2.24 2.31 2.1].';

l5=[3.61 3 2.97 2.57].';

sum=[l1 l2 l3 l4 l5];

kk=[]

i=1

for i=1:5

    kk(:,i)=Location(xx,yy,sum(:,i));

end;

k_x=kk(1,:)

k_y=kk(2,:)

X=[]

Y=[]

for i=1:5

    X(:,i)=[k_x(i),1]

    Y(:,i)=k_y(i)

end

X=X.'

Y=Y.'

a = -5:0.1:2;

b = 0:0.1:10;

drawline(k_x,k_y)

toc

第四种：实现代码更简洁

true_line.m

tic

xx=[0 1 0 0].';

yy=[0 0 1 2].';

l1=[0.71 0.78 0.64 1.52].';

l2=[1.4 0.95 1.05 1.5].';

l3=[2.12 1.61 1.55 1.52].';

l4=[2.87 2.24 2.31 2.1].';

l5=[3.61 3 2.97 2.57].';

sum=[l1 l2 l3 l4 l5];

kk=[]

i=1

for i=1:5

    kk(:,i)=Location(xx,yy,sum(:,i));

end;

k_x=kk(1,:)

k_y=kk(2,:)

X=[]

Y=[]

for i=1:5

    X(:,i)=[k_x(i),1]

    Y(:,i)=k_y(i)

end

X=X.'

Y=Y.'

%a = -5:0.1:2;

%b = 0:0.1:10;

EK=(X.'*X)^(-1)*X.'*Y

a=EK(1)

b=EK(2)

y1=a.*k_x+b;

plot(k_x,k_y,'*',k_x,y1);

toc