迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法

优化算法/TOA室内定位/
Target localization with range-only measurements
方法：1-非线性最小二乘估计-迭代最小二乘求解
2-极大似然估计-高斯牛顿法求解
二维仅距离测量的定位问题；
蒙特卡洛仿真
性能指标：CRLB， RMSE，**

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

% TOA定位
%迭代最小二乘
%极大似然估计—高斯牛顿解法
%牛顿法
clc;close all;clear all;
N=50;%迭代次数
deta=0;
Runs=500;%Monte karloml_runx=0;
ml_runy=0;
ls_runx=0;
ls_runy=0;
%%LS变量定义
ls_res=zeros(2,N);
ls_v=zeros(2,N);
ls_x=zeros(1,N);
ls_y=zeros(1,N);%%ML变量定义
ml_res=zeros(2,N);
ml_v=zeros(2,N);
ml_x=zeros(1,N);
ml_y=zeros(1,N);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=(40)^2;%noise variance 40m%非线性函数
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=sqrt((y1-yp)^2+(x1-xp)^2);
h02=sqrt((y2-yp)^2+(x2-xp)^2);
h03=sqrt((y3-yp)^2+(x3-xp)^2);%真值
H0(1,1)=h01;
H0(2,1)=h02;
H0(3,1)=h03;for i=1:1:Runs;i%白高斯噪声
w=sqrt(Q)*randn(3,1);
%measuremen z
Z=H0+w;
z1=Z(1,1);z2=Z(2,1);z3=Z(3,1);%ML估计初值
% ml_x0=(x3*((yp-y3)/(xp-x3))-x1*((yp-y1)/(xp-x1))-y3+y1)/((yp-y3)/(xp-x3)-(yp-y1)/(xp-x1)); %20060;%初始位置
% ml_y0=((x1-x3)*((yp-y1)/(xp-x1))*((yp-y3)/(xp-x3))-y1*((yp-y3)/...
%     (xp-x3))+y3*((yp-y1)/(xp-x1)))/((yp-y1)/(xp-x1)-(yp-y3)/(xp-x3));  %19770;y纵坐标初始位置
ml_x0=20060;
ml_y0=19770;
ml_v0=[ml_x0;ml_y0];%LS估计初值
ls_x0=(x3*((yp-y3)/(xp-x3))-x1*((yp-y1)/(xp-x1))-y3+y1)/((yp-y3)/(xp-x3)-(yp-y1)/(xp-x1)); %20060;%初始位置
ls_y0=((x1-x3)*((yp-y1)/(xp-x1))*((yp-y3)/(xp-x3))-y1*((yp-y3)/...(xp-x3))+y3*((yp-y1)/(xp-x1)))/((yp-y1)/(xp-x1)-(yp-y3)/(xp-x3));  %19770;y纵坐标初始位置ls_x0=20060;%初始位置ls_y0=19770;%y纵坐标初始位置
ls_v0=[ls_x0;ls_y0];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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%海森矩阵for k=1:3;ls_H(k,1)=sqrt((ls_y(t-1)-y(k))^2+(ls_x(t-1)-x(k))^2);end%雅可比矩阵for k=1:3;ls_J(k,1)=(ls_x(t-1)-x(k))/sqrt((ls_y(t-1)-y(k))^2+(ls_x(t-1)-x(k))^2);ls_J(k,2)=(ls_y(t-1)-y(k))/sqrt((ls_y(t-1)-y(k))^2+(ls_x(t-1)-x(k))^2);end%噪声方差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 ls_res(:,t)=ls_v(:,t);ls_x(t)=ls_v(1,t);ls_y(t)=ls_v(2,t);endfor k=1:50;ls_runx=ls_runx+(xp-ls_x(k))^2;ls_runy=ls_runy+(yp-ls_y(k))^2;end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ILS结束%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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%海森矩阵:ml_hfor k=1:3;ml_h(k,1)=sqrt((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2);end%Jml_Jfor k=1:3;ml_J(k,1)=(ml_x(t-1)-x(k))/sqrt((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2);ml_J(k,2)=(ml_y(t-1)-y(k))/sqrt((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2);end%函数h的二阶梯度ml_JJfor k=1:3ml_JJ(k,1)=((ml_y(t-1)-y(k))^2)/(((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2)^(3/2));%横坐标的二阶导ml_JJ(k,2)=((ml_x(t-1)-x(k))^2)/(((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2)^(3/2));%纵坐标的二阶导ml_JJ(k,3)=-((ml_x(t-1)-x(k))*(ml_y(t-1)-y(k)))/(((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2)^(3/2));%横纵坐标的二阶导end%一阶梯度矩阵ml_F（函数lamuda）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_Hml_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;endml_res(:,t)=ml_v(:,t);ml_x(t)=ml_v(1,t);ml_y(t)=ml_v(2,t);endfor k=1:50;ml_runx=ml_runx+(xp-ml_x(k))^2;ml_runy=ml_runy+(yp-ml_y(k))^2;end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ML—牛顿法结束%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end%蒙特卡洛
ml_Runsx=sqrt(ml_runx/(Runs*N));
ml_Runsy=sqrt(ml_runy/(Runs*N));
ls_Runsx=sqrt(ls_runx/(Runs*N));
ls_Runsy=sqrt(ls_runy/(Runs*N));%ML:fisher 信息矩阵for k=1:3;J(k,1)=(xp-x(k))/sqrt((yp-y(k))^2+(xp-x(k))^2);J(k,2)=(yp-y(k))/sqrt((yp-y(k))^2+(xp-x(k))^2);endFIM(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))%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');

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

迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法相关推荐

迭代求解最优化问题——信赖域方法
信赖域方法前面提到了Line Search算法分为两步,首先确定方向,然后确定步长,实际上是假设近似模型在某一方向上可以较好的代表真实模型.Trust region算法则在此基础上,假设在一个选定的 ...
迭代求解最优化问题——带约束问题
文章目录带约束问题惩罚因子迭代步修改带约束问题在求解最优化问题时,很多问题的变量满足一定的约束,其数学形式为 min⁡f(x)st.C(x)=0\min f(\mathbf x) \\ st ...
漫步最优化三十四——高斯-牛顿法
你的温柔像羽毛,\textbf{你的温柔像羽毛,} 秘密躺在我怀抱.\textbf{秘密躺在我怀抱.} 你的微笑像拥抱,\textbf{你的微笑像拥抱,} 只有我能看到.\textbf{只有我能看到. ...
SLAM从0到1——状态估计之最小二乘问题解法：最速下降法、牛顿法、高斯牛顿法、LM法...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达学习3D视觉核心技术,扫描查看介绍,3天内无条件退款圈里有高质量教程资料.可答疑解惑.助你高效解决问 ...
LIO-SAM：在高斯牛顿法求解过程中用SO3代替欧拉角
LIO-SAM发表于IROS2020,是一个效果非常好的惯性-激光紧耦合里程计我打算给我们的机器人搞一个激光里程计,于是打算把LIO-SAM改一改搞过来修改过程中发现一个问题:在里程计求解(map ...
matlab求解最优化问题(数学建模)
matlab求解最优化问题(数学建模) 1.线性规划 matlab中线性规划优化计算方法和实例在matlab中用于线性规划优化计算的是linprog()函数. 公式:[x,fval,exitflag ...
MATLAB 求解最优化问题
MATLAB 求解最优化问题 MATLAB 优化工具箱解线性规划模型1 minz=cXs.t.AX≤b \text{min} \quad z=cX \\ s.t.\quad AX\leq b 命令: ...
基于QR分解迭代求解方阵特征值和特征向量
基于QR分解迭代求解方阵特征值和特征向量一.特征值与特征向量求解的难点线性代数的知识告诉我们如果要求一个方阵的特征值,只需要求解如下的特征方程的根即可: f ( λ ) = ( λ − λ 1 ) ...
G-S迭代求解线性方程,以及三对角矩阵求解
G-S迭代代码最少可以这么写: def GS(A,b,X,N):X_old = X.copy()n = X.shape[0]for k in range(N):X[0,0] = (b[0,0] - ...

迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法

迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法

迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法相关推荐

最新文章

热门文章