ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

角度测量(AOA/DOA)室内定位-迭代最小二乘和高斯牛顿法\MATLAB

2021-04-11 09:59:12  阅读：703  来源： 互联网

标签：AOA ml DOA MATLAB y1 tan z3 z1 ls


角度测量(AOA/DOA)室内定位-迭代最小二乘和高斯牛顿法\MATLAB

原创不易，路过的各位大佬请点个赞

AOA定位，角度测量
迭代最小二乘、高斯牛顿法
二维仅角度测量的定位问题；
~ ~ ！！！！！！！！！！！！！！！！！！！！！！！！！！
总结束：
最优化问题方法
1-AOA定位，角度测量
2-非线性最小二乘估计-迭代最小二乘求解
3- 蒙特卡罗
性能指标：RMSE，CRLB**


clc;
close all;
clear all;
%迭代求解最优化问题（TOA定位）
% AOA定位
%迭代最小二乘
%极大似然估计—高斯牛顿解法
%牛顿法

%%% 参数设置
N=50;%迭代次数
deta=0;
Runs=600;%Monte karlo

%%initial varient

% ILS 变量定义
ls_res=zeros(2,N);
ls_v=zeros(2,N);
ls_x=zeros(1,N);
ls_y=zeros(1,N);
ls_runx=0;
ls_runy=0;

%%ML变量定义
ml_res=zeros(2,N);
ml_v=zeros(2,N);
ml_x=zeros(1,N);
ml_y=zeros(1,N);
ml_runx=0;
ml_runy=0;

d0=0.5;%迭代终止条件
% T=1;%采样周期
%target
xp=20000;
yp=20000;%目标位置
vp=[xp;yp];
x=zeros(1,50);
y=zeros(1,50);
%sensor 
x1=0;y1=0;
x2=15000;y2=5000;
x3=30000;y3=0;%传感器位置，三个
v1=[x1;y1];
v2=[x2;y2];
v3=[x3;y3];
y=[y1,y2,y3];
x=[x1,x2,x3];
Z=zeros(3,1);%传感器量测数据
z1=0;z2=0;z3=0;
Z(1,1)=z1;
Z(2,1)=z2;
Z(3,1)=z3;

%噪声
w=zeros(3,1);
Q=(pi/180)^2;%noise variance

%非线性函数
h01=0;h02=0;h03=0;
H0=zeros(3,1);
H=zeros(3,1);


% hatv=zeros(2,1);%v=[x;y]
%%
% nonlinear funchtion h() matraix
h01=atan2(yp-y1,xp-x1);
h02=atan2(yp-y2,xp-x2);
h03=atan2(yp-y3,xp-x3);%真实位置信息
H0(1,1)=h01;
H0(2,1)=h02;
H0(3,1)=h03;

for i=1:1:Runs;
    i
%white gassian noise
w=sqrt(Q)*randn(3,1);
%measuremen z
Z=H0+w;
z1=Z(1,1);
z2=Z(2,1);
z3=Z(3,1);

%LS估计初值
ls_x0=(x3*tan(z3)-x1*tan(z1)-y3+y1)/(tan(z3)-tan(z1)); %20060;%初始位置
ls_y0=((x1-x3)*tan(z1)*tan(z3)-y1*tan(z3)+y3*tan(z1))/(tan(z1)-tan(z3));  %19770;y纵坐标初始位置
ls_v0=[ls_x0;ls_y0];

