( x1 x )2 ( y1 y )2 d12 2 ( x 2 x ) 2 ( y2 y ) 2 d 2 2 ( x 3 x ) 2 ( y3 y ) 2 d 3

2 2 2 x 2 2 x1 x x1 y 2 2 y1 y y1 d1 数学模型 2 2 2 x 2 2 x 2 x x 2 y 2 2 y2 y y2 d 2 2 2 2 2 x 2 x 3 x x 3 y 2 2 y3 y y3 d 3
PDOP a11 a22 a33
HDOP a11 a22
VDOP a33
时间DOP
TDOP a44
《GPS卫星导航定位原理与方法》, 刘基余
y 40 ( x 10 ) 2 ( y 40 ) 2
g11 g 21
g12
g22 g32
y 10 ( x 50)2 ( y 10)2 y 50 ( x 100)2 ( y 50)2
g31
x 100 ( x 100) ( y 50)
矩阵表示
x 2 x1 x x 1 3
y2 y1 x b1 y3 y1 y b2
例1 已知三个接法定位
xk 10 50 100
yk 40 10 50
d 33.5 40 60
方程组局部线化
f1 ( x0 , y0 ) ( x x0 ) g11 ( y y0 ) g12 0 f 2 ( x0 , y0 ) ( x x0 ) g 21 ( y y0 ) g 22 0 f ( x , y ) ( x x )g ( y y )g 0 0 31 0 32 3 0 0
a11 a 21 A (G T G ) 1 a 31 a41
a12 a 22 a 32 a42
a13 a 23 a 33 a43
a14 a 24 a 34 a44
几何DOP 位置DOP 水平DOP 垂直DOP
GDOP a11 a22 a33 a44
x x0 T T (G0 G0 ) 1 G0 F0 y y0
初值修正 高斯-牛顿迭代
x1 x0 T T (G0 G0 )1 G0 F0 y1 y0 xk 1 xk T T (Gk Gk )1 Gk Fk yk 1 yk ( k 0,1,2, )
非线性超定方程组
f1 ( x , y ) 0 f 2 ( x, y) 0 f ( x, y) 0 3
( x X )2 ( y Y )2 0 1 1 1 2 2 ( x X 2 ) ( y Y2 ) 2 0 2 2 ( x X 3 ) ( y Y3 ) 3 0
x = 40.824 y = 49.2983
比较直接法结果
x = 40.9500 y = 47.5625
( x 10 ) 2 ( y 40 ) 2 33.5 s 0
修正模型:
( x 50 ) 2 ( y 10 ) 2 40 s 0 ( x 100 ) 2 ( y 50 ) 2 60 s 0
f k gk 1 x
gk 2
( x 0 , y0 )
f k y
gk 1
( x0 , y0 )
x0 X k
gk 2
( x0 X k )2 ( y0 Yk )2 y0 Yk
( x0 X k )2 ( y0 Yk )2
线性超定方程组
x x0 G0 F0 y y0
非线性超定方程组求解
——高斯-牛顿迭代法简介
平面三点定位原理 非线性超定方程组 高斯-牛顿迭代法 空间定位原理
手机位置的三点定位法
已知三个基站的位置
P1(x1, y1) P2(x2, y2) P3(x3, y3)
d1
d2 Q
d3
手机位置 Q(x, y) ？？ 已测得Q到点P1、P2以及 P3 的距离d1,d2和 d3

