GPS大作业.docx

GPS大作业学院：电子工程学院班级：学号：GPS定位技术与应用实验——GPS用户位置求解Matlab一、定位原理 GPS用户对卫星j进行伪距测量，产生观测方程：（1）其中(xj,yj,zj)表示第j颗卫星的位置坐标；(xu,yu,zu)是用户的位置坐标，是用户接收机钟与GPS系统时钟的相对误差。设一共观测到N颗卫星，则得到方程组： (2)定位的目的就是计算(xu,yu,zu)和。直接求解上述非线性方程组十分困难。可以采用牛顿迭代法这种常用的数值计算方法，其中关键思想是线性化及最小二乘法。具体过程如下：对用户位置进行估计，得到估计位置坐标(x0,y0,z0)，用表示估计位置与真实位置的偏移量，即(3)设，并把它在(x0,y0,z0)做一阶泰勒级数展开得：(4)因此，把卫星j的观测方程线性化后得到：(5)令(6)伪距观测方程变化为：(7)把方程组(2)中的每个方程组线性化，得到下面的线性方程组：(8)把(8)写成矩阵形式，可得： (9)其中按照上述方法，求解非线性方程组(2)的问题被转化为求解线性方程组。如果能观测到的卫星数量大于4，求解(8)(9)是个超定方程组，此时需要使用最小二乘法。迭代：因为线性化使用了一阶泰勒级数展开近似，这种近似只有当估计坐标非常接近真实坐标时才有效。如果差距太太，需要用本次计算得出的坐标作为下次计算的估计坐标，重新迭代上述计算过程，知道计算得到的比较小为止。二、程序代码1、主程序SatellitePosition=[17746 17572 7365 1;12127 -9774 21091 1;13324 -18178 14392 1;14000 -13073 19058 1;19376 -15756 -7365 1;zeros(19, 4)];UserPosition=[6400 0 0 ];Prange=CalculatePseudoRange(SatellitePosition, UserPosition); [CalUserPosition, OK]=CalculateUserPosition2(SatellitePosition, Prange);2、伪距测量模拟函数functionPrange=CalculatePseudoRange(SatellitePosition,UserPosition) c=3e5;DeltaT=1e-4; VisSatNum=0;SatellitePosNew=[];for k=1:24ifSatellitePosition(k,4)==1VisSatNum=VisSatNum+1;SatellitePosNew=[SatellitePosNew; SatellitePosition(k,1:3)];endendPrange=ones(1,VisSatNum);for n=1:VisSatNumPrange(1,n)=sqrt((SatellitePosNew(n,:)-UserPosition)*(SatellitePosNew(n,:)-UserPosition))+c*DeltaT ;end3、定位计算函数function [CalUserPosition,CalculateOK]=CalculateUserPosition2(SatellitePosition,Prange)c=3e5; DeltaT=1e-4;VisSatNum=0;CalculateOK=1;SatellitePosNew=[];for k=1:24ifSatellitePosition(k,4)==1VisSatNum=VisSatNum+1;SatellitePosNew=[SatellitePosNew; SatellitePosition(k,1:3)];endendifVisSatNum4CalculateOK=0;CalUserPosition=[0 0 0];returnendXYZ0=[0 0 0];CalculateRecord=XYZ0;DeltaT0=0;Wxyz=SatellitePosNew;Error=1000;ComputeTime=0;while (Error0.01) (ComputeTime1000)ComputeTime=ComputeTime+1;R=ones(1,VisSatNum);for n=1:VisSatNumR(1,n)=sqrt( (Wxyz(n,:)-XYZ0) * (Wxyz(n,:)-XYZ0) );end%forDeltaP=R-Prange;A=ones(VisSatNum,3);for n=1:VisSatNumA(n,:)=(Wxyz(n,:)-XYZ0)./R(1,n);endH=[A ones(VisSat