%ML估计初值
ml_x0=(x3*tan(z3)-x1*tan(z1)-y3+y1)/(tan(z3)-tan(z1)); %20060;%初始位置
ml_y0=((x1-x3)*tan(z1)*tan(z3)-y1*tan(z3)+y3*tan(z1))/(tan(z1)-tan(z3));  %19770;y纵坐标初始位置
ml_x0=ls_x0; %20060;%初始位置
ml_y0=ls_y0;  %19770;y纵坐标初始位置
ml_v0=[ml_x0;ml_y0];
       
       
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ML估计/牛顿法求解%%%%%%%%%%%%%%%%%%%%%%%%%%
%ML迭代
for t=1:1:N;
    if(t==1)
        ml_res(:,t)=ml_v0;
        ml_v(:,1)=ml_v0;
        ml_x(t)=ml_x0;
        ml_y(t)=ml_y0;
    else
        %H矩阵:ml_h
        ml_h(1,1)=atan2(ml_y(t-1)-y1,ml_x(t-1)-x1);
        ml_h(2,1)=atan2(ml_y(t-1)-y2,ml_x(t-1)-x2);
        ml_h(3,1)=atan2(ml_y(t-1)-y3,ml_x(t-1)-x3);
        %雅可比矩阵ml_J
        ml_J(1,1)=-(ml_y(t-1)-y1)/((ml_x(t-1)-x1)^2+(ml_y(t-1)-y1)^2);
        ml_J(1,2)=(ml_x(t-1)-x1)/((ml_x(t-1)-x1)^2+(ml_y(t-1)-y1)^2);
        ml_J(2,1)=-(ml_y(t-1)-y2)/((ml_x(t-1)-x2)^2+(ml_y(t-1)-y2)^2);
        ml_J(2,2)=(ml_x(t-1)-x2)/((ml_x(t-1)-x2)^2+(ml_y(t-1)-y2)^2);
        ml_J(3,1)=-(ml_y(t-1)-y3)/((ml_x(t-1)-x3)^2+(ml_y(t-1)-y3)^2);
        ml_J(3,2)=(ml_x(t-1)-x3)/((ml_x(t-1)-x3)^2+(ml_y(t-1)-y3)^2);
        %函数h的二阶梯度ml_JJ
        for i=1:3
            ml_JJ(i,1)=(2*(ml_y(t-1)-y(i))*(ml_x(t-1)-x(i)))/((ml_y(t-1)-y(i))^2+(ml_x(t-1)-x(i))^2)^2;
            ml_JJ(i,2)=-(2*(ml_y(t-1)-y(i))*(ml_x(t-1)-x(i)))/((ml_y(t-1)-y(i))^2+(ml_x(t-1)-x(i))^2)^2;
            ml_JJ(i,3)=((ml_y(t-1)-y(i))^2-(ml_x(t-1)-x(i))^2)/((ml_y(t-1)-y(i))^2+(ml_x(t-1)-x(i))^2)^2;
        end
        
        %一阶梯度矩阵ml_F
        ml_F(1,1)=1/Q*((z1-ml_h(1,1))*ml_J(1,1)+(z2-ml_h(2,1))*ml_J(2,1)+(z3-ml_h(3,1))*ml_J(3,1));
        ml_F(2,1)=1/Q*((z1-ml_h(1,1))*ml_J(1,2)+(z2-ml_h(2,1))*ml_J(2,2)+(z3-ml_h(3,1))*ml_J(3,2)); 
        
        %Hessian矩阵：ml_H
        ml_H(1,1)=((z1-ml_h(1,1))*ml_JJ(1,1)-(ml_J(1,1))^2+(z2-ml_h(2,1))*ml_JJ(2,1)-(ml_J(2,1))^2+(z3-ml_h(3,1))*ml_JJ(3,1)-(ml_J(3,1))^2)/Q;
        ml_H(2,2)=((z1-ml_h(1,1))*ml_JJ(1,2)-(ml_J(1,2))^2+(z2-ml_h(2,1))*ml_JJ(2,2)-(ml_J(2,2))^2+(z3-ml_h(3,1))*ml_JJ(3,2)-(ml_J(3,2))^2)/Q;
        ml_H(1,2)=((z1-ml_h(1,1))*ml_JJ(1,3)-(ml_J(1,1))*(ml_J(1,2))+(z2-ml_h(2,1))*ml_JJ(2,3)-(ml_J(2,1))*(ml_J(2,2))+(z3-ml_h(3,1))*ml_JJ(3,3)-(ml_J(3,1))*(ml_J(3,2)))/Q;
        ml_H(2,1)=ml_H(1,2);
        
        %ML迭代
        ml_v(:,t)=ml_v(:,t-1)-inv(ml_H)*ml_F;
    end
    
        ml_res(:,t)=ml_v(:,t);
        ml_x(t)=ml_v(1,t);
        ml_y(t)=ml_v(2,t);
       