( x2 x1 ) x ( y2 y1 ) y b1 ( x3 x1 ) x ( y3 y1 ) y b2
2 2 2 2 2 b1 [d 2 d12 ( x1 y1 ) ( x2 y2 )]/ 2 其中， 2 2 2 2 2 2 b2 [d3 d1 ( x1 y1 ) ( x3 y3 )]/ 2
( x 10 ) 2 ( y 40 ) 2 33 .5 0
非线性方程组
x 10 ( x 10 ) 2 ( y 40 ) 2 x 50 ( x 50 ) 2 ( y 10 ) 2
( x 50 ) 2 ( y 10 ) 2 40 0 ( x 100 ) 2 ( y 50 ) 2 60 0
x = 40.9500 y = 47.5625
定位解算实验1: P=[10,40;50,10;100,50]; d=[33.5;40;60]; t=linspace(0,2*pi,100); xd=P(:,1); yd=P(:,2); xt=xd*ones(1,100)+d*cos(t); yt=yd*ones(1,100)+d*sin(t); figure(1),plot(xd,yd,'or',xt',yt','b') A=[P(2,:)-P(1,:);P(3,:)-P(1,:)]; R=xd.^2+yd.^2;D=d.^2; b=-0.5*[D(2)-D(1)+R(1)-R(2);D(3)-D(1)+R(1)-R(3)]; Q=A\b
2.最小二乘法解超定方程组 GX=L 其中, X = [ x 3. 计算修正量
y z b ]T
x x + x , y y + y, z z + z
4.判断 | x|+| y|+| z|≤10-8 是否成立，
若不成立则转第1步； 若成立，则结束计算。
精度因子(DOP)
局部线化:
g11 g 21 g 31
g12 g 22 g 32
1 x x0 f 1 ( x0 , y0 ) y y f ( x , y ) 1 0 2 0 0 f 3 ( x 0 , y 0 ) 1 s
2 2
初值选取:
x0 (10 50 100) / 3 y0 (40 10 50) / 3
高斯-牛顿迭代法
53.3333 41.2921 41.0806 41.0823 41.0824 41.0824 ··· ··· ·· 33.3333 49.4713 49.3049 49.2986 49.2983 49.2983 ··· ··· ··
1.构造几何方程组系数矩阵 G 和右端向量 L
计算 gj1=(Xj – x )/Dj,gj2=(Yj – y )/Dj,gj3=(Zj – z )/Dj, gj4= – 1 , Lj= Dj – j ( j =1,2,···,N) ···
D j ( X j x ) 2 (Y j y ) 2 ( Z j z ) 2
yt=yd*ones(1,100)+d*sin(t);
figure(1),plot(xd,yd,'or',xt',yt','b')
x=sum(P(:,1))/3;y=sum(P(:,2))/3; X=[x;y];Pk=X; for t=1:6 for k=1:3 Dk=X'-P(k,:);dk=norm(Dk); G(k,1:2)=Dk/dk;F(k)=dk-d(k); end G(:,3)=ones(3,1);dX=G\F'; X=X-dX(1:2,:); Pk=[Pk,X]; end s=dX(3) Pk D=P-[1;1;1]*X'; rd=sqrt(sum(D.^2,2)) d+s
g11 g 21 g 31
g12 f 1 ( x0 , y0 ) x x 0 f ( x , y ) g 22 2 0 0 y y0 f 3 ( x 0 , y 0 ) g 32
超定方程组最小二乘解
GPS接收机定位解算原理
S2 S3
几何方程组
S4
GX = L
g13 g 23 gm 3 1 1 1
S1
T
g11 g 21 G gm1
g12 g 22 gm 2
( x X )2 ( y Y )2 ( z Z )2 b 0 1 1 1 1 ( x X )2 ( y Y )2 ( z Z )2 b 0 2 2 2 2 2 2 2 ( x X m ) ( y Ym ) ( z Z m ) b m 0
全球定位系统（Global Positioning System - GPS） 是美国从本世纪70年代开始研制，历时20年，耗 资200亿美元，于1994年全面建成，具有在海、陆、 空进行全方位实时三维导航与定位能力的新一代 卫星导航与定位系统。
北斗卫星导航系统是中国自行研制 开发的区域性卫星定位与通信系统。 和美国GPS、俄罗斯GLONASS相 比，增加了通讯功能。 2011年7月27日5时44分，我国西昌卫星发射中 心“长征三号甲”运载火箭，成功将第九颗北 斗卫星送入预定转移轨道。