1 //============================================================================== 2 // 3 // This file is part of GPSTk, the GPS Toolkit. 4 // 5 // The GPSTk is free software; you can redistribute it and/or modify 6 // it under the terms of the GNU Lesser General Public License as published 7 // by the Free Software Foundation; either version 3.0 of the License, or 8 // any later version. 9 // 10 // The GPSTk is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 // GNU Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with GPSTk; if not, write to the Free Software Foundation, 17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA 18 // 19 // This software was developed by Applied Research Laboratories at the 20 // University of Texas at Austin. 21 // Copyright 2004-2020, The Board of Regents of The University of Texas System 22 // 23 //============================================================================== 24 25 //============================================================================== 26 // 27 // This software was developed by Applied Research Laboratories at the 28 // University of Texas at Austin, under contract to an agency or agencies 29 // within the U.S. Department of Defense. The U.S. Government retains all 30 // rights to use, duplicate, distribute, disclose, or release this software. 31 // 32 // Pursuant to DoD Directive 523024 33 // 34 // DISTRIBUTION STATEMENT A: This software has been approved for public 35 // release, distribution is unlimited. 36 // 37 //============================================================================== 38 39 /** 40 * @file Xvt.cpp 41 * Position and velocity as Triples, clock bias and drift as doubles. 42 */ 43 44 #include <iostream> 45 #include "Xvt.hpp" 46 47 namespace gpstk 48 { 49 operator <<(std::ostream & os,const gpstk::Xvt & xvt)50 std::ostream& operator<<(std::ostream& os, const gpstk::Xvt& xvt) throw() 51 { 52 os << "x:" << xvt.x 53 << ", v:" << xvt.v 54 << ", clk bias:" << xvt.clkbias 55 << ", clk drift:" << xvt.clkdrift 56 << ", relcorr:" << xvt.relcorr 57 << ", health:" << xvt.health; 58 return os; 59 } 60 operator <<(std::ostream & os,const Xvt::HealthStatus & health)61 std::ostream& operator<<(std::ostream& os, const Xvt::HealthStatus& health) 62 throw() 63 { 64 switch (health) 65 { 66 case Xvt::Uninitialized: 67 os << "Uninitialized"; 68 break; 69 case Xvt::Unavailable: 70 os << "Unavailable"; 71 break; 72 case Xvt::Unused: 73 os << "Unused"; 74 break; 75 case Xvt::Unknown: 76 os << "Unknown"; 77 break; 78 case Xvt::Unhealthy: 79 os << "Unhealthy"; 80 break; 81 case Xvt::Degraded: 82 os << "Degraded"; 83 break; 84 case Xvt::Healthy: 85 os << "Healthy"; 86 break; 87 default: 88 os << "???"; 89 break; 90 } 91 return os; 92 } 93 94 // compute the relativity correction computeRelativityCorrection(void)95 double Xvt::computeRelativityCorrection(void) 96 { 97 relcorr = -2.0*( (x[0]/C_MPS)*(v[0]/C_MPS) 98 +(x[1]/C_MPS)*(v[1]/C_MPS) 99 +(x[2]/C_MPS)*(v[2]/C_MPS) ); 100 return relcorr; 101 } 102 103 // Function to find the range and position from a ground 104 // location, rxPos, to the spacecraft position (*this).x 105 // Use the pseudorange corrected for SV clock effects to get a 106 // rough time of flight (dt). Account directly for Earth 107 // rotation, then compute a rough receiver bias by differencing 108 // the initial time of flight with the new estimate. Then 109 // correct the rotation by a small amount. preciseRho(const Triple & rxPos,const EllipsoidModel & ellips,double correction) const110 double Xvt::preciseRho(const Triple& rxPos, 111 const EllipsoidModel& ellips, 112 double correction) const 113 throw() 114 { 115 // Compute initial time of flight estimate using the 116 // geometric range at transmit time. This fails to account 117 // for the rotation of the earth, but should be good to 118 // within about 40 m 119 double sr1 = rxPos.slantRange(x); 120 double dt = sr1 / ellips.c(); 121 122 // compute rotation angle in the time of signal transit 123 double rotation_angle = -ellips.angVelocity() * dt; 124 125 // rotate original GS coordinates to new values to correct for 126 // rotation of ECEF frame 127 // Ref: Satellite Geodesy, Gunter Seeber, 1993, pg 291 and the 128 // ICD-GPS-200 sheet 102 May 1993 version 129 // xnew[0]=xg[0]*cos(rotation_angle)-xg[1]*sin(rotation_angle); 130 // xnew[1]=xg[1]*cos(rotation_angle)+xg[0]*sin(rotation_angle); 131 // xnew[2]=xg[2]; 132 // since cosine and sine are small, approximate by the first 133 // order terms in an expansion. 134 gpstk::Triple xnew; 135 for (int i = 0; i < 2; i++) 136 { 137 xnew[0] = x[0] - x[1] * rotation_angle; 138 xnew[1] = x[1] + x[0] * rotation_angle; 139 xnew[2] = x[2]; 140 141 // Compute geometric slant range from ground station to 142 // the rotated new coord's 143 sr1 = rxPos.slantRange(xnew); 144 145 // Recompute the time of flight (dt) based on PR, with the 146 // time of flight based on geometric range. Note that 147 // this is a really unneeded, in that the change in PR is 148 // < 40 m, hence the change in tof is < 20 ns 149 dt = sr1 / ellips.c(); 150 151 // Compute new rotation in this time 152 rotation_angle = -ellips.angVelocity() * dt; 153 } 154 155 // Account for SV clock drift, relativity and user input correction 156 double rho = sr1 - (clkbias + relcorr) * ellips.c() - correction; 157 158 return rho; 159 160 } // end of preciseRho() 161 162 } // end of gpstk namespace 163