//------------------------------------------------------------------------ RealArray PhysicalMeasurement::TroposphereCorrection(Real freq, Rvector3 rVec, Rmatrix Ro_j2k) { RealArray tropoCorrection; if (troposphere != NULL) { Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6); troposphere->SetWaveLength(wavelength); Real elevationAngle = asin((Ro_j2k*rVec.GetUnitVector()).GetElement(2)); troposphere->SetElevationAngle(elevationAngle); troposphere->SetRange(rVec.GetMagnitude()*GmatMathConstants::KM_TO_M); tropoCorrection = troposphere->Correction(); // Real rangeCorrection = tropoCorrection[0]/GmatMathConstants::KM_TO_M; #ifdef DEBUG_RANGE_CALC_WITH_EVENTS MessageInterface::ShowMessage(" Apply Troposphere media correction:\n"); MessageInterface::ShowMessage(" .Wave length = %.12lf m\n", wavelength); MessageInterface::ShowMessage(" .Elevation angle = %.12lf degree\n", elevationAngle*GmatMathConstants::DEG_PER_RAD); MessageInterface::ShowMessage(" .Range correction = %.12lf m\n", tropoCorrection[0]); #endif } else { tropoCorrection.push_back(0.0); tropoCorrection.push_back(0.0); tropoCorrection.push_back(0.0); } return tropoCorrection; }
bool ASMs1D::getGridParameters (RealArray& prm, int nSegPerSpan) const { if (!curv) return false; if (nSegPerSpan < 1) { std::cerr <<" *** ASMs1D::getGridParameters: Too few knot-span points " << nSegPerSpan+1 << std::endl; return false; } RealArray::const_iterator uit = curv->basis().begin(); double ucurr = 0.0, uprev = *(uit++); while (uit != curv->basis().end()) { ucurr = *(uit++); if (ucurr > uprev) if (nSegPerSpan == 1) prm.push_back(uprev); else for (int i = 0; i < nSegPerSpan; i++) { double xg = (double)(2*i-nSegPerSpan)/(double)nSegPerSpan; prm.push_back(0.5*(ucurr*(1.0+xg) + uprev*(1.0-xg))); } uprev = ucurr; } if (ucurr > prm.back()) prm.push_back(ucurr); return true; }
bool ASMs1DSpec::getGridParameters (RealArray& prm, int nSegPerSpan) const { if (!curv) return false; // Evaluate the Gauss-Lobatto-Legendre points Vector dummy, xGLL; if (!Legendre::GLL(dummy,xGLL,curv->order())) return false; if (xGLL.size() != (size_t)(nSegPerSpan+1)) { nSegPerSpan = xGLL.size() - 1; std::cout <<"Spectral elements: Number of nodes per knot-span reset to " << nSegPerSpan <<" (GLL points)"<< std::endl; } RealArray::const_iterator uit = curv->basis().begin(); double ucurr, uprev = *(uit++); while (uit != curv->basis().end()) { ucurr = *(uit++); if (ucurr > uprev) for (int i = 1; i <= nSegPerSpan; i++) prm.push_back(0.5*(ucurr-uprev)*(1.0+xGLL(i)) + uprev); uprev = ucurr; } prm.push_back(curv->basis().endparam()); return true; }
bool ASMs2DSpec::getGridParameters (RealArray& prm, int dir, int nSegPerSpan) const { if (!surf) return false; // Evaluate the Gauss-Lobatto-Legendre points in this direction Vector dummy, xGLL; int p = dir == 0 ? surf->order_u() : surf->order_v(); if (!Legendre::GLL(dummy,xGLL,p)) return false; if (xGLL.size() != (size_t)(nSegPerSpan+1)) { nSegPerSpan = xGLL.size() - 1; std::cout <<"Spectral elements: Number of nodes per knot-span in " << char('u'+dir) <<"-directon reset to "<< nSegPerSpan <<" (GLL points)"<< std::endl; } RealArray::const_iterator uit = surf->basis(dir).begin(); double uprev = *(uit++); while (uit != surf->basis(dir).end()) { double ucurr = *(uit++); if (ucurr > uprev) for (int i = 1; i <= nSegPerSpan; i++) prm.push_back(0.5*(ucurr-uprev)*(1.0+xGLL(i)) + uprev); uprev = ucurr; } prm.push_back(surf->basis(dir).endparam()); return true; }
bool ASMs1D::refine (const RealArray& xi) { if (!curv || xi.empty()) return false; if (xi.front() < 0.0 || xi.back() > 1.0) return false; if (shareFE) return true; RealArray extraKnots; RealArray::const_iterator uit = curv->basis().begin(); double ucurr, uprev = *(uit++); while (uit != curv->basis().end()) { ucurr = *(uit++); if (ucurr > uprev) for (size_t i = 0; i < xi.size(); i++) if (i > 0 && xi[i] < xi[i-1]) return false; else extraKnots.push_back(ucurr*xi[i] + uprev*(1.0-xi[i])); uprev = ucurr; } curv->insertKnot(extraKnots); return true; }
//------------------------------------------------------------------------------ void LocatedEventTable::CollectData(const wxString &forCurve, RealArray &xv, RealArray &yv) { xv.clear(); yv.clear(); for (UnsignedInt i = 0; i < events.size(); ++i) { if ((events[i]->dataName == forCurve) && (events[i]->partner != NULL)) { if (events[i]->isEntry) { xv.push_back(events[i]->epoch); yv.push_back(events[i]->duration); } } } }
bool ASMs2D::getQuasiInterplParameters (RealArray& prm, int dir) const { if (!surf || dir < 0 || dir > 1) return false; const Go::BsplineBasis& basis = surf->basis(dir); RealArray knots_simple; basis.knotsSimple(knots_simple); prm.clear(); for (size_t i = 0; i+1 < knots_simple.size(); i++) { prm.push_back(knots_simple[i]); prm.push_back(0.5*(knots_simple[i]+knots_simple[i+1])); } prm.push_back(knots_simple.back()); return true; }
//--------------------------------------------------------------------------- // RealArray Ionosphere::Correction() // This function is used to calculate Ionosphere correction // Return values: // . Range correction (unit: m) // . Angle correction (unit: radian) // . Time correction (unit: s) //--------------------------------------------------------------------------- RealArray Ionosphere::Correction() { Real freq = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / waveLength; Real tec = TEC(); // Equation 6.70 of MONTENBRUCK and GILL Real drho = 40.3*tec/(freq*freq); // Equation 6.69 of MONTENBRUCK and GILL // unit: meter Real dphi = 0; //BendingAngle()*180/GmatConstants::PI; // It has not been defined yet Real dtime = drho/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM; // unit: s //MessageInterface::ShowMessage // ("Ionosphere::Correction: freq = %.12lf MHz, tec = %.12lfe16, " // "drho = %.12lfm, dphi = %f, dtime = %.12lfs\n", freq/1.0e6, // tec/1.0e16, drho, dphi*3600, dtime); RealArray ra; ra.push_back(drho); ra.push_back(dphi); ra.push_back(dtime); return ra; }
bool ASMu2D::getGrevilleParameters (RealArray& prm, int dir) const { if (!lrspline || dir < 0 || dir > 1) return false; prm.clear(); prm.reserve(lrspline->nBasisFunctions()); for(LR::Basisfunction *b : lrspline->getAllBasisfunctions()) prm.push_back(b->getGrevilleParameter()[dir]); return true; }
bool SplineField2D::valueGrid (RealArray& val, const int* npe) const { val.clear(); if (!basis) return false; // Compute parameter values of the visualization points std::array<RealArray,2> gpar; for (int dir = 0; dir < 2; dir++) { int nSegPerSpan = npe[dir] - 1; if (nSegPerSpan < 1) return false; RealArray::const_iterator uit = basis->basis(dir).begin(); double ucurr = 0.0, uprev = *(uit++); while (uit != basis->basis(dir).end()) { ucurr = *(uit++); if (ucurr > uprev) if (nSegPerSpan == 1) gpar[dir].push_back(uprev); else for (int i = 0; i < nSegPerSpan; i++) { double xg = (double)(2*i-nSegPerSpan)/(double)nSegPerSpan; gpar[dir].push_back(0.5*(ucurr*(1.0+xg) + uprev*(1.0-xg))); } uprev = ucurr; } if (ucurr > gpar[dir].back()) gpar[dir].push_back(ucurr); } // Evaluate the field in the visualization points val.reserve(gpar[0].size()*gpar[1].size()); for (size_t j = 0; j < gpar[1].size(); j++) for (size_t i = 0; i < gpar[0].size(); i++) { Go::BasisPtsSf spline; basis->computeBasis(gpar[0][i],gpar[1][j],spline); IntVec ip; ASMs2D::scatterInd(basis->numCoefs_u(),basis->numCoefs_v(), basis->order_u(),basis->order_v(), spline.left_idx,ip); Vector Vnod; utl::gather(ip,1,values,Vnod); val.push_back(Vnod.dot(spline.basisValues)); } return true; }
void FieldsFuncBase::addPatchField (ASMbase* pch, const RealArrays& coefs) { if (coefs.size() == 1) field.push_back(Fields::create(pch,coefs.front(),1,pch->getNoFields(1))); else if (coefs.size() > 1) { RealArray coef; coef.reserve(coefs.size()*coefs.front().size()); for (size_t i = 0; i < coefs.front().size(); i++) for (size_t j = 0; j < coefs.size(); j++) coef.push_back(coefs[j][i]); field.push_back(Fields::create(pch,coef,1,pch->getNoFields(1))); } }
bool ASMs1D::uniformRefine (int nInsert) { if (!curv || nInsert < 1) return false; if (shareFE) return true; RealArray extraKnots; RealArray::const_iterator uit = curv->basis().begin(); double ucurr, uprev = *(uit++); while (uit != curv->basis().end()) { ucurr = *(uit++); if (ucurr > uprev) for (int i = 0; i < nInsert; i++) { double xi = (double)(i+1)/(double)(nInsert+1); extraKnots.push_back(ucurr*xi + uprev*(1.0-xi)); } uprev = ucurr; } curv->insertKnot(extraKnots); return true; }
//------------------------------------------------------------------------------ RealArray Troposphere::Correction() { // Determine Re value if (!solarSystem) { MessageInterface::ShowMessage("Troposphere::Correction: Solar System is NULL; Cannot obtain Earth radius\n"); throw MeasurementException("Troposphere::Correction: Solar System is NULL; Cannot obtain Earth radius\n"); } CelestialBody *earth= solarSystem->GetBody(GmatSolarSystemDefaults::EARTH_NAME); if (!earth) { MessageInterface::ShowMessage("Troposphere::Correction: Cannot obtain Earth radius\n"); throw MeasurementException("Troposphere::Correction: Cannot obtain Earth radius\n"); } Real Re = earth->GetEquatorialRadius()*GmatMathConstants::KM_TO_M; // get Erath radius in meters #ifdef DEBUG_TROPOSPHERE_CORRECTION MessageInterface::ShowMessage("Troposphere::Correction():\n"); MessageInterface::ShowMessage(" temperature = %f K , pressure = %f hPa, humidity = %f\n", temperature, pressure, humidityFraction); MessageInterface::ShowMessage(" range = %lfm , elevationAngle = %lf radian, waveLenght = %lfm\n", range, elevationAngle, waveLength); MessageInterface::ShowMessage(" earth radius = %lf m\n",Re); #endif // Specify Ce and Crho: double lambda = waveLength; double lp2_inv = 1.0 / ((lambda * 1.0E+06)*(lambda * 1.0E+06)); double denom = (173.3 - lp2_inv); double term1 = 170.2649 / denom; double Ce = term1*term2; double term3 = (173.3 + lp2_inv)/denom; double Crho = Ce * term3; #ifdef DEBUG_TROPOSPHERE_CORRECTION MessageInterface::ShowMessage(" Ce = %lf , Crho = %lf\n", Ce, Crho); #endif // Specify inputs: double p = pressure; double T = temperature; double fh = humidityFraction; double E = elevationAngle; double rho = range; // refractivities double N[2]; // compute dry component refractivity N[0] = 77.624 * p / T; // compute wet component refractivity double Tc = T + GmatPhysicalConstants::ABSOLUTE_ZERO_C; double e = 6.10 * fh * exp(17.15 * Tc /(234.7 + Tc)); N[1] = 371900.0 * e / (T*T) - 12.92 * e/T; // troposphere heights double h[2]; // compute dry troposphere height h[0] = 5.0 * 0.002277 * p / (N[0] * 1.0E-06); // compute wet troposphere height h[1] = 5.0 * 0.002277 * e * (1255.0/T + 0.05) / (N[1] * 1.0E-06); // distances to top of the troposphere double r[2]; double alpha[9][2]; double beta[7][2]; double cosE = cos(E); double cosE2 = cosE * cosE; double sinE = sin(E); for (int j = 0; j < 2; j++) { r[j] = sqrt((Re + h[j])*(Re + h[j]) - (Re*Re*cosE2)) - Re*sinE; double aj = -1.0 * sinE/h[j]; double bj = - 1.0 * cosE2/(2.0 * h[j] * Re); alpha[0][j] = 1.0; alpha[1][j] = 4.0*aj; alpha[2][j] = 6.0*aj*aj + 4.0*bj; alpha[3][j] = 4.0*aj*(aj*aj + 3.0*bj); alpha[4][j] = pow(aj, 4) + 12.0*aj*aj*bj + 6.0*bj*bj; alpha[5][j] = 4.0*aj*bj*(aj*aj + 3.0*bj); alpha[6][j] = bj*bj*(6.0*aj*aj + 4.0*bj); alpha[7][j] = 4.0 * aj * bj*bj*bj; alpha[8][j] = pow(bj,4); beta[0][j] = 1.0; beta[1][j] = 3.0*aj; beta[2][j] = 3.0*(aj*aj + bj); beta[3][j] = aj*(aj*aj + 6.0*bj); beta[4][j] = 3.0*bj*(aj*aj + bj); beta[5][j] = 3.0 * aj * bj*bj; beta[6][j] = pow(bj,3); } double drho = 0.0; double dE = 0.0; for (int j = 0; j < 2; j++) { double sum1 = 0.0; for (int i = 0; i < 9; i++) { double ii = (double)i; double temp1 = alpha[i][j]*pow(r[j],(i+1))/(ii+1.0); sum1 = sum1 + temp1; } double sum2 = 0.0; for (int k = 0; k < 7; k++) { double kk = (double)k; double temp2 = beta[k][j]*pow(r[j],(k+2))/((kk+1.0)*(kk+2.0)) + beta[k][j]*pow(r[j],(k+1))*(rho - r[j])/(kk+1); sum2 = sum2 + temp2; } drho = drho + N[j] * 1.0E-06 * sum1; dE = dE + N[j] * 1.0E-06 * sum2 / h[j]; } drho = Crho * drho; dE = Ce * 4.0 * cosE * dE/ rho; dE = dE / GmatMathConstants::RAD_PER_ARCSEC; RealArray out; out.push_back(drho); out.push_back(dE); out.push_back(drho/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM); #ifdef DEBUG_TROPOSPHERE_CORRECTION MessageInterface::ShowMessage(" Troposphere correction result:\n"); MessageInterface::ShowMessage(" Range correction = %f m\n", drho); MessageInterface::ShowMessage(" Elevation angle correction = %f arcsec", dE); MessageInterface::ShowMessage(" Time correction = %f sec\n", drho/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM); #endif return out; }
//------------------------------------------------------------------------------ void Estimator::PlotResiduals() { #ifdef DEBUG_RESIDUAL_PLOTS MessageInterface::ShowMessage("Entered PlotResiduals\n"); MessageInterface::ShowMessage("Processing plot with %d Residuals\n", measurementResiduals.size()); #endif std::vector<RealArray*> dataBlast; RealArray epochs; RealArray values; RealArray hiErrors; RealArray lowErrors; RealArray *hi = NULL, *low = NULL; for (UnsignedInt i = 0; i < residualPlots.size(); ++i) { dataBlast.clear(); epochs.clear(); values.clear(); if (showErrorBars) { hiErrors.clear(); lowErrors.clear(); if (hiLowData.size() > 0) { hi = hiLowData[0]; if (hiLowData.size() > 1) { low = hiLowData[1]; } } } // Collect residuals by plot for (UnsignedInt j = 0; j < measurementResiduals.size(); ++j) { if (residualPlots[i]->UsesData(measurementResidualID[j]) >= 0) { epochs.push_back(measurementEpochs[j]); values.push_back(measurementResiduals[j]); if (hi && showErrorBars) { hiErrors.push_back((*hi)[j]); } if (low && showErrorBars) lowErrors.push_back((*low)[j]); } } if (epochs.size() > 0) { dataBlast.push_back(&epochs); dataBlast.push_back(&values); residualPlots[i]->TakeAction("ClearData"); residualPlots[i]->Deactivate(); residualPlots[i]->SetData(dataBlast, hiErrors, lowErrors); residualPlots[i]->TakeAction("Rescale"); residualPlots[i]->Activate(); } #ifdef DEBUG_RESIDUALS // Dump the data to screen MessageInterface::ShowMessage("DataDump for residuals plot %d:\n", i); for (UnsignedInt k = 0; k < epochs.size(); ++k) { MessageInterface::ShowMessage(" %.12lf %.12lf", epochs[k], values[k]); if (hi) if (hi->size() > k) if (low) MessageInterface::ShowMessage(" + %.12lf", (*hi)[k]); else MessageInterface::ShowMessage(" +/- %.12lf", (*hi)[k]); if (low) if (low->size() > k) MessageInterface::ShowMessage(" - %.12lf", (*low)[k]); MessageInterface::ShowMessage("\n"); } #endif } }
RealArray PhysicalMeasurement::IonosphereCorrection(Real freq, Rvector3 r1, Rvector3 r2, Real epoch) { RealArray ionoCorrection; if (ionosphere != NULL) { // 1. Set wave length: Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6); // unit: meter ionosphere->SetWaveLength(wavelength); // 2. Set time: ionosphere->SetTime(epoch); // unit: Julian day // 3. Set station and spacecraft positions: GroundstationInterface* gs = (GroundstationInterface*)participants[0]; CoordinateSystem* cs = gs->GetBodyFixedCoordinateSystem(); Rvector inState(6, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); Rvector outState(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // Rvector bogusOut = cs->FromMJ2000Eq(epoch, bogusIn, true); // Rmatrix33 R_g_j2k = cs->GetLastRotationMatrix().Transpose(); CoordinateConverter* cv = new CoordinateConverter(); A1Mjd time(epoch); CoordinateSystem* fk5cs = Moderator::Instance()->GetCoordinateSystem("EarthMJ2000Eq"); cv->Convert(time, inState, cs, outState, fk5cs); // convert EarthFK5 coordinate system to EarthBodyFixed coordinate system Rmatrix33 R_g_j2k = cv->GetLastRotationMatrix().Transpose(); // MessageInterface::ShowMessage("[ %f %f %f\n", R_g_j2k.GetElement(0,0), R_g_j2k.GetElement(0,1), R_g_j2k.GetElement(0,2)); // MessageInterface::ShowMessage(" %f %f %f\n", R_g_j2k.GetElement(1,0), R_g_j2k.GetElement(1,1), R_g_j2k.GetElement(1,2)); // MessageInterface::ShowMessage(" %f %f %f]\n", R_g_j2k.GetElement(2,0), R_g_j2k.GetElement(2,1), R_g_j2k.GetElement(2,2)); ionosphere->SetStationPosition(R_g_j2k*r1); // unit: km ionosphere->SetSpacecraftPosition(R_g_j2k*r2); // unit: km // 4. Set earth radius: SpacePoint* earth = (SpacePoint*)gs->GetRefObject(Gmat::SPACE_POINT, "Earth"); Real earthRadius = earth->GetRealParameter("EquatorialRadius"); ionosphere->SetEarthRadius(earthRadius); // unit: km #ifdef DEBUG_IONOSPHERE_MEDIA_CORRECTION MessageInterface::ShowMessage(" *Run Ionosphere media correction for:\n"); MessageInterface::ShowMessage(" +Earth radius = %lf km\n", earthRadius); MessageInterface::ShowMessage(" +Wave length = %.12lf m\n", wavelength); MessageInterface::ShowMessage(" +Time = %.12lf\n", epoch); MessageInterface::ShowMessage(" +Station location in Earth body fixed coordinate system (km):\n" " (%s)", (R_g_j2k*r1).ToString().c_str()); MessageInterface::ShowMessage(" +Spacecraft location in Earth body fixed coordinate system (km):\n" " (%s)", (R_g_j2k*r2).ToString().c_str()); #endif // 5. Run ionosphere correction: ionoCorrection = ionosphere->Correction(); Real rangeCorrection = ionoCorrection[0]*GmatMathConstants::M_TO_KM; // unit: meter #ifdef DEBUG_IONOSPHERE_MEDIA_CORRECTION MessageInterface::ShowMessage(" *Ionosphere media correction result:\n"); MessageInterface::ShowMessage(" +Range correction = %.12lf m\n", rangeCorrection*GmatMathConstants::KM_TO_M); #endif } else { ionoCorrection.push_back(0.0); ionoCorrection.push_back(0.0); ionoCorrection.push_back(0.0); } return ionoCorrection; }