//------------------------------------------------------------------------------
// <friend>
// TransposeTimesTranspose(const Rmatrix &m1, const Rmatrix &m2)
//------------------------------------------------------------------------------
Rmatrix TransposeTimesTranspose(const Rmatrix &m1, const Rmatrix &m2)
{
if ((m1.IsSized() == false) || (m2.IsSized() == false))
{
throw TableTemplateExceptions::UnsizedTable();
}
if (m1.rowsD != m2.rowsD)
throw TableTemplateExceptions::DimensionError();
Rmatrix m(m1.colsD, m2.rowsD);
int i, j, k;
for (i = 0; i < m1.colsD; i++)
{
for (j = 0; j < m2.rowsD; j++)
{
for (k = 0; k < m1.rowsD; k++)
{
m(i, j) += m1(k, i)*m2(j, k);
}
}
}
return m;
}
bool Covariance::FillMatrix(const Rmatrix& rhs, bool overrideAndFill)
{
bool retval = true;
// Check sizes of the matrices
if (!overrideAndFill && (dimension != rhs.GetNumRows()))
throw GmatBaseException("Covariance assignment dimensions do not match");
if (rhs.GetNumRows() != rhs.GetNumColumns())
throw GmatBaseException("Input covariance matrix is not square");
// Fill in the matrix info
if (dimension != rhs.GetNumRows())
{
// Must be in override and fill mode; names, indices, sizes & owners are
// all invalid; clear & set as unknown
elementNames.clear();
elementIndices.clear();
elementSizes.clear();
elementOwners.clear();
elementNames.push_back("GenericCovariance");
elementIndices.push_back(-1);
// set dimension to the input size
dimension = rhs.GetNumRows();
elementSizes.push_back(dimension);
elementOwners.push_back(NULL);
}
theCovariance = rhs;
return retval;
}
//------------------------------------------------------------------------------
Rmatrix Inverse::MatrixEvaluate()
{
//=======================================================
#ifdef DEBUG_EVALUATE
//=======================================================
Rmatrix rmat = leftNode->MatrixEvaluate();
MessageInterface::ShowMessage
("Inverse::MatrixEvaluate() left node =\n%s\n", rmat.ToString(12).c_str());
try
{
Rmatrix result = rmat.Inverse();
MessageInterface::ShowMessage
("Inverse::MatrixEvaluate() returning\n%s\n", result.ToString(12).c_str());
return result;
}
catch (BaseException &be)
{
MessageInterface::ShowMessage
("Inverse::MatrixEvaluate() %s for %s\n", be.GetFullMessage().c_str(),
GetName().c_str());
throw;
}
//=======================================================
#else
//=======================================================
return (leftNode->MatrixEvaluate()).Inverse();
//=======================================================
#endif
//=======================================================
}
bool Covariance::ConstructRHS(Rmatrix data, Integer start)
{
bool retval = false;
if (data.IsSized() == false)
throw GmatBaseException("Input covariance matrix is not properly "
"initialized");
Integer length = data.GetNumRows();
if (data.GetNumColumns() != length)
throw GmatBaseException("Input covariance matrix is not square");
if (start + length > dimension)
throw GmatBaseException("Input covariance matrix is will not fit in "
"the allocated covariance matrix");
for (Integer i = 0; i < length; ++i)
for (Integer j = i; j < length; ++j)
if (i == j)
theCovariance(i+start, j+start) = data(i, j);
else
{
// Symmetrize as we go
theCovariance(i+start, j+start) = theCovariance(j+start, i+start)
= (data(i, j) + data(j, i)) / 2.0;
}
return retval;
}
//------------------------------------------------------------------------------
// bool MatrixEvaluate()
//------------------------------------------------------------------------------
Rmatrix MathElement::MatrixEvaluate()
{
#ifdef DEBUG_EVALUATE
MessageInterface::ShowMessage
("MathElement::MatrixEvaluate() this='%s', refObjectName='%s', refObject=<%p>, "
"elementType=%d\n", GetName().c_str(), refObjectName.c_str(), refObject, elementType);
#endif
// If this MathElement is function Input, just return since it is handled in
// the FunctionRunner
if (isFunctionInput)
throw MathException("MathElement::MatrixEvaluate() Function input should "
"not be handled here");
if (elementType == Gmat::RMATRIX_TYPE)
{
if (refObject)
{
#ifdef DEBUG_EVALUATE
Rmatrix rmat = refObject->GetRmatrix();
MessageInterface::ShowMessage
("MathElement::MatrixEvaluate() It's an Array: %s matVal =\n%s\n",
refObject->GetName().c_str(), rmat.ToString().c_str());
#endif
ElementWrapper *wrapper = FindWrapper(refObjectName);
return wrapper->EvaluateArray();
}
else
{
#ifdef DEBUG_EVALUATE
MessageInterface::ShowMessage
("MathElement::MatrixEvaluate() It's a Rmatrix. matVal =\n%s\n",
matrix.ToString().c_str());
#endif
return matrix;
}
}
else
{
Real rval = Evaluate();
#ifdef DEBUG_EVALUATE
MessageInterface::ShowMessage
("MathElement::MatrixEvaluate() It's a number: rval = %f\n", rval);
#endif
// Set matrix 1x1 and return
Rmatrix rmat(1, 1, rval);
return rmat;
//throw MathException("MathElement::MatrixEvaluate() Invalid matrix");
}
}
//------------------------------------------------------------------------------
void PhysicalMeasurement::GetInverseSTM(GmatBase *forObject, Rmatrix &stmInv)
{
Integer stmId;
try
{
stmId = forObject->GetParameterID("CartesianX");
}
catch (BaseException &)
{
stmId = -1;
}
if (stmId >= 0)
{
stmInv = (forObject->GetParameterSTM(stmId))->Inverse();
}
else
{
// Use identity if there is no STM
stmInv(0,0) = stmInv(1,1) = stmInv(2,2) = stmInv(3,3)
= stmInv(4,4) = stmInv(5,5) = 1.0;
stmInv(0,1) = stmInv(0,2) = stmInv(0,3) = stmInv(0,4) = stmInv(0,5)
= stmInv(1,0) = stmInv(1,2) = stmInv(1,3) = stmInv(1,4)
= stmInv(1,5) = stmInv(2,0) = stmInv(2,1) = stmInv(2,3)
= stmInv(2,4) = stmInv(2,5) = stmInv(3,0) = stmInv(3,1)
= stmInv(3,2) = stmInv(3,4) = stmInv(3,5) = stmInv(4,0)
= stmInv(4,1) = stmInv(4,2) = stmInv(4,3) = stmInv(4,5)
= stmInv(5,0) = stmInv(5,1) = stmInv(5,2) = stmInv(5,3)
= stmInv(5,4) = 0.0;
}
#ifdef DEBUG_STM_INVERTER
Rmatrix mat = *forObject->GetParameterSTM(stmId);
MessageInterface::ShowMessage("STM:\n");
for (Integer i = 0; i < mat.GetNumRows(); ++i)
{
MessageInterface::ShowMessage(" [");
for (Integer j = 0; j < mat.GetNumColumns(); ++j)
MessageInterface::ShowMessage(" %.12lf ", mat(i,j));
MessageInterface::ShowMessage("]\n");
}
mat = stmInv;
MessageInterface::ShowMessage("STM Inverse:\n");
for (Integer i = 0; i < mat.GetNumRows(); ++i)
{
MessageInterface::ShowMessage(" [");
for (Integer j = 0; j < mat.GetNumColumns(); ++j)
MessageInterface::ShowMessage(" %.12lf ", mat(i,j));
MessageInterface::ShowMessage("]\n");
}
#endif
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::Symmetrize(Rmatrix& mat)
{
Integer size = mat.GetNumRows();
if (size != mat.GetNumColumns())
throw EstimatorException("Cannot symmetrize non-square matrices");
for (Integer i = 0; i < size; ++i)
{
for (Integer j = i+1; j < size; ++j)
{
mat(i,j) = 0.5 * (mat(i,j) + mat(j,i));
mat(j,i) = mat(i,j);
}
}
}
//------------------------------------------------------------------------------
// bool operator==(const Rmatrix &m)const
//------------------------------------------------------------------------------
bool Rmatrix::operator==(const Rmatrix &m)const
{
int ii,jj;
if ((isSizedD == false) || (m.IsSized() == false))
{
throw TableTemplateExceptions::UnsizedTable();
}
if ( this != &m)
{
if ((rowsD == m.rowsD) && (colsD == m.colsD))
{
for (ii=0; ii<rowsD; ii++)
{
for (jj=0; jj<colsD; jj++)
{
//loj: 5/5/06 used epsilon
//if (elementD[ii*colsD+jj] != m(ii,jj))
if (GmatMathUtil::Abs(elementD[ii*colsD+jj] - m(ii,jj)) >
GmatRealConstants::REAL_TOL)
{
return false;
}
}
}
}
else
{
return false;
}
}
return true;
}
//------------------------------------------------------------------------------
bool EstimationStateManager::MapSTMToObjects()
{
bool retval = true;
#ifdef DEBUG_STM_MAPPING
MessageInterface::ShowMessage("Setting object STM's to\n");
for (Integer i = 0; i < stateSize; ++i)
{
for (Integer j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", stm(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Fill in the STM based on the objects that comprise the state vector
GmatBase* obj;
Integer elementId; //, elementLength;
for (UnsignedInt h = 0; h < stateMap.size(); ++h)
{
obj = stateMap[h]->object;
if (stateMap[h]->subelement == 1)
{
elementId = stateMap[h]->parameterID;
// elementLength = stateMap[h]->length;
bool hasDstm = obj->HasDynamicParameterSTM(elementId);
if (hasDstm)
{
Rmatrix* dstm = obj->GetParameterSTM(elementId);
Integer stmSize = dstm->GetNumRows();
// Fill in the object stm's from the master stm
for (Integer i = 0; i < stmSize; ++i)
for (Integer j = 0; j < stmSize; ++j)
(*dstm)(i,j) = stm(h+i, h+j);
}
}
}
return retval;
}
//------------------------------------------------------------------------------
// Rmatrix operator-(const Rmatrix &m) const
//------------------------------------------------------------------------------
Rmatrix Rmatrix::operator-(const Rmatrix &m) const
{
if ((isSizedD == false) || (m.IsSized() == false))
throw TableTemplateExceptions::UnsizedTable();
// Added handling of 1x1 - MxN or MxN - 1x1 (LOJ: 2010.10.29)
bool oneByOneMinusMatrix = false;
bool matrixMinusOneByOne = false;
if (rowsD != m.rowsD || colsD != m.colsD)
{
if (rowsD == 1 && colsD == 1)
oneByOneMinusMatrix = true;
else if (m.rowsD == 1 && m.colsD == 1)
matrixMinusOneByOne = true;
else
throw TableTemplateExceptions::DimensionError();
}
Rmatrix diff(rowsD, colsD);
if (oneByOneMinusMatrix)
{
diff.SetSize(m.rowsD, m.colsD);
Real oneByOne = GetElement(0, 0);
for (int i = 0; i < m.rowsD*m.colsD; i++)
diff.elementD[i] = oneByOne - m.elementD[i];
}
else if (matrixMinusOneByOne)
{
Real oneByOne = m.GetElement(0, 0);
for (int i = 0; i < rowsD*colsD; i++)
diff.elementD[i] = elementD[i] - oneByOne;
}
else
{
for (int i = 0; i < rowsD*colsD; i++)
diff.elementD[i] = elementD[i] - m.elementD[i];
}
return diff;
}
//------------------------------------------------------------------------------
// <friend>
// Rmatrix operator/(Real scalar, const Rmatrix &m)
//------------------------------------------------------------------------------
Rmatrix operator/(Real scalar, const Rmatrix &m)
{
if (m.IsSized() == false)
throw TableTemplateExceptions::UnsizedTable();
Rmatrix div(m);
for (int i = 0; i < m.rowsD*m.colsD; i++)
div.elementD[i] = scalar / m.elementD[i];
return div;
}
//------------------------------------------------------------------------------
// <friend>
// Rmatrix operator*(Real scalar, const Rmatrix &m)
//------------------------------------------------------------------------------
Rmatrix operator*(Real scalar, const Rmatrix &m)
{
if (m.IsSized() == false)
throw TableTemplateExceptions::UnsizedTable();
Rmatrix prod(m);
int i;
for (i = 0; i < m.rowsD*m.colsD; i++)
prod.elementD[i] *= scalar;
return prod;
}
//------------------------------------------------------------------------------
Rmatrix Divide::MatrixEvaluate()
{
#ifdef DEBUG_EVALUATE
MessageInterface::ShowMessage
("\nDivide::MatrixEvaluate() '%s' entered\n", GetName().c_str());
#endif
Integer type1, row1, col1; // Left node matrix
Integer type2, row2, col2; // Right node matrix
Rmatrix div;
// Get the type(Real or Matrix), # rows and # columns of the left node
leftNode->GetOutputInfo(type1, row1, col1);
// Get the type(Real or Matrix), # rows and # columns of the right node
rightNode->GetOutputInfo(type2, row2, col2);
// Divide matrix by matrix
if( type1 == Gmat::RMATRIX_TYPE && type2 == Gmat::RMATRIX_TYPE)
div = leftNode->MatrixEvaluate() / rightNode->MatrixEvaluate();
// Divide scalar by matrix
else if( type1 == Gmat::REAL_TYPE && type2 == Gmat::RMATRIX_TYPE)
div = leftNode->Evaluate() / rightNode->MatrixEvaluate();
// Divide matrix by scalar
else if( type1 == Gmat::RMATRIX_TYPE && type2 == Gmat::REAL_TYPE)
div = leftNode->MatrixEvaluate() / rightNode->Evaluate();
#ifdef DEBUG_EVALUATE
MessageInterface::ShowMessage
("Divide::MatrixEvaluate() '%s' returning %s\n", GetName().c_str(), div.ToString().c_str());
#endif
return div;
}
//------------------------------------------------------------------------------
bool EstimationStateManager::BuildState()
{
bool retval = false;
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Entered BuildState()\n");
#endif
// Determine the size of the propagation state vector
stateSize = SortVector();
if (stateSize <= 0)
{
std::stringstream sizeVal;
sizeVal << stateSize;
throw EstimatorException("Estimation state vector size is " +
sizeVal.str() + "; estimation is not possible.");
}
std::map<std::string,Integer> associateMap;
// Build the associate map
std::string name;
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
if (associateMap.find(name) == associateMap.end())
associateMap[name] = index;
}
state.SetSize(stateSize);
// Build the data structures for the STM and covariance matrix
stm.SetSize(stateSize, stateSize);
covariance.SetDimension(stateSize);
stmColCount = stateSize;
covColCount = stateSize;
for (Integer index = 0; index < stateSize; ++index)
{
name = stateMap[index]->objectName;
std::stringstream sel("");
sel << stateMap[index]->subelement;
state.SetElementProperties(index, stateMap[index]->elementID,
name + "." + stateMap[index]->elementName + "." + sel.str(),
associateMap[name]);
}
// Initialize the STM matrix
for (Integer i = 0; i < stateSize; ++i)
for (Integer j = 0; j < stateSize; ++j)
if (i == j)
stm(i,j) = 1.0;
else
stm(i,j) = 0.0;
// Now build the covariance, using the elements the user has set and defaults
// for the rest
for (Integer i = 0; i < stateSize;)
{
Integer size = stateMap[i]->length;
Integer id = stateMap[i]->parameterID;
Covariance *cov = stateMap[i]->object->GetCovariance();
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found length = %d for id = %d\n", size, id);
#endif
Rmatrix *subcov = cov->GetCovariance(id);
if (subcov)
{
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found %d by %d subcovariance\n",
subcov->GetNumRows(), subcov->GetNumColumns());
#endif
for (Integer j = 0; j < size; ++j)
for (Integer k = 0; k < size; ++k)
covariance(i+j,i+k) = (*subcov)(j,k);
}
else
{
#ifdef DEBUG_COVARIANCE
MessageInterface::ShowMessage("Found no subcovariance\n");
#endif
if (stateMap[i]->elementName == "CartesianState")
{
for (Integer j = 0; j < size; ++j)
{
for (Integer k = 0; k < size; ++k)
{
if (j < 3)
covariance(i+j,i+k) = (j == k ? 1.0e12 : 0.0);
else
covariance(i+j,i+k) = (j == k ? 1.0e6 : 0.0);
}
}
}
else // Other defaults are set to the identity
{
for (Integer j = 0; j < size; ++j)
{
for (Integer k = 0; k < size; ++k)
{
covariance(i+j,i+k) = (j == k ? 1.0e0 : 0.0);
}
}
}
}
i += size;
}
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"Estimation state vector has %d elements:\n", stateSize);
StringArray props = state.GetElementDescriptions();
for (Integer index = 0; index < stateSize; ++index)
MessageInterface::ShowMessage(" %d: %s\n", index,
props[index].c_str());
#endif
#ifdef DUMP_STATE
MapObjectsToVector();
for (Integer i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage("State[%02d] = %.12lf, %s %d\n", i, state[i],
"RefState start =", state.GetAssociateIndex(i));
MessageInterface::ShowMessage("State Transition Matrix = %s\n",
stm.ToString().c_str());
MessageInterface::ShowMessage("Covariance Matrix = %s\n",
covariance.ToString().c_str());
#endif
return retval;
}
//------------------------------------------------------------------------------
void PhysicalMeasurement::GetRangeDerivative(Event &ev, const Rmatrix &stmInv,
Rvector &deriv, bool wrtP1, Integer p1Index, Integer p2Index, bool wrtR,
bool wrtV)
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("PhysicalMeasurement::GetRangeDerivative(%s, "
"stmInv, deriv, %d <%s>, %d <%s>, %s, %s)\n", ev.GetName().c_str(),
p1Index, participants[p1Index]->GetName().c_str(), p2Index,
participants[p2Index]->GetName().c_str(),
(wrtR == true ? "true" : "false"),
(wrtV == true ? "true" : "false"));
#endif
Rmatrix derivMatrix;
if (wrtR && wrtV)
derivMatrix.SetSize(6,6);
else
derivMatrix.SetSize(3,3);
GetRangeVectorDerivative(ev, stmInv, derivMatrix, wrtP1, p1Index, p2Index,
wrtR, wrtV);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Derivative matrix = \n");
for (Integer i = 0; i < derivMatrix.GetNumRows(); ++i)
{
MessageInterface::ShowMessage(" [");
for (Integer j = 0; j < derivMatrix.GetNumColumns(); ++j)
MessageInterface::ShowMessage(" %.12lf ", derivMatrix(i,j));
MessageInterface::ShowMessage("]\n");
}
#endif
Rvector3 temp;
Rmatrix33 mPart;
Rvector3 unitRange = rangeVec / rangeVec.GetMagnitude();
if (wrtR)
{
for (Integer i = 0; i < 3; ++i)
for (Integer j = 0; j < 3; ++j)
mPart(i,j) = derivMatrix(i,j);
temp = unitRange * mPart;
for (Integer i = 0; i < 3; ++i)
deriv[i] = temp(i);
}
if (wrtV)
{
Integer offset = (wrtR ? 3 : 0);
for (Integer i = 0; i < 3; ++i)
for (Integer j = 0; j < 3; ++j)
mPart(i,j) = derivMatrix(i+offset, j+offset);
temp = unitRange * mPart;
for (Integer i = 0; i < 3; ++i)
deriv[i+offset] = temp(i);
}
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Derivative = [");
for (Integer i = 0; i < deriv.GetSize(); ++i)
MessageInterface::ShowMessage(" %.12lf ", deriv[i]);
MessageInterface::ShowMessage("]\n");
#endif
}
bool KalmanFilterBase::checkRmatrix(const Rmatrix & a) const
{
return (unsigned(a.rows())==m_ && (unsigned(a.cols()))==m_);
}
//------------------------------------------------------------------------------
void PhysicalMeasurement::GetRangeVectorDerivative(Event &ev,
const Rmatrix &stmInv, Rmatrix &deriv, bool wrtP1, Integer p1Index,
Integer p2Index, bool wrtR, bool wrtV)
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("PhysicalMeasurement::GetRangeVector"
"Derivative(%s, stmInv, deriv, %d <%s>, %d <%s>, %s, %s)\n",
ev.GetName().c_str(), p1Index,
participants[p1Index]->GetName().c_str(), p2Index,
participants[p2Index]->GetName().c_str(),
(wrtR == true ? "true" : "false"),
(wrtV == true ? "true" : "false"));
#endif
EventData p1Data = ev.GetEventData(participants[p1Index]);
EventData p2Data = ev.GetEventData(participants[p2Index]);
rangeVec = p2Data.position - p1Data.position;
// Be sure to use the correct rotation matrices, etc
EventData dataToUse = (wrtP1 ? p1Data : p2Data);
// p1 derivatives pick up a minus sign, handled with this variable
Real sign = (wrtP1 ? -1.0 : 1.0);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("p1Position: %s\n", p1Data.position.ToString().c_str());
MessageInterface::ShowMessage("p2Position: %s\n", p2Data.position.ToString().c_str());
MessageInterface::ShowMessage(" Range vector: [%.12lf %.12lf %.12lf]\n",
rangeVec[0], rangeVec[1], rangeVec[2]);
#endif
Rmatrix phi = dataToUse.stm * stmInv;
Rmatrix33 A, B;
for (Integer i = 0; i < 3; ++i)
for (Integer j = 0; j < 3; ++j)
{
if (wrtR)
A(i,j) = phi(i,j);
if(wrtV)
B(i,j) = phi(i,j+3);
}
Rmatrix33 temp;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("Phi: %s\n", phi.ToString().c_str());
MessageInterface::ShowMessage(" A: %s\n", A.ToString().c_str());
MessageInterface::ShowMessage(" B: %s\n", B.ToString().c_str());
#endif
if (wrtR)
{
temp = dataToUse.rInertial2obj * A;
for (Integer i = 0; i < 3; ++i)
for (Integer j = 0; j < 3; ++j)
deriv(i,j) = sign * temp(i,j);
}
if (wrtV)
{
temp = dataToUse.rInertial2obj * B;
Integer offset = (wrtR ? 3 : 0);
for (Integer i = 0; i < 3; ++i)
for (Integer j = 0; j < 3; ++j)
deriv(i+offset,j+offset) = sign * temp(i,j);
}
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Rotation matrix for %s:\n",
participants[p2Index]->GetName().c_str());
for (Integer i = 0; i < 3; ++i)
{
MessageInterface::ShowMessage(" [");
for (Integer j = 0; j < 3; ++j)
MessageInterface::ShowMessage(" %.12lf ",
p2Data.rInertial2obj(i,j));
MessageInterface::ShowMessage("]\n");
}
#endif
}
//------------------------------------------------------------------------------
// const Rmatrix operator/(const Rmatrix &m) const
//------------------------------------------------------------------------------
Rmatrix Rmatrix::operator/( const Rmatrix &m) const
{
#ifdef DEBUG_DIVIDE
MessageInterface::ShowMessage
(wxT("Rmatrix::operator/() entered this=%s, m=%s\n"), this->ToString().c_str(),
m.ToString().c_str());
#endif
if ((isSizedD == false) || (m.IsSized() == false))
throw TableTemplateExceptions::UnsizedTable();
bool oneByOneDivideMatrix = false;
bool matrixDivideOneByOne = false;
#ifdef DEBUG_DIVIDE
MessageInterface::ShowMessage
(wxT(" rowsD=%d, colsD=%d, m.rowsD=%d, m.colsD=%d\n"), rowsD, colsD, m.rowsD, m.colsD);
#endif
if (rowsD == 1 && colsD == 1)
oneByOneDivideMatrix = true;
else if (m.rowsD == 1 && m.colsD == 1)
matrixDivideOneByOne = true;
#ifdef DEBUG_DIVIDE
MessageInterface::ShowMessage
(wxT(" oneByOneDivideMatrix=%d, matrixDivideOneByOne=%d\n"),
oneByOneDivideMatrix, matrixDivideOneByOne);
#endif
if (oneByOneDivideMatrix)
{
Rmatrix div(m.rowsD, m.colsD);
Real oneByOne = GetElement(0, 0);
for (int i = 0; i < m.rowsD; i++)
for (int j = 0; j < m.colsD; j++)
div(i, j) = oneByOne / m.GetElement(i, j);
#ifdef DEBUG_DIVIDE
MessageInterface::ShowMessage
(wxT("Rmatrix::operator/() returning OneByOne/Matrix %s\n"), div.ToString().c_str());
#endif
return div;
}
else if (matrixDivideOneByOne)
{
Rmatrix div(rowsD, colsD);
Real oneByOne = m.GetElement(0, 0);
for (int i = 0; i < rowsD; i++)
for (int j = 0; j < colsD; j++)
div(i, j) = GetElement(i, j) / oneByOne;
#ifdef DEBUG_DIVIDE
MessageInterface::ShowMessage
(wxT("Rmatrix::operator/() returning Matrix/OneByOne %s\n"), div.ToString().c_str());
#endif
return div;
}
else
return (*this)*m.Inverse();
}
//------------------------------------------------------------------------------
// const Rmatrix operator*(const Rmatrix &m) const
//------------------------------------------------------------------------------
Rmatrix Rmatrix::operator*(const Rmatrix &m) const
{
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT("Rmatrix::operator*() entered this=%s, m=%s\n"), this->ToString().c_str(),
m.ToString().c_str());
#endif
if ((isSizedD == false) || (m.IsSized() == false))
throw TableTemplateExceptions::UnsizedTable();
// Added handling of 1x1 * MxN or MxN * 1x1 (LOJ: 2010.10.29)
bool oneByOneTimesMatrix = false;
bool matrixTimesOneByOne = false;
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT(" rowsD=%d, colsD=%d, m.rowsD=%d, m.colsD=%d\n"), rowsD, colsD, m.rowsD, m.colsD);
#endif
if (colsD != m.rowsD)
{
if (rowsD == 1 && colsD == 1)
oneByOneTimesMatrix = true;
else if (m.rowsD == 1 && m.colsD == 1)
matrixTimesOneByOne = true;
else
throw TableTemplateExceptions::DimensionError();
}
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT(" oneByOneTimesMatrix=%d, matrixTimesOneByOne=%d\n"),
oneByOneTimesMatrix, matrixTimesOneByOne);
#endif
if (oneByOneTimesMatrix)
{
Rmatrix prod(m.rowsD, m.colsD); // declare a zero matrix
Real oneByOne = GetElement(0, 0);
for (int i = 0; i < m.rowsD; i++)
for (int j = 0; j < m.colsD; j++)
prod(i, j) = m.GetElement(i, j) * oneByOne;
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT("Rmatrix::operator*() returning OneByOne*Matrix %s\n"), prod.ToString().c_str());
#endif
return prod;
}
else if ( matrixTimesOneByOne)
{
Rmatrix prod(rowsD, colsD); // declare a zero matrix
Real oneByOne = m.GetElement(0, 0);
for (int i = 0; i < rowsD; i++)
for (int j = 0; j < colsD; j++)
prod(i, j) = GetElement(i, j) * oneByOne;
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT("Rmatrix::operator*() returning Matrix*OneByOne %s\n"), prod.ToString().c_str());
#endif
return prod;
}
else
{
Rmatrix prod(rowsD, m.colsD); // declare a zero matrix
for (int i = 0; i < rowsD; i++)
for (int j = 0; j < m.colsD; j++)
for (int k = 0; k < colsD; k++)
prod(i, j) += elementD[i*colsD + k] * m(k, j);
#ifdef DEBUG_MULTIPLY
MessageInterface::ShowMessage
(wxT("Rmatrix::operator*() returning %s\n"), prod.ToString().c_str());
#endif
return prod;
}
}
//------------------------------------------------------------------------------
std::string SequentialEstimator::GetProgressString()
{
StringArray::iterator current;
std::stringstream progress;
progress.str("");
progress.precision(12);
const std::vector<ListItem*> *map = esm.GetStateMap();
if (isInitialized)
{
switch (currentState)
{
case INITIALIZING:
// This state is basically a "paused state" used for the Target
// command to finalize the initial data for the variables and
// goals. All that is written here is the header information.
{
progress << "************************************************"
<< "********\n"
<< "*** Performing Estimation "
<< "(using \"" << instanceName << "\")\n";
// Write out the setup data
progress << "*** " ;
progress << "\n****************************"
<< "****************************\n\na priori state:\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
};
progress << "\n a priori covariance:\n\n";
Rmatrix aPrioriCovariance = *(stateCovariance->GetCovariance());
for (Integer i = 0; i < aPrioriCovariance.GetNumRows(); ++i)
{
progress << "----- Row " << (i+1) << "\n";
for (Integer j=0; j < aPrioriCovariance.GetNumColumns(); ++j)
progress << " " << aPrioriCovariance(i, j);
progress << "\n";
}
}
break;
case ESTIMATING:
progress << "Current estimated state:\n";
progress << " Estimation Epoch: "
<< currentEpoch << "\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*estimationState)[i];
}
progress << "\n Current Residual Value: "
<< *(--(measurementResiduals.end())) << " "
<< " Trace of the State Covariance: "
<< stateCovariance->GetCovariance()->Trace() << "\n";
break;
case FINISHED:
progress << "\n****************************"
<< "****************************\n"
<< "*** Estimating Completed"
<< "\n****************************"
<< "****************************\n\n"
<< "\n\nFinal Estimated State:\n\n";
progress << " Estimation Epoch (A.1 modified Julian): "
<< currentEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
}
{ // Switch statement scoping
Rmatrix finalCovariance = *(stateCovariance->GetCovariance());
progress << "\nFinal Covariance Matrix:\n\n";
for (Integer i = 0; i < finalCovariance.GetNumRows(); ++i)
{
progress << "----- Row " << (i+1) << "\n";
for (Integer j = 0; j < finalCovariance.GetNumColumns(); ++j)
progress << " " << finalCovariance(i, j);
progress << "\n";
}
}
#ifdef DUMP_RESIDUALS
MessageInterface::ShowMessage("\nMeasurement Residuals: \n");
for (UnsignedInt i = 0; i < measurementEpochs.size(); ++i)
{
MessageInterface::ShowMessage(" %.12lf %.12lf\n",
measurementEpochs[i], measurementResiduals[i]);
}
#endif
progress << "\n****************************"
<< "****************************\n"
<< std::endl;
break;
default:
throw SolverException(
"Solver state not supported for the sequential estimator");
}
}
else
return Estimator::GetProgressString();
return progress.str();
}
//------------------------------------------------------------------------------
std::string BatchEstimator::GetProgressString()
{
StringArray::iterator current;
std::stringstream progress;
progress.str("");
progress.precision(12);
const std::vector<ListItem*> *map = esm.GetStateMap();
if (isInitialized)
{
switch (currentState)
{
case INITIALIZING:
// This state is basically a "paused state" used for the Target
// command to finalize the initial data for the variables and
// goals. All that is written here is the header information.
{
progress << "************************************************"
<< "********\n"
<< "*** Performing Estimation "
<< "(using \"" << instanceName << "\")\n";
// Write out the setup data
progress << "*** " ;
progress << "\n****************************"
<< "****************************\n\na priori state:\n";
if (estEpochFormat != "FromParticipants")
progress << " Estimation Epoch (" << estEpochFormat
<< "): " << estEpoch << "\n\n";
else
progress << " Estimation Epoch (A.1 modified Julian): "
<< estimationEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
};
progress << "\n a priori covariance:\n\n";
Rmatrix aPrioriCovariance = *(stateCovariance->GetCovariance());
for (Integer i = 0; i < aPrioriCovariance.GetNumRows(); ++i)
{
progress << "----- Row " << (i+1) << "\n";
for (Integer j = 0; j < aPrioriCovariance.GetNumColumns(); ++j)
progress << " " << aPrioriCovariance(i, j);
progress << "\n";
}
}
break;
case CHECKINGRUN:
progress << "------------------------------"
<< "------------------------\n"
<< "Iteration " << iterationsTaken
<< "\n\nCurrent estimated state:\n";
progress << " Estimation Epoch: "
<< estimationEpoch << "\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*estimationState)[i];
}
progress << "\n RMS residuals: "
<< newResidualRMS << "\n";
break;
case FINISHED:
progress << "\n****************************"
<< "****************************\n"
<< "*** Estimating Completed in " << iterationsTaken
<< " iterations"
<< "\n****************************"
<< "****************************\n\n"
<< "Estimation "
<< (converged ? "converged!" : "did not converge")
<< "\n\nFinal Estimated State:\n\n";
if (estEpochFormat != "FromParticipants")
progress << " Estimation Epoch (" << estEpochFormat
<< "): " << estEpoch << "\n\n";
else
progress << " Estimation Epoch (A.1 modified Julian): "
<< estimationEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
progress << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
}
{ // Switch statement scoping
Rmatrix finalCovariance = information.Inverse();
progress << "\nFinal Covariance Matrix:\n\n";
for (Integer i = 0; i < finalCovariance.GetNumRows(); ++i)
{
progress << "----- Row " << (i+1) << "\n";
for (Integer j = 0; j < finalCovariance.GetNumColumns(); ++j)
progress << " " << finalCovariance(i, j);
progress << "\n";
}
}
if (textFileMode == "Verbose")
{
progress << "\n RMS residuals at previous iteration: "
<< oldResidualRMS;
progress << "\n RMS residuals at this iteration: "
<< newResidualRMS << "\n\n";
}
progress << "\n****************************"
<< "****************************\n\n"
<< std::endl;
#ifdef DUMP_FINAL_RESIDUALS
{
std::fstream residFile;
residFile.open("FinalResiduals.csv", std::ios::out);
for (UnsignedInt i = 0; i < measurementResiduals.size(); ++i)
{
residFile << i << " " << measurementEpochs[i]
<< " " << measurementResiduals[i] << "\n";
}
residFile.close();
}
#endif
break;
default:
throw SolverException(
"Solver state not supported for the simulator");
}
}
else
return Estimator::GetProgressString();
return progress.str();
}
//------------------------------------------------------------------------------
Integer PropagationStateManager::SortVector()
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"Entered PropagationStateManager::SortVector()\n");
#endif
StringArray *propList;
std::vector<Integer> order;
std::vector<Gmat::StateElementId> idList;
ObjectArray owners;
StringArray property;
std::vector<Integer>::iterator oLoc;
Gmat::StateElementId id;
Integer size, loc = 0, val;
stateSize = 0;
// Initially assume there is no post superposition member
hasPostSuperpositionMember = false;
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Element list:\n");
Integer k = 0;
for (std::map<GmatBase*, StringArray*>::iterator i = elements.begin();
i != elements.end(); ++i)
{
current = i->first;
propList = i->second;
MessageInterface::ShowMessage(" %d: %s ->\n", ++k,
current->GetName().c_str());
for (UnsignedInt j = 0; j < propList->size(); ++j)
{
MessageInterface::ShowMessage(" %s\n", (*propList)[j].c_str());
}
}
#endif
// First build a list of the property IDs and objects, measuring state size
// at the same time
for (UnsignedInt q = 0; q < objects.size(); ++q)
{
current = objects[q];
propList = elements[current];
for (StringArray::iterator j = propList->begin();
j != propList->end(); ++j)
{
id = (Gmat::StateElementId)current->SetPropItem(*j);
if (id == Gmat::UNKNOWN_STATE)
throw PropagatorException("Unknown state element: " + (*j) +
" on object " + current->GetName() + ", a " +
current->GetTypeName());
size = current->GetPropItemSize(id);
if (size <= 0)
throw PropagatorException("State element " + (*j) +
" has size set less than or equal to 0; unable to continue.");
stateSize += size;
for (Integer k = 0; k < size; ++k)
{
idList.push_back(id);
if (current->PropItemNeedsFinalUpdate(id))
hasPostSuperpositionMember = true;
owners.push_back(current);
property.push_back(*j);
// Put this item in the ordering list
oLoc = order.begin();
while (oLoc != order.end())
{
val = idList[*oLoc];
if (id < val)
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Inserting; id = %d, z = %d,"
" loc = %d\n", id, (*oLoc), loc);
#endif
order.insert(oLoc, loc);
break;
}
++oLoc;
}
if (oLoc == order.end())
order.push_back(loc);
++loc;
}
}
}
ListItem *newItem;
val = 0;
completionIndexList.clear();
completionSizeList.clear();
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"State size is %d()\n", stateSize);
#endif
for (Integer i = 0; i < stateSize; ++i)
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("%d <- %d: %d %s.%s gives ", i, order[i],
idList[order[i]], owners[order[i]]->GetName().c_str(),
property[order[i]].c_str());
#endif
newItem = new ListItem;
newItem->objectName = owners[order[i]]->GetName();
newItem->elementName = property[order[i]];
if (owners[order[i]]->HasAssociatedStateObjects())
newItem->associateName = owners[order[i]]->GetAssociateName(val);
else
newItem->associateName = owners[order[i]]->GetName();
newItem->object = owners[order[i]];
newItem->elementID = idList[order[i]];
newItem->subelement = ++val;
newItem->parameterID =
owners[order[i]]->GetParameterID(property[order[i]]);
newItem->parameterType =
owners[order[i]]->GetParameterType(newItem->parameterID);
newItem->dynamicObjectProperty =
newItem->object->ParameterAffectsDynamics(newItem->parameterID);
if (newItem->parameterType == Gmat::REAL_TYPE)
newItem->parameterID += val - 1;
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("[%s, %s, %s, %d, %d, %d, %d, %s]\n",
newItem->objectName.c_str(),
newItem->elementName.c_str(),
newItem->associateName.c_str(),
newItem->elementID,
newItem->subelement,
newItem->parameterID,
newItem->parameterType,
(newItem->dynamicObjectProperty ? "dynamic" : "static"));
#endif
if (newItem->parameterType == Gmat::RVECTOR_TYPE)
{
const Rvector vec =
owners[order[i]]->GetRvectorParameter(property[order[i]]);
newItem->rowLength = vec.GetSize();
newItem->rowIndex = val - 1;
}
if (newItem->parameterType == Gmat::RMATRIX_TYPE)
{
const Rmatrix mat =
owners[order[i]]->GetRmatrixParameter(property[order[i]]);
newItem->rowLength = mat.GetNumColumns();
newItem->colIndex = (val-1) % newItem->rowLength;
newItem->rowIndex = (Integer)((val - 1) / newItem->rowLength);
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"RowLen = %d, %d -> row %2d col %2d\n", newItem->rowLength,
val, newItem->rowIndex, newItem->colIndex);
#endif
}
newItem->nonzeroInit = owners[order[i]]->
ParameterDvInitializesNonzero(newItem->parameterID,
newItem->rowIndex, newItem->colIndex);
if (newItem->nonzeroInit)
{
newItem->initialValue = owners[order[i]]->
ParameterDvInitialValue(newItem->parameterID,
newItem->rowIndex, newItem->colIndex);
}
if (newItem->object->PropItemNeedsFinalUpdate(newItem->elementID))
{
completionIndexList.push_back(newItem->elementID);
completionSizeList.push_back(1); // Or count sizes?
// newItem->nonzeroInit = true;
// newItem->initialValue = 1.0;
}
newItem->postDerivativeUpdate = owners[order[i]]->
ParameterUpdatesAfterSuperposition(newItem->parameterID);
newItem->length = owners[order[i]]->GetPropItemSize(idList[order[i]]);
if (val == newItem->length)
val = 0;
stateMap.push_back(newItem);
}
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("State map contents:\n");
for (std::vector<ListItem*>::iterator i = stateMap.begin();
i != stateMap.end(); ++i)
MessageInterface::ShowMessage(" %s %s %d %d of %d, id = %d\n",
(*i)->objectName.c_str(), (*i)->elementName.c_str(),
(*i)->elementID, (*i)->subelement, (*i)->length,
(*i)->parameterID);
MessageInterface::ShowMessage(
"Finished PropagationStateManager::SortVector()\n");
#endif
return stateSize;
}
//------------------------------------------------------------------------------
void BatchEstimator::WriteToTextFile(Solver::SolverState sState)
{
if (!showProgress)
return;
if (!textFile.is_open())
OpenSolverTextFile();
Solver::SolverState theState = sState;
if (sState == Solver::UNDEFINED_STATE)
theState = currentState;
const std::vector<ListItem*> *map = esm.GetStateMap();
switch (theState)
{
case INITIALIZING:
textFile << "\n****************************"
<< "****************************\n"
<< "*** Initializing Estimation \n"
<< "****************************"
<< "****************************\n"
<< "\n\na priori state:\n";
if (estEpochFormat != "FromParticipants")
textFile << " Estimation Epoch (" << estEpochFormat
<< "): " << estEpoch << "\n\n";
else
textFile << " Estimation Epoch (A.1 modified Julian): "
<< estimationEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
textFile << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
}
break;
case CHECKINGRUN:
if (textFileMode == "Verbose")
{
textFile << "------------------------------"
<< "------------------------\n"
<< "Iteration " << iterationsTaken
<< "\n\nCurrent estimated state:\n";
if (estEpochFormat != "FromParticipants")
textFile << " Estimation Epoch (" << estEpochFormat
<< "): " << estEpoch << "\n\n";
else
textFile << " Estimation Epoch (A.1 modified Julian): "
<< estimationEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
textFile << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
}
textFile << "\n RMS residuals at this iteration: "
<< newResidualRMS << "\n\n";
}
break;
case FINISHED:
textFile << "\n****************************"
<< "****************************\n"
<< "*** Estimating Completed in " << iterationsTaken
<< " iterations"
<< "\n****************************"
<< "****************************\n\n"
<< "Estimation "
<< (converged ? "converged!" : "did not converge")
<< "\n\nFinal Estimated State:\n\n";
if (estEpochFormat != "FromParticipants")
textFile << " Estimation Epoch (" << estEpochFormat
<< "): " << estEpoch << "\n\n";
else
textFile << " Estimation Epoch (A.1 modified Julian): "
<< estimationEpoch << "\n\n";
for (UnsignedInt i = 0; i < map->size(); ++i)
{
textFile << " "
<< (*map)[i]->objectName << "."
<< (*map)[i]->elementName << "."
<< (*map)[i]->subelement << " = "
<< (*estimationState)[i] << "\n";
}
{ // Switch statement scoping
Rmatrix finalCovariance = information.Inverse();
textFile << "\nFinal Covariance Matrix:\n\n";
for (Integer i = 0; i < finalCovariance.GetNumRows(); ++i)
{
for (Integer j = 0; j < finalCovariance.GetNumColumns(); ++j)
textFile << " " << finalCovariance(i, j);
textFile << "\n";
}
}
if (textFileMode == "Verbose")
{
textFile << "\n RMS residuals at previous iteration: "
<< oldResidualRMS;
textFile << "\n RMS residuals at this iteration: "
<< newResidualRMS << "\n\n";
}
textFile << "\n****************************"
<< "****************************\n\n"
<< std::endl;
break;
default:
break;
}
}
//------------------------------------------------------------------------------
Integer EstimationStateManager::SortVector()
{
StringArray *elementList;
std::vector<Integer> order;
std::vector<Integer> idList;
ObjectArray owners;
StringArray property;
std::vector<Integer>::iterator oLoc;
Integer id = -1;
Integer size, loc = 0, val;
stateSize = 0;
// First build a list of the property IDs and objects, measuring state size
// at the same time
for (std::map<GmatBase*, StringArray*>::iterator i = elements.begin();
i != elements.end(); ++i)
{
current = i->first;
elementList = i->second;
for (StringArray::iterator j = elementList->begin();
j != elementList->end(); ++j)
{
id = current->GetEstimationParameterID(*j);
// if (id == Gmat::UNKNOWN_STATE)
// throw EstimatorException("Unknown state element: " + (*j));
#ifdef DEBUG_OBJECT_MAPPING
MessageInterface::ShowMessage("Element ID for %s is %d;", j->c_str(),
id);
#endif
size = current->GetEstimationParameterSize(id);
#ifdef DEBUG_OBJECT_MAPPING
MessageInterface::ShowMessage(" size is %d\n", size);
#endif
stateSize += size;
for (Integer k = 0; k < size; ++k)
{
idList.push_back(id);
owners.push_back(current);
property.push_back(*j);
// Put this item in the ordering list
oLoc = order.begin();
while (oLoc != order.end())
{
val = idList[*oLoc];
if (id < val)
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("Inserting; id = %d, z = %d,"
" loc = %d\n", id, (*oLoc), loc);
#endif
order.insert(oLoc, loc);
break;
}
++oLoc;
}
if (oLoc == order.end())
order.push_back(loc);
++loc;
}
}
}
ListItem *newItem;
val = 0;
for (Integer i = 0; i < stateSize; ++i)
{
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("%d <- %d: %d %s.%s gives ", i, order[i],
idList[order[i]], owners[order[i]]->GetName().c_str(),
property[order[i]].c_str());
#endif
newItem = new ListItem;
newItem->objectName = owners[order[i]]->GetName();
newItem->elementName = property[order[i]];
newItem->object = owners[order[i]];
newItem->elementID = idList[order[i]];
newItem->subelement = ++val;
newItem->parameterID =
owners[order[i]]->GetParameterID(property[order[i]]);
newItem->parameterType =
owners[order[i]]->GetParameterType(newItem->parameterID);
if (newItem->parameterType == Gmat::REAL_TYPE)
newItem->parameterID += val - 1;
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("[%s, %s, %d, %d, %d, %d]\n",
newItem->objectName.c_str(),
newItem->elementName.c_str(),
newItem->elementID,
newItem->subelement,
newItem->parameterID,
newItem->parameterType);
#endif
if (newItem->parameterType == Gmat::RVECTOR_TYPE)
{
const Rvector mat =
owners[order[i]]->GetRvectorParameter(property[order[i]]);
newItem->rowLength = mat.GetSize();
newItem->rowIndex = val-1;
}
if (newItem->parameterType == Gmat::RMATRIX_TYPE)
{
const Rmatrix mat =
owners[order[i]]->GetRmatrixParameter(property[order[i]]);
newItem->rowLength = mat.GetNumColumns();
newItem->colIndex = (val-1) % newItem->rowLength;
newItem->rowIndex = (Integer)((val - 1) / newItem->rowLength);
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage(
"RowLen = %d, %d -> row %2d col %2d\n", newItem->rowLength,
val, newItem->rowIndex, newItem->colIndex);
#endif
}
newItem->length =
owners[order[i]]->GetEstimationParameterSize(idList[order[i]]);
if (val == newItem->length)
val = 0;
stateMap.push_back(newItem);
}
#ifdef DEBUG_STATE_CONSTRUCTION
MessageInterface::ShowMessage("State size = %d; map contents:\n",
stateSize);
for (std::vector<ListItem*>::iterator i = stateMap.begin();
i != stateMap.end(); ++i)
MessageInterface::ShowMessage(" %s %s %d %d of %d, id = %d\n",
(*i)->objectName.c_str(), (*i)->elementName.c_str(),
(*i)->elementID, (*i)->subelement, (*i)->length,
(*i)->parameterID);
#endif
return stateSize;
}
