室内定位TOA距离量测—迭代最小二乘和高斯牛顿法\MATLAB

距离测量(TOA)室内定位—迭代最小二乘和高斯牛顿法\MATLAB

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

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

1. 算法原理

1.1 广义量测函数

1.2 迭代最小二乘

CRLB:

1.3 最大似然估计—高斯牛顿法

似然函数目标函数

高斯牛顿法


CRLB:

2 量测方程（TOA定位）

2.1 雅可比矩阵

2.2 海森矩阵

3 参考代码

clc;
close all;
clear all;
N=50;%迭代次数
deta=0;
Runs=350;%Monte karlo
ml_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;

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

%噪声
w=zeros(3,1);
Q=(40)^2;%noise variance 40m

% 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;
    
%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*((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];
%ML估计初值
ml_x0=ls_x0;
ml_y0=ls_y0;
ml_v0=[ml_x0;ml_y0];

%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
        for k=1:3;
            ml_h(k,1)=sqrt((ml_y(t-1)-y(k))^2+(ml_x(t-1)-x(k))^2);
        end
       
        %雅可比矩阵ml_J
        for 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_JJ
        for k=1:3
            ml_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_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 
       
%%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矩阵
        for k=1:3;
            ls_H(k,1)=sqrt((ls_y(t-1)-y(k))^2+(ls_x(t-1)-x(k))^2);
        end
        %J矩阵
        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
        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);
       
end
       for k=1:50;
       ls_runx=ls_runx+(xp-ls_x(k))^2;
       ls_runy=ls_runy+(yp-ls_y(k))^2;
       end
  
end
%Monte karlo runs
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 信息
       %J矩阵,代入真值
       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);
       end
       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(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(2);
    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');

    figure(3);
    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)');
    
%     title('Estimation of East Position')
%     xlabel('Iteration j'), ylabel('The East Position');
%     legend('ILS','ML')
% %LS结果
%  t=1:1:10;
% figure('Color',[1 1 1]); 
% plot(t,hatx(t),'b-');
% % axis([1 10 20060 20260]);
% hold on
% 
% figure('Color',[1 1 1]); 
% plot(t, haty(t),'r-');
% % axis([1 10 19580 19780]);
% hold on
% 
% %ML结果
% figure('Color',[1 1 1]); 
% plot(t,ml_x(t),'b-');
% % axis([1 10 20060 20260]);
% hold on
% 
% figure('Color',[1 1 1]); 
% plot(t, ml_y(t),'r-');
% % axis([1 10 19580 19780]);
% hold on

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237