//------------------------------------------------------------------------------ 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 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; }
//------------------------------------------------------------------------------ const std::vector<RealArray>& TDRSSTwoWayRange::CalculateMeasurementDerivatives( GmatBase *obj, Integer id) { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("TDRSSTwoWayRange::CalculateMeasurement" "Derivatives(%s, %d) called\n", obj->GetName().c_str(), id); #endif if (!initialized) InitializeMeasurement(); GmatBase *objPtr = NULL; Integer size = obj->GetEstimationParameterSize(id); Integer objNumber = -1; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" ParameterSize = %d\n", size); #endif if (size <= 0) throw MeasurementException("The derivative parameter on derivative " "object " + obj->GetName() + "is not recognized"); // Check to see if obj is a participant for (UnsignedInt i = 0; i < this->participants.size(); ++i) { if (participants[i] == obj) { objPtr = participants[i]; objNumber = i + 1; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Participant %s found\n", objPtr->GetName().c_str()); #endif break; } } // Or if it is the measurement model for this object if (obj->IsOfType(Gmat::MEASUREMENT_MODEL)) if (obj->GetRefObject(Gmat::CORE_MEASUREMENT, "") == this) { objPtr = obj; objNumber = 0; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" The measurement is the object\n", objPtr->GetName().c_str()); #endif } if (objNumber == -1) throw MeasurementException( "TDRSSTwoWayRange error - object is neither participant nor " "measurement model."); RealArray oneRow; oneRow.assign(size, 0.0); currentDerivatives.clear(); currentDerivatives.push_back(oneRow); Integer parameterID = GetParmIdFromEstID(id, obj); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Looking up id %d\n", parameterID); #endif if (objPtr != NULL) { if (objNumber == 1) // participant number 1, either a GroundStation or a Spacecraft { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 1\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Position") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " Position is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "Velocity") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " Velocity is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "CartesianX") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " CartesianState is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } else { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. something " "independent, so zero\n"); #endif for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 2) // participant 2, should be a TDRSS Spacecraft { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 1\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Position") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " Position is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "Velocity") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " Velocity is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "CartesianX") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() + " CartesianState is not yet implemented"); } else if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } else { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. something " "independent, so zero\n"); #endif for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 3) // participant 3, always a Spacecraft { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 3\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Position") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector3 forwardlinkRderiv; GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkRderiv, false, 1, 2, true, false); // Downlink leg Rvector3 backlinkRderiv; GetRangeDerivative(backlinkLeg, stmInv, backlinkRderiv, false, 1, 2, true, false); // Add 'em up per eq tbd for (Integer i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.5 * (forwardlinkRderiv[i] + backlinkRderiv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("Position Derivative: [%.12lf " "%.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2]); #endif } else if (objPtr->GetParameterText(parameterID) == "Velocity") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector3 forwardlinkVderiv; GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkVderiv, false, 1, 2, false); // Downlink leg Rvector3 backlinkVderiv; GetRangeDerivative(backlinkLeg, stmInv, backlinkVderiv, false, 1, 2, false); // Add 'em up per eq tbd for (Integer i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.5 * (forwardlinkVderiv[i] + backlinkVderiv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("Velocity Derivative: [%.12lf " "%.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2]); #endif } else if (objPtr->GetParameterText(parameterID) == "CartesianX") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector6 forwardlinkDeriv; GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkDeriv, false, 1, 2); // Downlink leg Rvector6 backlinkDeriv; GetRangeDerivative(backlinkLeg, stmInv, backlinkDeriv, false, 1, 2); // Add 'em up per eq tbd for (Integer i = 0; i < 6; ++i) currentDerivatives[0][i] = 0.5 * (forwardlinkDeriv[i] + backlinkDeriv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("CartesianState Derivative: " "[%.12lf %.12lf %.12lf %.12lf %.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2], currentDerivatives[0][3], currentDerivatives[0][4], currentDerivatives[0][5]); #endif } else if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } else { for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 0) // measurement model { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of the " "measurement model\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } } else { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of a non-" "Participant\n", objPtr->GetParameterText(parameterID).c_str()); #endif for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv =\n "); for (Integer i = 0; i < size; ++i) MessageInterface::ShowMessage(" %.12le",currentDerivatives[0][i]); MessageInterface::ShowMessage("\n"); #endif } return currentDerivatives; }
Variant _jobject_to_variant(JNIEnv * env, jobject obj) { jclass c = env->GetObjectClass(obj); bool array; String name = _get_class_name(env, c, &array); //print_line("name is " + name + ", array "+Variant(array)); if (name == "java.lang.String") { return String::utf8(env->GetStringUTFChars( (jstring)obj, NULL )); }; if (name == "[Ljava.lang.String;") { jobjectArray arr = (jobjectArray)obj; int stringCount = env->GetArrayLength(arr); //print_line("String array! " + String::num(stringCount)); DVector<String> sarr; for (int i=0; i<stringCount; i++) { jstring string = (jstring) env->GetObjectArrayElement(arr, i); const char *rawString = env->GetStringUTFChars(string, 0); sarr.push_back(String(rawString)); } return sarr; }; if (name == "java.lang.Boolean") { jmethodID boolValue = env->GetMethodID(c, "booleanValue", "()Z"); bool ret = env->CallBooleanMethod(obj, boolValue); return ret; }; if (name == "java.lang.Integer") { jclass nclass = env->FindClass("java/lang/Number"); jmethodID intValue = env->GetMethodID(nclass, "intValue", "()I"); int ret = env->CallIntMethod(obj, intValue); return ret; }; if (name == "[I") { jintArray arr = (jintArray)obj; int fCount = env->GetArrayLength(arr); DVector<int> sarr; sarr.resize(fCount); DVector<int>::Write w = sarr.write(); env->GetIntArrayRegion(arr,0,fCount,w.ptr()); w = DVector<int>::Write(); return sarr; }; if (name == "[B") { jbyteArray arr = (jbyteArray)obj; int fCount = env->GetArrayLength(arr); DVector<uint8_t> sarr; sarr.resize(fCount); DVector<uint8_t>::Write w = sarr.write(); env->GetByteArrayRegion(arr,0,fCount,reinterpret_cast<signed char*>(w.ptr())); w = DVector<uint8_t>::Write(); return sarr; }; if (name == "java.lang.Float" || name == "java.lang.Double") { jclass nclass = env->FindClass("java/lang/Number"); jmethodID doubleValue = env->GetMethodID(nclass, "doubleValue", "()D"); double ret = env->CallDoubleMethod(obj, doubleValue); return ret; }; if (name == "[D") { jdoubleArray arr = (jdoubleArray)obj; int fCount = env->GetArrayLength(arr); RealArray sarr; sarr.resize(fCount); RealArray::Write w = sarr.write(); for (int i=0; i<fCount; i++) { double n; env->GetDoubleArrayRegion(arr, i, 1, &n); w.ptr()[i] = n; }; return sarr; }; if (name == "[F") { jfloatArray arr = (jfloatArray)obj; int fCount = env->GetArrayLength(arr); RealArray sarr; sarr.resize(fCount); RealArray::Write w = sarr.write(); for (int i=0; i<fCount; i++) { float n; env->GetFloatArrayRegion(arr, i, 1, &n); w.ptr()[i] = n; }; return sarr; }; if (name == "[Ljava.lang.Object;") { jobjectArray arr = (jobjectArray)obj; int objCount = env->GetArrayLength(arr); Array varr; for (int i=0; i<objCount; i++) { jobject jobj = env->GetObjectArrayElement(arr, i); Variant v = _jobject_to_variant(env, jobj); varr.push_back(v); } return varr; }; if (name == "com.android.godot.Dictionary") { Dictionary ret; jclass oclass = c; jmethodID get_keys = env->GetMethodID(oclass, "get_keys", "()[Ljava/lang/String;"); jobjectArray arr = (jobjectArray)env->CallObjectMethod(obj, get_keys); StringArray keys = _jobject_to_variant(env, arr); jmethodID get_values = env->GetMethodID(oclass, "get_values", "()[Ljava/lang/Object;"); arr = (jobjectArray)env->CallObjectMethod(obj, get_values); Array vals = _jobject_to_variant(env, arr); //print_line("adding " + String::num(keys.size()) + " to Dictionary!"); for (int i=0; i<keys.size(); i++) { ret[keys[i]] = vals[i]; }; return ret; }; return Variant(); };
//------------------------------------------------------------------------------ void ExtendedKalmanInv::ComputeObs() { #ifdef DEBUG_ESTIMATION MessageInterface::ShowMessage("Computing obs and hTilde\n"); #endif // Compute the O-C, Htilde, and Kalman gain const MeasurementData *calculatedMeas = NULL; std::vector<RealArray> stateDeriv; const std::vector<ListItem*> *stateMap = esm.GetStateMap(); const ObservationData *currentObs = measManager.GetObsData(); hTilde.clear(); #ifdef DEBUG_ESTIMATION_DETAILS MessageInterface::ShowMessage("StateMap size is %d\n", stateMap->size()); #endif modelsToAccess = measManager.GetValidMeasurementList(); UnsignedInt rowCount; // Currently assuming uniqueness in models to access if (measManager.Calculate(modelsToAccess[0], true) >= 1) { calculatedMeas = measManager.GetMeasurement(modelsToAccess[0]); RealArray hTrow; hTrow.assign(stateSize, 0.0); rowCount = calculatedMeas->value.size(); measSize = currentObs->value.size(); #ifdef DEBUG_ESTIMATION if (rowCount != measSize) MessageInterface::ShowMessage("Mismatch between rowCount (%d) and " "measSize(%d)\n", rowCount, measSize); #endif for (UnsignedInt i = 0; i < rowCount; ++i) hTilde.push_back(hTrow); // Now walk the state vector and get elements of H-tilde for each piece for (UnsignedInt i = 0; i < stateSize; ++i) { if ((*stateMap)[i]->subelement == 1) { stateDeriv = measManager.CalculateDerivatives( (*stateMap)[i]->object, (*stateMap)[i]->elementID, modelsToAccess[0]); // Fill in the corresponding elements of hTilde for (UnsignedInt j = 0; j < rowCount; ++j) for (Integer k = 0; k < (*stateMap)[i]->length; ++k) hTilde[j][i+k] = stateDeriv[j][k]; } } } if (calculatedMeas == NULL) throw EstimatorException("No measurement was calculated!"); Real ocDiff; yi.clear(); for (UnsignedInt k = 0; k < measSize; ++k) { ocDiff = currentObs->value[k] - calculatedMeas->value[k]; measurementEpochs.push_back(currentEpoch); measurementResiduals.push_back(ocDiff); measurementResidualID.push_back(calculatedMeas->uniqueID); yi.push_back(ocDiff); #ifdef DEBUG_ESTIMATION MessageInterface::ShowMessage("*** Current O-C = %.12lf\n", ocDiff); #endif } if (currentObs->noiseCovariance == NULL) measCovariance = calculatedMeas->covariance; else measCovariance = currentObs->noiseCovariance; }
//------------------------------------------------------------------------------ const std::vector<RealArray>& DSNTwoWayRange::CalculateMeasurementDerivatives( GmatBase *obj, Integer id) { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("DSNTwoWayRange::CalculateMeasurement" "Derivatives(%s, %d) called\n", obj->GetName().c_str(), id); #endif if (!initialized) InitializeMeasurement(); GmatBase *objPtr = NULL; Integer size = obj->GetEstimationParameterSize(id); Integer objNumber = -1; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" ParameterSize = %d\n", size); #endif if (size <= 0) throw MeasurementException("The derivative parameter on derivative " "object " + obj->GetName() + "is not recognized"); // Check to see if obj is a participant for (UnsignedInt i = 0; i < this->participants.size(); ++i) { if (participants[i] == obj) { objPtr = participants[i]; objNumber = i + 1; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Participant %s found\n", objPtr->GetName().c_str()); #endif break; } } // Or if it is the measurement model for this object if (obj->IsOfType(Gmat::MEASUREMENT_MODEL)) if (obj->GetRefObject(Gmat::CORE_MEASUREMENT, "") == this) { objPtr = obj; objNumber = 0; #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" The measurement is the object\n", objPtr->GetName().c_str()); #endif } if (objNumber == -1) throw MeasurementException( "DSNTwoWayRange error - object is neither participant nor " "measurement model."); RealArray oneRow; oneRow.assign(size, 0.0); currentDerivatives.clear(); currentDerivatives.push_back(oneRow); Integer parameterID = GetParmIdFromEstID(id, obj); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Looking up id %d\n", parameterID); #endif if (objPtr != NULL) { if (objNumber == 1) // participant number 1, either a GroundStation or a Spacecraft { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 1\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Position") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() +" position is not yet implemented"); // CalculateRangeVectorInertial(); // Rvector3 tmp, result; // Rvector3 rangeUnit = rangeVecInertial.GetUnitVector(); // #ifdef DEBUG_DERIVATIVES // MessageInterface::ShowMessage(" RVInertial = %.12lf %.12lf %.12lf\n", // rangeVecInertial[0], rangeVecInertial[1], rangeVecInertial[2]); // MessageInterface::ShowMessage(" Unit RVInertial = %.12lf %.12lf %.12lf ", // rangeUnit[0], rangeUnit[1], rangeUnit[2]); // #endif // if (stationParticipant) // { // for (UnsignedInt i = 0; i < 3; ++i) // tmp[i] = - rangeUnit[i]; // // // for a Ground Station, need to rotate to the F1 frame // result = tmp * R_j2k_1; // for (UnsignedInt jj = 0; jj < 3; jj++) // currentDerivatives[0][jj] = result[jj]; // } // else // { // // for a spacecraft participant 1, we don't need the rotation matrices (I33) // for (UnsignedInt i = 0; i < 3; ++i) // currentDerivatives[0][i] = - rangeUnit[i]; // } } else if (objPtr->GetParameterText(parameterID) == "Velocity") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() +" velocity is not yet implemented"); // for (UnsignedInt i = 0; i < 3; ++i) // currentDerivatives[0][i] = 0.0; } else if (objPtr->GetParameterText(parameterID) == "CartesianX") { throw MeasurementException("Derivative w.r.t. " + participants[0]->GetName() +" Cartesian state is not yet " "implemented"); // CalculateRangeVectorInertial(); // Rvector3 tmp, result; // Rvector3 rangeUnit = rangeVecInertial.GetUnitVector(); // #ifdef DEBUG_DERIVATIVES // MessageInterface::ShowMessage(" RVInertial = %.12lf %.12lf %.12lf\n", // rangeVecInertial[0], rangeVecInertial[1], rangeVecInertial[2]); // MessageInterface::ShowMessage(" Unit RVInertial = %.12lf %.12lf %.12lf ", // rangeUnit[0], rangeUnit[1], rangeUnit[2]); // #endif // if (stationParticipant) // { // for (UnsignedInt i = 0; i < 3; ++i) // tmp[i] = - rangeUnit[i]; // // // for a Ground Station, need to rotate to the F1 frame // result = tmp * R_j2k_1; // for (UnsignedInt jj = 0; jj < 3; jj++) // currentDerivatives[0][jj] = result[jj]; // } // else // { // // for a spacecraft participant 1, we don't need the rotation matrices (I33) // for (UnsignedInt i = 0; i < 3; ++i) // currentDerivatives[0][i] = - rangeUnit[i]; // } // // velocity all zeroes // for (UnsignedInt ii = 3; ii < 6; ii++) // currentDerivatives[0][ii] = 0.0; } else if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } else { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. something " "independent, so zero\n"); #endif for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 2) // participant 2, always a Spacecraft { Real fFactor = GetFrequencyFactor(frequency) / (GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 2\n", objPtr->GetParameterText(parameterID).c_str()); MessageInterface::ShowMessage(" freq = %15lf, F = %15lf, F/c = " "%15lf\n", frequency, GetFrequencyFactor(frequency), fFactor); #endif if (objPtr->GetParameterText(parameterID) == "Position") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector3 uplinkRderiv; GetRangeDerivative(uplinkLeg, stmInv, uplinkRderiv, false, 0, 1, true, false); // Downlink leg Rvector3 downlinkRderiv; GetRangeDerivative(downlinkLeg, stmInv, downlinkRderiv, false, 0, 1, true, false); // Add 'em up per eq 7.80 for (Integer i = 0; i < 3; ++i) currentDerivatives[0][i] = fFactor * (uplinkRderiv[i] + downlinkRderiv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("Position Derivative: [%.12lf " "%.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2]); #endif } else if (objPtr->GetParameterText(parameterID) == "Velocity") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector3 uplinkVderiv; GetRangeDerivative(uplinkLeg, stmInv, uplinkVderiv, false, 0, 1, false); // Downlink leg Rvector3 downlinkVderiv; GetRangeDerivative(downlinkLeg, stmInv, downlinkVderiv, false, 0, 1, false); // Add 'em up per eq 7.81 for (Integer i = 0; i < 3; ++i) currentDerivatives[0][i] = fFactor * (uplinkVderiv[i] + downlinkVderiv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("Velocity Derivative: [%.12lf " "%.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2]); #endif } else if (objPtr->GetParameterText(parameterID) == "CartesianX") { // Get the inverse of the orbit STM at the measurement epoch // Will need adjustment if stm changes Rmatrix stmInv(6,6); GetInverseSTM(obj, stmInv); Rvector6 uplinkDeriv; GetRangeDerivative(uplinkLeg, stmInv, uplinkDeriv, false); // Downlink leg Rvector6 downlinkDeriv; GetRangeDerivative(downlinkLeg, stmInv, downlinkDeriv, false); // Add 'em up per eq 7.80 and 7.81 for (Integer i = 0; i < 6; ++i) currentDerivatives[0][i] = fFactor * (uplinkDeriv[i] + downlinkDeriv[i]); #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("CartesianState Derivative: " "[%.12lf %.12lf %.12lf %.12lf %.12lf %.12lf]\n", currentDerivatives[0][0], currentDerivatives[0][1], currentDerivatives[0][2], currentDerivatives[0][3], currentDerivatives[0][4], currentDerivatives[0][5]); #endif } else if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } else { for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 0) // measurement model { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of the " "measurement model\n", objPtr->GetParameterText(parameterID).c_str()); #endif if (objPtr->GetParameterText(parameterID) == "Bias") { for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 1.0; } } else { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of a non-" "Participant\n", objPtr->GetParameterText(parameterID).c_str()); #endif for (UnsignedInt i = 0; i < 3; ++i) currentDerivatives[0][i] = 0.0; } #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv =\n "); for (Integer i = 0; i < size; ++i) MessageInterface::ShowMessage(" %.12le",currentDerivatives[0][i]); MessageInterface::ShowMessage("\n"); #endif } return currentDerivatives; }
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; }
int LoadKappaZ (RealArray& kappa, RealArray& kappaEnergy, RealArray abundances) { // Get data directory from XSPEC xset variables string windtabsDirectory = getXspecVariable ("WINDTABSDIRECTORY", "./"); // Get filename from XSPEC xset variables string FITSfilename = windtabsDirectory + "/"; FITSfilename += getXspecVariable ("KAPPAZFILENAME", "kappa.fits"); // string KeywordNameMu ("mu"); // Set up load of 2D array kappaZ; // dimensions are Z and energyx RealArray kappaZ; // this gets appropriately resized when it is loaded size_t ax1 (0); size_t ax2 (0); try { auto_ptr<FITS> pInfile (new FITS (FITSfilename, Read, true)); // Primary HDU - Image PHDU& image = pInfile->pHDU (); image.read (kappaZ); // this is a 1D representation of a 2D array ax1 = image.axis (0); ax2 = image.axis (1); } catch (FitsException& issue) { cerr << "LoadKappaZ: CCfits / FITSio exception:" << endl; cerr << issue.message () << endl; cerr << "(failed reading KappaZ)" << endl; return 1; } // Load energy axis for kappa table: try { int extensionNumber (1); auto_ptr<FITS> pInfile (new FITS (FITSfilename, Read, extensionNumber, false)); ExtHDU& table = pInfile->currentExtension (); size_t NumberOfRows = table.column(1).rows (); table.column(1).read (kappaEnergy, 1, NumberOfRows); // table.column(2).read (kappaZ, 1, NumberOfRows); // see if this works??? } catch (FitsException& issue) { cerr << "LoadKappa: CCfits / FITSio exception:" << endl; cerr << issue.message () << endl; cerr << "(file probably doesn't exist)" << endl; return 1; } kappaEnergy *= 1.e-3; // convert from eV to keV // get mass fractions based on xspec abund, model abund parameter, // and atomic masses RealArray massFractions; getMassFractions (abundances, massFractions); // sum kappas weighted by mass fractions size_t NEnergies (kappaEnergy.size ()); size_t NZ (massFractions.size ()); kappa.resize (NEnergies, 0.); // It would be better if there was a vectorized way to do this, // but I don't know what it is. for (size_t i=0; i<NZ; i++) { for (size_t j=0; j<NEnergies; j++){ size_t k = i*ax1 + j; // this is the 1d representation of the 2d array kappa[j] += kappaZ[k] * massFractions[i]; } } return 0; }
void KaiserWindowDesigner::getWindow(RealArray &w) { w.resize(size); Real beta = getBeta(); calculateWindow(w, beta); }