GPS用户位置求解Matlab仿真121118

GPS定位技术与应用实验——GPS用户位置求解Matlab仿真 一、定位原理 GPS用户对卫星j进行伪距测量，产生观测方程：222()()()jjujujuuxxyyzzc(1)其中(xj,yj,zj)表示第j颗卫星的位置坐标；(xu,yu,zu)是用户的位置坐标，u是用户接收机钟与GPS系统时钟的相对误差。设一共观测到N颗卫星，则得到方程组：22211112222222222()()()()()()()()()uuuuuuuuNNuNuNuuxxyyzzcxxyyzzcxxyyzzc(2)定位的目的就是计算(xu,yu,zu)和u。直接求解上述非线性方程组十分困难。可以采用牛顿迭代法这种常用的数值计算方法，其中的关键思想是线性化及最小二乘法。具体过程如下：对用户位置进行估计，得到估计位置坐标(x0,y0,z0)，用(x,y,z)表示估计位置与真实位置的偏移量，即000uuuxxxyyyzzz(3)设222(,,)()()()uuujujujufxyzxxyyzz，并把它在(x0,y0,z0)做一阶泰勒级数展开得：000000000000000000000000000000(,,)(,,)(,,)(,,)(,,)(,,)(,,)(,,)(,,)uuujjjfxyzfxyzfxyzfxyzfxyzxyzxyzxxyyzzfxyzxyzfxyzfxyzfxyz(4)因此，把对卫星j的观测方程线性化后得到：000000000000000(,,)(,,)(,,)(,,)jjjjuxxyyzzfxyzxyzcfxyzfxyzfxyz(5)令000000000000000(,,)(,,)(,,)(,,)jjjxjjyjjzjfxyzxxafxyzyyafxyzzzafxyz(6)伪距观测方程变化为：jxjyjzjuaxayazc(7)把方程组(2)中的每个方程线性化，得到下面的线性方程组：11112222xyzuxyzuNxNyNzNuaxayazcaxayazcaxayazc(8)把(8)写成矩阵形式，可得：ρHx(9)其中12Nρ，111222111xyzxyzxNyNzNaaaaaaaaaH，uxyzcx。按照上述方法，求解非线性方程组(2)的问题被转化为求解线性方程组（见(8)和(9)）。如果只能观察到4颗卫星，即N=4，(8)和(9)是个根据4条线性方程求解4个未知数的问题，具有唯一解：1xHρ(10)如果能观察到的卫星数量大于4，即N4，求解(8)和(9)是个超定方程组（即方程数量大于未知数的数量），此时需要使用最小二乘法求，解的形式为：1()TTxHHHρ(11)求出(x,y,z)后，使用(3)便求出用户坐标。迭代：因为线性化使用了一阶泰勒级数展开近似，这种近似只有当估计坐标(x0,y0,z0)非常接近真实坐标(xu,yu,zu)时才有效。如果(x,y,z)太大，需要用本次计算得出的坐标(xu,yu,zu)作为下一次计算的估计坐标(x0,y0,z0)，重新迭代上述计算过程，直到计算得到的(x,y,z)的值比较小为止。二、Matlab程序代码 下面Matlab程序完成利用伪距测量用户位置的Matlab仿真计算。1、主程序SatellitePosition=[177461757273651;12127-9774210911;13324-18178143921;14000-13073190581;19376-15756-73651;zeros(19,4)];%卫星位置%卫星位置坐标，每一行数据的前三列分别表示卫星的X、Y、Z坐标值，第4列数据表示本颗卫星是否可见，1为可见，0为不可见。UserPosition=[640000];%用户真实位置（注意：定位程序并未用到此参数）Prange=CalculatePseudoRange(SatellitePosition,UserPosition);%函数CalculatePseudoRange用于计算机模拟伪距测量结果，计算结果Prange是一个矢量，其中的不同元素表示对不同卫星的伪距测量结果[CalUserPosition,OK]=CalculateUserPosition2(SatellitePosition,Prange);%调用函数CalculateUserPosition2，进行定位计算，计算CalUserPosition中包含位置计算结果。2、伪距测量模拟函数CalculatePseudoRangefunctionPrange=CalculatePseudoRange(SatellitePosition,UserPosition)%计算机模拟伪距测量c=3e5;%光速，单位：km/s；DeltaT=1e-4;%钟差为1e-4数量级秒，假设卫星钟间时钟一致，DeltaT=Tu-Ts；钟差不宜超过3e-4，否则不收敛；VisSatNum=0;%首先找出可以观测到的卫星数量SatellitePosNew=[];fork=1:24ifSatellitePosition(k,4)==1VisSatNum=VisSatNum+1;SatellitePosNew=[SatellitePosNew;SatellitePosition(k,1:3)];end%ifend%forPrange=ones(1,VisSatNum);%求解用户接收机收到的伪距信息forn=1:VisSatNumPrange(1,n)=sqrt((SatellitePosNew(n,:)-UserPosition)*(SatellitePosNew(n,:)-UserPosition)'+c*DeltaT);end3、定位计算函数CalculateUserPosition2function[CalUserPosition,CalculateOK]=CalculateUserPosition2(SatellitePosition,Prange)%输入参数：%卫星位置坐标SatellitePosition，对每一颗可见卫星的伪距测量结果Prange%输出参数：%用户位置坐标：CalUserPosition，是一个矩阵，第一行表示最终定位结果，后面几行显示定位计算的中间过程结果；%参数CalculateOK表示用户位置计算是否成功，1为成功，0为失败；%该程序用线性化方法求解四个或多个卫星的伪距、钟差方程，具体算法见课本%假设我们接收到4个或者更多伪距后，有如下方程%PR=sqrt((xi-x)^2+(yi-y)^2+(zi-z)^2)+ct,i=1,2,3,4%使用最小二乘法求解c=3e5;%光速，单位：km/s；DeltaT=1e-3;%钟差为1e-4数量级秒，假设卫星钟间时钟一致，DeltaT=Tu-Ts；钟差不宜超过3e-4，否则不收敛；VisSatNum=0;CalculateOK=1;%首先找出可以接收到的卫星，多于4颗继续运算，否则返回SatellitePosNew=[];fork=1:24ifSatellitePosition(k,4)==1VisSatNum=VisSatNum+1;SatellitePosNew=[SatellitePosNew;SatellitePosition(k,1:3)];endendifVisSatNum4%不足4颗可见卫星CalculateOK=0;CalUserPosition=[000];returnendXYZ0=[000];%给用户位置赋初值CalculateRecord=XYZ0;%此变量用于保存每一步迭代计算的中间结果DeltaT0=0;%时钟差初始值Wxyz=SatellitePosNew;%卫星位置坐标Error=1000;ComputeTime=0;while(Error0.01)&&(ComputeTime1000)%开始迭代运算ComputeTime=ComputeTime+1;R=ones(1,VisSatNum);forn=1:VisSatNumR(1,n)=sqrt((Wxyz(n,:)-XYZ0)*(Wxyz(n,:)-XYZ0)')+DeltaT0*c;end%forDeltaP=R-Prange;A=ones(VisSatNum,3);forn=1:VisSatNumA(n,:)=(Wxyz(n,:)-XYZ0)./R(1,n);endH=[Aones(VisSatNum,1)];DeltaX=inv(H'*H)*H'*DeltaP';%最小二乘法求卫星位置TempDeltaX=DeltaX(1:3,:);Error=max(abs(TempDeltaX));XYZ0=XYZ0+DeltaX(1:3,:)';ifComputeTime10CalculateRecord=[CalculateRecord;XYZ0];endDeltaT0=DeltaX(4,1)/(-c);end%whileifComputeTime==1000CalUserPosition=[000];CalculateOK=0;elseCalUserPosition=[XYZ0;CalculateRecord];end三、实验内容 1、熟悉Matlab编程的语法、环境。2、定位程序是一个迭代运算程序，卫星坐标和用户坐标由上述语句给出，运行程序，那么a)写出最终算出的用户坐标结果（要求精确到小数点后4位）；b)写出程序的迭代计算次数；c)通过编程，计算出每次迭代计算的中间结果与用户真实位置的距离。（要求：给出具体程序语句。）提示：CalculateUserPosition2程序返回的变量CalUserPosition包含了每一次迭代计算的中间结果。设用户真实坐标为(xu,yu,zu)，迭代中间结果的坐标为(xc,yc,zc)，则两者的距离为：222()()()ucucucxxyyzz，这反映了计算结果的误差。3、把用户真实坐标设为(你的学号后四位+4000,0,0)，保持卫星坐标不变，重复上述的a)、b)、c)三步，分别给出程序运行结果。四、思考题 1.程序中的坐标是在哪个坐标系下的坐标？坐标的单位是什么？2.在实验中，卫星的位置坐标是已知量。问：在实际使用中，用户如何获知卫星的坐标信息？3.定位计算，除了要知道卫星的坐标，还要知道伪距信息。问：a)什么叫伪距？b)伪距信息如何获得？4.程序CalculateUserPosition2中，语句“XYZ0=[000];”给用户坐标附的初始值为（0，0，0）。问（0，0，0）是否最佳的用户坐标初始值？有更好的用户坐标初始值吗？为什么？5.程序CalculateUserPosition2中，while语句的判断条件为“(Error1)&&(ComputeTime1000)”，即要求两个判断条件同时成立才循环，问：a)判断条件Error1是什么意思？解释其物理含义。b)判断条件ComputeTime1000是什么意思？解释其在程序中的作用。6.程序CalculateUserPosition2中，语句“A(n,:)=(Wxyz(n,:)-XYZ0)./R(1,n);”是什么意思？请用定位原理中的数学公式来解释。7.程序CalculateUserPosition2中，语句“XYZ0=XYZ0+DeltaX(1:3,:)';”是什么意思？请用定位原理中的内容解释。8.指出程序需要改善的地方。实验报