bool ASMs1D::evaluate (const RealFunc* func, RealArray& values, int, double time) const { Go::SplineCurve* scrv = SplineUtils::project(curv,*func,time); if (!scrv) { std::cerr <<" *** ASMs1D::evaluate: Projection failure."<< std::endl; return false; } values.assign(scrv->coefs_begin(),scrv->coefs_end()); delete scrv; return true; }
bool ASMs2D::evaluate (const ASMbase* basis, const Vector& locVec, RealArray& vec, int basisNum) const { const ASMs2D* pch = dynamic_cast<const ASMs2D*>(basis); if (!pch) return false; // Compute parameter values of the result sampling points (Greville points) std::array<RealArray,2> gpar; for (int dir = 0; dir < 2; dir++) if (!this->getGrevilleParameters(gpar[dir],dir,basisNum)) return false; // Evaluate the result field at all sampling points. // Note: it is here assumed that *basis and *this have spline bases // defined over the same parameter domain. Matrix sValues; if (!pch->evalSolution(sValues,locVec,gpar.data())) return false; Go::SplineSurface* surf = this->getBasis(basisNum); // Project the results onto the spline basis to find control point // values based on the result values evaluated at the Greville points. // Note that we here implicitly assume that the number of Greville points // equals the number of control points such that we don't have to resize // the result array. Think that is always the case, but beware if trying // other projection schemes later. RealArray weights; if (surf->rational()) surf->getWeights(weights); const Vector& vec2 = sValues; Go::SplineSurface* surf_new = Go::SurfaceInterpolator::regularInterpolation(surf->basis(0), surf->basis(1), gpar[0], gpar[1], const_cast<Vector&>(vec2), sValues.rows(), surf->rational(), weights); vec.assign(surf_new->coefs_begin(),surf_new->coefs_end()); delete surf_new; return true; }
bool ASMs2D::evaluate (const Field* field, RealArray& vec, int basisNum) const { // Compute parameter values of the result sampling points (Greville points) std::array<RealArray,2> gpar; for (int dir = 0; dir < 2; dir++) if (!this->getGrevilleParameters(gpar[dir],dir,basisNum)) return false; // Evaluate the result field at all sampling points. // Note: it is here assumed that *basis and *this have spline bases // defined over the same parameter domain. Vector sValues(gpar[0].size()*gpar[1].size()); Vector::iterator it=sValues.begin(); for (size_t j=0;j<gpar[1].size();++j) { FiniteElement fe; fe.v = gpar[1][j]; for (size_t i=0;i<gpar[0].size();++i) { fe.u = gpar[0][i]; *it++ = field->valueFE(fe); } } Go::SplineSurface* surf = this->getBasis(basisNum); // Project the results onto the spline basis to find control point // values based on the result values evaluated at the Greville points. // Note that we here implicitly assume that the number of Greville points // equals the number of control points such that we don't have to resize // the result array. Think that is always the case, but beware if trying // other projection schemes later. RealArray weights; if (surf->rational()) surf->getWeights(weights); Go::SplineSurface* surf_new = VariationDiminishingSplineApproximation(surf->basis(0), surf->basis(1), gpar[0], gpar[1], sValues, 1, surf->rational(), weights); vec.assign(surf_new->coefs_begin(),surf_new->coefs_end()); delete surf_new; return true; }
//------------------------------------------------------------------------------ const std::vector<RealArray>& USNTwoWayRange::CalculateMeasurementDerivatives( GmatBase *obj, Integer id) { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage("USNTwoWayRange::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 < 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( "USNTwoWayRange 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() + " CartesianState 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 < size; ++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 < size; 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 < size; ++i) // currentDerivatives[0][i] = - rangeUnit[i]; // } // // velocity all zeroes // for (UnsignedInt ii = 3; ii < size; 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 (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 0.0; } } else if (objNumber == 2) // participant 2, always a Spacecraft { #ifdef DEBUG_DERIVATIVES MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant" " 2\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 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.52 and 7.53 for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 0.5 * (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.52 and 7.53 for (Integer i = 0; i < size; ++i) currentDerivatives[0][i] = 0.5 * (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.52 and 7.53 for (Integer i = 0; i < 6; ++i) currentDerivatives[0][i] = 0.5 * (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 (Integer i = 0; i < size; ++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 (Integer i = 0; i < size; ++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; }
//------------------------------------------------------------------------------ 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; }
//------------------------------------------------------------------------------ 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; }