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

Love The Way You Lie 2022-01-15 21:17 276阅读 0赞

**轨迹定位方法——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 ）求此人行走的直线方程。

**定位方法****——****多点定位**

![20190613160925880.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70][]

解题思路：

（1）线性回归方程

![20190613155649541.png][]

（2）最小二乘法

*  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 1][]

程序实现：

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 3][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 4][]

求出坐标

X1 =

0.4483    1.0283    1.4512    2.1122    2.5165

Y1 =

0.5487    0.9273    1.5460    1.9580    2.6070

即5个待定点的坐标，可确定直线。

最小二乘法确定系数a和b：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 5][]

![20190613162439144.png][]

思路同上

（感谢物联网老师，让我从入坑到放弃，哈哈哈！小编还是会继续努力的，争取发表更多有意义的原创文章）

希望能帮到您

非常感谢您的观看，喜欢的话点个赞吧?

一只喜欢猫和老鼠的呵呵哒！

[20190613160925880.png]: /images/20220115/f3ee0055903144118018570a72fbc29a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70]: /images/20220115/6586c6d3b72d430082ffbfcfa347b024.png
[20190613155649541.png]: /images/20220115/254cd000e5874a11b1ab0a616d8a3063.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 1]: /images/20220115/8d1e5480504341a987d1b1f6d002fd16.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 2]: /images/20220115/95b436db0e5b4a5db516e05c5ab83e00.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 3]: /images/20220115/16497d8be36a49a696242487b1d90bb7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 4]: /images/20220115/ea005324d6a340dc8c82a11c151b63f5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MTk4NzAxNg_size_16_color_FFFFFF_t_70 5]: /images/20220115/dd7555159a9d41c3a4efd8112ca2bd48.png
[20190613162439144.png]: /images/20220115/bc23b2df27484e8c8457ca515bf4422d.png