高斯牛顿迭代

1、非线性超定方程组求解非线性超定方程组求解高斯高斯- -牛顿迭代法简介牛顿迭代法简介平面三点定位原理平面三点定位原理非线性超定方程组非线性超定方程组高斯高斯-牛顿迭代法牛顿迭代法空间定位原理空间定位原理手机位置的三点定位法手机位置的三点定位法已知三个基站的位置已知三个基站的位置 P1(x1, y1) P2(x2, y2) P3(x3, y3)手机位置手机位置 Q(x, y) ？已测得已测得Q到点到点P1、P2以及以及 P3 的距离的距离d1, ,d2和和 d3 232323222222212121)()()()()()(dyyxxdyyxxdyyxx d1d2d3Q数学模型数学模型 232332

2、233222222222222121122112222222dyyyyxxxxdyyyyxxxxdyyyyxxxx 2131311212)()()()(byyyxxxbyyyxxx2/)()(2222212121221yxyxddb 2/)()(2323212121232yxyxddb 其中，其中， 2113131212bbyxyyxxyyxx矩阵表示矩阵表示 例例1 已知三个接收站位置数据及手机距离数据已知三个接收站位置数据及手机距离数据Pk xk yk dP1 10 40 33.5P2 50 10 40P3 100 50 60 x = 40.9500 y = 47.5625 直接法定位直接

3、法定位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=Ab定位解算实验定位解算实验1:1: 0)()(0)()(0)()(323

4、232222212121 YyXxYyXxYyXx 0),(0),(0),(321yxfyxfyxf非线性超定方程组非线性超定方程组方程组局部线化方程组局部线化 0)()(),(0)()(),(0)()(),(320310003220210002120110001gyygxxyxfgyygxxyxfgyygxxyxf202001)()(kkkkYyXxXxg 202002)()(kkkkYyXxYyg ),(100yxkkxfg ),(200yxkkyfg 线性超定方程组线性超定方程组 ),(),(),(00300200100323122211211yxfyxfyxfyyxxgggggg000

5、0FyyxxG 超定方程组最小二乘解超定方程组最小二乘解0010000)(FGGGyyxxTT 初值修正初值修正001000011)(FGGGyxyxTT 高斯高斯-牛顿迭代牛顿迭代kTkkTkkkkkFGGGyxyx111)( ), 2 , 1 , 0( k05 .33)40()10(22 yx040)10()50(22 yx060)50()100(22 yx2211)40()10(10 yxxg2221)10()50(50 yxxg2231)50()100(100 yxxg2212)40()10(40 yxyg2222)10()50(10 yxyg2232)50()100(50 yxyg非

6、线性方程组非线性方程组初值选取初值选取:3/ )1005010(0 x3/ )501040(0 y高斯高斯- -牛顿迭代法牛顿迭代法53.3333 41.2921 41.0806 41.0823 41.0824 41.0824 33.3333 49.4713 49.3049 49.2986 49.2983 49.2983 x = 40.824 y = 49.2983 比较直接法结果比较直接法结果x = 40.9500 y = 47.5625 05 .33)40()10(22 syx040)10()50(22 syx060)50()100(22 syx修正模型修正模型: ),(),(),(111

7、00300200100323122211211yxfyxfyxfsyyxxgggggg局部线化局部线化:高斯高斯- -牛顿迭代向量序列牛顿迭代向量序列53.3333 41.1841 41.2982 41.2983 41.2983 33.3333 47.7668 47.7550 47.7548 47.7548 s = 1.2553定位点到接收站距离数据定位点到接收站距离数据32.244738.744758.744733.54060原始距离数据原始距离数据32.2447= 33.5 - s38.7447= 40 - s58.7447= 60 - s人工变量人工变量 sP=10,40;50,10;1

8、00,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)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=GF; X=X-dX(1:

9、2,:); Pk=Pk,X;ends=dX(3)PkD=P-1;1;1*X;rd=sqrt(sum(D.2,2)d+s全球定位系统（全球定位系统（Global Positioning System - GPS）是美国从本世纪是美国从本世纪70年代开始研制，历时年代开始研制，历时20年，耗年，耗资资200亿美元，于亿美元，于1994年全面建成，具有在海、陆、年全面建成，具有在海、陆、空进行全方位实时三维导航与定位能力的新一代空进行全方位实时三维导航与定位能力的新一代卫星导航与定位系统。卫星导航与定位系统。2011年年7月月27日日5时时44分，我国西昌卫星发射中分，我国西昌卫星发射中心心“长征三

10、号甲长征三号甲”运载火箭，成功将第九颗北运载火箭，成功将第九颗北斗卫星送入预定转移轨道。斗卫星送入预定转移轨道。北斗卫星导航系统是中国自行研制北斗卫星导航系统是中国自行研制开发的区域性卫星定位与通信系统。开发的区域性卫星定位与通信系统。和美国和美国GPS、俄罗斯俄罗斯GLONASS相相比，增加了通讯功能。比，增加了通讯功能。 S2 S3 S1 S4 TGPS接收机定位解算原理接收机定位解算原理几何方程组几何方程组 GX = L 111321232221131211mmmgggggggggG 0)()()(0)()()(0)()()(22222222221212121mmmmbZzYyXxbZz

11、YyXxbZzYyXx 2.最小二乘法解超定方程组最小二乘法解超定方程组 GX=L 其中其中, X = x y z b T1. .构造几何方程组系数矩阵构造几何方程组系数矩阵 G 和右端向量和右端向量 L222)()()(zZyYxXDjjjj 计算计算 gj1=(Xj x )/Dj, ,gj2=(Yj y )/Dj, ,gj3=(Zj z )/Dj, ,gj4= 1 , Lj= Dj j ( j =1,2,N)4.判断判断 | x|+| y|+| z|10-8 是否成立是否成立，3. 计算修正量计算修正量 x x + x , y y + y, z z + z若不成立则转第若不成立则转第1 1步；步；若成立，则结束计算。若成立，则结束计算。 444342413433323124232221141312111)(