end
        for k=1:50;
        ml_runx=ml_runx+(xp-ml_x(k))^2;
        ml_runy=ml_runy+(yp-ml_y(k))^2;
        end 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ML估计/牛顿法求解 结束%%%%%%%%%%%%%%%%%%%%%%%%%%        
       
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ILS%%%%%%%%%%%%%%%%%%%%%%%%%%       
%%LS迭代
for t=1:1:N;%迭代不超过50次
    if(t==1)
        ls_res(:,t)=ls_v0;
        ls_v(:,1)=ls_v0;
        ls_x(t)=ls_x0;
        ls_y(t)=ls_y0;
    else
        %H矩阵
        ls_h1=atan2(ls_y(t-1)-y1,ls_x(t-1)-x1);
        ls_h2=atan2(ls_y(t-1)-y2,ls_x(t-1)-x2);
        ls_h3=atan2(ls_y(t-1)-y3,ls_x(t-1)-x3);
        ls_H(1,1)=ls_h1;
        ls_H(2,1)=ls_h2;
        ls_H(3,1)=ls_h3;
       
        %J矩阵
        ls_J(1,1)=-(ls_y(t-1)-y1)/((ls_x(t-1)-x1)^2+(ls_y(t-1)-y1)^2);
        ls_J(1,2)=(ls_x(t-1)-x1)/((ls_x(t-1)-x1)^2+(ls_y(t-1)-y1)^2);
        ls_J(2,1)=-(ls_y(t-1)-y2)/((ls_x(t-1)-x2)^2+(ls_y(t-1)-y2)^2);
        ls_J(2,2)=(ls_x(t-1)-x2)/((ls_x(t-1)-x2)^2+(ls_y(t-1)-y2)^2);
        ls_J(3,1)=-(ls_y(t-1)-y3)/((ls_x(t-1)-x3)^2+(ls_y(t-1)-y3)^2);
        ls_J(3,2)=(ls_x(t-1)-x3)/((ls_x(t-1)-x3)^2+(ls_y(t-1)-y3)^2);
        %噪声R
        R=eye(3)*Q;
        
        ls_v(:,t)=ls_v(:,t-1)+inv(ls_J'*inv(R)*ls_J)*ls_J'*inv(R)*(Z-ls_H);%最小二乘迭代位置状态拟合
    
    end
    
%         deta=hatx(t)-hatx(t-1);%判断迭代终止；
%          if deta<=d0
%           break;
%          end
         
       ls_res(:,t)=ls_v(:,t);
       ls_x(t)=ls_v(1,t);
       ls_y(t)=ls_v(2,t);
       
end
       for k=1:50;
       ls_runx=ls_runx+(xp-ls_x(k))^2;
       ls_runy=ls_runy+(yp-ls_y(k))^2;
       end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ILS 结束%%%%%%%%%%%%%%%%%%%%%%%%%%        
       
end
%Monte karlo runs
ml_Runs_x=sqrt(ml_runx/(Runs*N))
ml_Runs_y=sqrt(ml_runy/(Runs*N))
ls_Runs_x=sqrt(ls_runx/(Runs*N))
ls_Runs_y=sqrt(ls_runy/(Runs*N))
%fisher 信息
       %J矩阵,代入真值
        J(1,1)=-(yp-y1)/((xp-x1)^2+(yp-y1)^2);
        J(1,2)=(xp-x1)/((xp-x1)^2+(yp-y1)^2);
        J(2,1)=-(yp-y2)/((xp-x2)^2+(yp-y2)^2);
        J(2,2)=(xp-x2)/((xp-x2)^2+(yp-y2)^2);
        J(3,1)=-(yp-y3)/((xp-x3)^2+(yp-y3)^2);
        J(3,2)=(xp-x3)/((xp-x3)^2+(yp-y3)^2);
% FI(1,1)=((z1-h01)*JI(1,1)+(z2-h02)*JI(2,1)+(z3-h03)*JI(3,1))/Q;
% FI(2,1)=((z1-h01)*JI(1,2)+(z2-h02)*JI(2,2)+(z3-h03)*JI(3,2))/Q;
FIM(1,1)=((J(1,1))^2+(J(2,1))^2+(J(3,1))^2)/Q;
FIM(2,2)=((J(1,2))^2+(J(2,2))^2+(J(3,2))^2)/Q;
FIM(1,2)=(J(1,1)*J(1,2)+J(2,1)*J(2,2)+J(3,1)*J(3,2))/Q;
FIM(2,1)=(J(1,1)*J(1,2)+J(2,1)*J(2,2)+J(3,1)*J(3,2))/Q;
ml_MSE=inv(FIM);
ml_CRLB_x=sqrt(ml_MSE(1,1))
ml_CRLB_y=sqrt(ml_MSE(2,2))
%LS:MSE
ls_MSE=inv(J'*inv(R)*J);
ls_CRLB_x=sqrt(ls_MSE(1,1))
ls_CRLB_y=sqrt(ls_MSE(2,2))

%结果比较
t=1:1:10;
    figure(1);
    plot([x1 xp],[y1 xp],'-o');hold on; 
    plot([x2 xp],[y2 yp],'-o');hold on;
    plot([x3 xp],[y3 yp],'-o');hold on;
    plot(xp,yp,'*');
    axis([-10000 50000 -10000 50000]);
    xlabel('East(m)'), ylabel('North(m)');
    
    
    figure(2);
    plot(t,ls_res(1,t),t,ml_res(1,t),'r--')
    title('Estimation of East Position')
    xlabel('Iteration j'), ylabel('The East Position');
    legend('ILS','ML')
    hold on; 
    
    figure(3);
    plot(t,ls_res(2,t),t,ml_res(2,t),'r--')
    title('Estimation of North Position')
    xlabel('Iteration j'), ylabel('The North Position');
    legend('ILS','ML');

在这里插入图片描述


定位布局

在这里插入图片描述


x-位置RMSE

在这里插入图片描述


Y-位置RMSE

AOA定位，角度测量
迭代最小二乘、高斯牛顿法
二维仅角度测量的定位问题；
~ ~ ！！！！！！！！！！！！！！！！！！！！！！！！！！
总结束：
最优化问题方法
1-AOA定位，角度测量
2-非线性最小二乘估计-迭代最小二乘求解
3- 蒙特卡罗
性能指标：RMSE，CRLB**

原创不易，路过的各位大佬请点个赞

标签：AOA,ml,DOA,MATLAB,y1,tan,z3,z1,ls
来源： https://blog.csdn.net/weixin_44044161/article/details/115583432

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有