00001 #pragma ident "$Id: Xvt.cpp 70 2006-08-01 18:36:21Z ehagen $" 00002 00003 00004 00005 //============================================================================ 00006 // 00007 // This file is part of GPSTk, the GPS Toolkit. 00008 // 00009 // The GPSTk is free software; you can redistribute it and/or modify 00010 // it under the terms of the GNU Lesser General Public License as published 00011 // by the Free Software Foundation; either version 2.1 of the License, or 00012 // any later version. 00013 // 00014 // The GPSTk is distributed in the hope that it will be useful, 00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 // GNU Lesser General Public License for more details. 00018 // 00019 // You should have received a copy of the GNU Lesser General Public 00020 // License along with GPSTk; if not, write to the Free Software Foundation, 00021 // Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00022 // 00023 // Copyright 2004, The University of Texas at Austin 00024 // 00025 //============================================================================ 00026 00027 //============================================================================ 00028 // 00029 //This software developed by Applied Research Laboratories at the University of 00030 //Texas at Austin, under contract to an agency or agencies within the U.S. 00031 //Department of Defense. The U.S. Government retains all rights to use, 00032 //duplicate, distribute, disclose, or release this software. 00033 // 00034 //Pursuant to DoD Directive 523024 00035 // 00036 // DISTRIBUTION STATEMENT A: This software has been approved for public 00037 // release, distribution is unlimited. 00038 // 00039 //============================================================================= 00040 00041 00042 00043 00044 00045 00051 #include "GeoidModel.hpp" 00052 #include "Xvt.hpp" 00053 00054 namespace gpstk 00055 { 00056 00057 /* 00058 * Function to find the range and position from a ground 00059 * location, rxPos, to the spacecraft position (*this).x. 00060 * 00061 * Use the pseudorange corrected for SV clock effects to get a 00062 * rough time of flight (dt). Account directly for Earth 00063 * rotation, then compute a rough receiver bias by differencing 00064 * the initial time of flight with the new estimate. Then 00065 * correct the rotation by a small amount. 00066 */ 00067 double Xvt :: preciseRho(const ECEF& rxPos, 00068 const GeoidModel& geoid, 00069 double correction) const 00070 throw() 00071 { 00072 // Compute initial time of flight estimate using the 00073 // geometric range at transmit time. This fails to account 00074 // for the rotation of the earth, but should be good to 00075 // within about 40 m 00076 double sr1 = rxPos.slantRange(x); 00077 double dt = sr1 / geoid.c(); 00078 00079 // compute rotation angle in the time of signal transit 00080 double rotation_angle = -geoid.angVelocity() * dt; 00081 00082 // rotate original GS coordinates to new values to correct for 00083 // rotation of ECEF frame 00084 // Ref: Satellite Geodesy, Gunter Seeber, 1993, pg 291 and the 00085 // ICD-GPS-200 sheet 102 May 1993 version 00086 // xnew[0]=xg[0]*cos(rotation_angle)-xg[1]*sin(rotation_angle); 00087 // xnew[1]=xg[1]*cos(rotation_angle)+xg[0]*sin(rotation_angle); 00088 // xnew[2]=xg[2]; 00089 // since cosine and sine are small, approximate by the first 00090 // order terms in an expansion. 00091 Triple xnew; 00092 for (int i = 0; i < 2; i++) 00093 { 00094 xnew[0] = x[0] - x[1] * rotation_angle; 00095 xnew[1] = x[1] + x[0] * rotation_angle; 00096 xnew[2] = x[2]; 00097 00098 // Compute geometric slant range from ground station to 00099 // the rotated new coord's 00100 sr1 = rxPos.slantRange(xnew); 00101 00102 // Recompute the time of flight (dt) based on PR, with the 00103 // time of flight based on geometric range. Note that 00104 // this is a really unneeded, in that the change in PR is 00105 // < 40 m, hence the change in tof is < 20 ns 00106 dt = sr1 / geoid.c(); 00107 00108 // Compute new rotation in this time 00109 rotation_angle = -geoid.angVelocity() * dt; 00110 } 00111 // Account for SV clock drift and other factors 00112 double rho = sr1 - (dtime * geoid.c()) - correction; 00113 return rho; 00114 } // end of preciseRho() 00115 } 00116 00117 std::ostream& operator<<( std::ostream& s, 00118 const gpstk::Xvt& xvt ) 00119 { 00120 s << "x:" << xvt.x 00121 << ", v:" << xvt.v 00122 << ", clk bias:" << xvt.dtime 00123 << ", clk drift:" << xvt.ddtime; 00124 return s; 00125 }
1.3.9.1