iNEMO_fMATRIX_TYPE* iNEMO_UpdateP(iNEMO_fMATRIX_TYPE* pPoldMat,
iNEMO_fMATRIX_TYPE* pKMat,
iNEMO_fMATRIX_TYPE* pHJMat,
iNEMO_fMATRIX_TYPE* pPnewMat)
{
iNEMO_fMATRIX_TYPE* pTmp;
iNEMO_fMATRIX_TYPE* pI;
pTmp = iNEMO_fMatCreateZero(iNEMO_MatRow(pPoldMat), iNEMO_MatCol(pPoldMat));
pI = iNEMO_fMatCreateUnit(iNEMO_MatRow(pPoldMat), iNEMO_MatCol(pPoldMat));
// First Step: pTmp = (pKMat * pHJMat)
pTmp = iNEMO_fMatMulMat(pKMat, pHJMat, pTmp);
// Second Step: pTmp = I - pTmp
pTmp = iNEMO_fMatSub(pI, pTmp, pTmp);
// Third Step: pPnewMat = pTmp * pPoldMat
pPnewMat = iNEMO_fMatMulMat(pTmp, pPoldMat, pPnewMat);
iNEMO_fMatFree(pTmp);
iNEMO_fMatFree(pI);
return (pPnewMat);
}
/**
********************************************************************************
* @brief Copy pSource to pDest
* @param pSource : the source Matrix
* @param pDest : the destination Matrix
* @retval the resulting Matrix
* @par Functions called:
* None
*/
iNEMO_fMATRIX_TYPE* iNEMO_fMatCopy(iNEMO_fMATRIX_TYPE* pSource, iNEMO_fMATRIX_TYPE* pDest)
{
int i, j;
// Check the dimensions
if ( iNEMO_MatRow(pSource) != iNEMO_MatRow(pDest) ||
iNEMO_MatCol(pSource) != iNEMO_MatCol(pDest) )
return NULL;
else
{
for (i=0; i < iNEMO_MatRow(pSource); i++)
for (j=0; j < iNEMO_MatCol(pSource); j++)
iNEMO_MatData(pDest)[i][j] = iNEMO_MatData(pSource)[i][j];
return(pDest);
}
}
/**
*******************************************************************************
* @brief Add two Matrix iNEMO_fMATRIX_TYPE
* @param pTerm1 : the first Addend source Matrix
* @param pTerm2 : the second Addend source Matrix
* @param pAdd : the resulting Matrix
* @retval the resulting Matrix
* @par Functions called:
* None
*/
iNEMO_fMATRIX_TYPE* iNEMO_fMatAdd(iNEMO_fMATRIX_TYPE* pTerm1,
iNEMO_fMATRIX_TYPE* pTerm2,
iNEMO_fMATRIX_TYPE* pAdd)
{
int i, j;
/* Check if dimensions are wrong */
if ( iNEMO_MatRow(pAdd) != iNEMO_MatRow(pTerm1) || iNEMO_MatCol(pAdd) != iNEMO_MatCol(pTerm2) )
return NULL;
else
{
for (i=0; i < iNEMO_MatRow(pTerm1); ++i)
for (j=0; j < iNEMO_MatCol(pTerm1); ++j)
iNEMO_MatData(pAdd)[i][j] = iNEMO_MatData(pTerm1)[i][j] +
iNEMO_MatData(pTerm2)[i][j];
}
return(pAdd);
}
/**
********************************************************************************
* @brief Fill the Matrix with a specific value
* @param pMat : Pointer to iNEMO_fMATRIX_TYPE
* @param fValue : the float value to fill the Matrix
* @retval Pointer to iNEMO_fMATRIX_TYPE
* @par Functions called:
* None
*/
iNEMO_fMATRIX_TYPE *iNEMO_fMatFill(iNEMO_fMATRIX_TYPE *pMat, float fValue)
{
int i, j;
for (i=0; i<iNEMO_MatRow(pMat); i++)
for (j=0; j<iNEMO_MatCol(pMat); j++)
iNEMO_MatData(pMat)[i][j] = (float) (fValue);
return (pMat);
}
/**
********************************************************************************
* @brief Multiply one Matrix for the Transpose of another one
* @param pTerm1 : the first source Matrix
* @param pTerm2 : the second source Matrix (MT)
* @param pMul : the destination Matrix
* @retval the resulting Matrix
* @par Functions called:
* None
*/
iNEMO_fMATRIX_TYPE* iNEMO_fMatMulMatMT(iNEMO_fMATRIX_TYPE* pTerm1,
iNEMO_fMATRIX_TYPE* pTerm2,
iNEMO_fMATRIX_TYPE* pMul)
{
int i, j, k;
// if dimensions are wrong
if ( iNEMO_MatRow(pMul) != iNEMO_MatRow(pTerm1) ||
iNEMO_MatCol(pMul) != iNEMO_MatRow(pTerm2) )
return NULL;
else
{
for (i=0; i < iNEMO_MatRow(pTerm1); i++)
for (j=0; j < iNEMO_MatRow(pTerm2); j++)
for (k=0, iNEMO_MatData(pMul)[i][j]=0.0; k < iNEMO_MatCol(pTerm1); k++)
iNEMO_MatData(pMul)[i][j] += iNEMO_MatData(pTerm1)[i][k]
* iNEMO_MatData(pTerm2)[j][k];
}
return(pMul);
}
iNEMO_fMATRIX_TYPE* iNEMO_CalculateK(iNEMO_fMATRIX_TYPE* pPMat,
iNEMO_fMATRIX_TYPE* pHJMat,
iNEMO_fMATRIX_TYPE* pRMat,
iNEMO_fMATRIX_TYPE* pKMat)
{
/* Internal Variables */
iNEMO_fMATRIX_TYPE* pTmp1;
iNEMO_fMATRIX_TYPE* pTmp2;
iNEMO_fMATRIX_TYPE* pRInvMat;
pTmp1 = iNEMO_fMatCreate(iNEMO_MatRow(pPMat), iNEMO_MatRow(pHJMat));
pTmp2 = iNEMO_fMatCreate(iNEMO_MatRow(pHJMat), iNEMO_MatCol(pTmp1));
/* First Step: pTmp1 = pPMat * pHJMat' */
pTmp1 = iNEMO_fMatMulMatMT(pPMat, pHJMat, pTmp1);
/* Second Step: pTmp2 = pHJMat * (pPMat * pHJMat') = pHJMat * pTmp1 */
pTmp2 = iNEMO_fMatMulMat(pHJMat, pTmp1, pTmp2);
/* Third Step: pTmp2 = pTmp2 + pRMat */
pTmp2 = iNEMO_fMatAdd(pTmp2, pRMat, pTmp2);
/* Fourth Step: pRInvMat = (pTmp2)^-1 */
pRInvMat = iNEMO_fMatCreate(iNEMO_MatRow(pTmp2), iNEMO_MatCol(pTmp2));
pRInvMat = iNEMO_fMatInv(pTmp2, pRInvMat);
/* Fifth Step: pKMat = pTmp1 * pRInvMat */
pKMat = iNEMO_fMatMulMat(pTmp1, pRInvMat, pKMat);
/* Delete the Temporary Matrix before to exit */
iNEMO_fMatFree(pTmp1);
iNEMO_fMatFree(pTmp2);
iNEMO_fMatFree(pRInvMat);
return pKMat;
}
/**
********************************************************************************
* @brief Free the Matrix from Memory
* @param pMat : the short int Matrix to delete
* @retval 0 if NULL pMat is passed, 1 if Normal Exit
* @par Functions called:
* None
*/
int iNEMO_sMatFree(iNEMO_sMATRIX_TYPE *pMat)
{
int i;
if (pMat == NULL)
return (0);
for (i=0; i<iNEMO_MatRow(pMat); i++)
{
iNEMO_Free( iNEMO_MatData(pMat)[i] );
}
iNEMO_Free(iNEMO_MatData(pMat));
iNEMO_Free( pMat );
return (1);
}
iNEMO_fMATRIX_TYPE* iNEMO_PropagateP(iNEMO_fMATRIX_TYPE* pPoldMat,
iNEMO_fMATRIX_TYPE* pStateMat,
iNEMO_fMATRIX_TYPE* pQMat,
iNEMO_fMATRIX_TYPE* pPnewMat)
{
/* Internal Variables */
iNEMO_fMATRIX_TYPE* pTmp;
pTmp = iNEMO_fMatCreate(iNEMO_MatRow(pPoldMat), iNEMO_MatCol(pPoldMat));
/* First Step: pTmp = pStateMat * pPoldMat */
pTmp = iNEMO_fMatMulMat(pStateMat, pPoldMat, pTmp);
/* Second Step: pPnewMat = pTmp * pStateMat' */
pPnewMat = iNEMO_fMatMulMatMT(pTmp, pStateMat, pPnewMat);
/* Third Step: pPnewMat += Q */
pPnewMat = iNEMO_fMatAdd(pPnewMat, pQMat, pPnewMat);
/* Delete the Temporary Matrix before to exit */
iNEMO_fMatFree(pTmp);
return pPnewMat;
}
/**
********************************************************************************
* @brief Find Inverse of a Matrix
* @param pSource : the source Matrix
* @param pDest : square inverse matrix of pSource, NULL in case of fail
* @retval the resulting Matrix
* @par Functions called:
* @ref iNEMO_fMatCreate
* @ref iNEMO_fMatCopy
* @ref iNEMO_MatLUP
* @ref iNEMO_fMatFree
*/
iNEMO_fMATRIX_TYPE* iNEMO_fMatInv(iNEMO_fMATRIX_TYPE* pSource,
iNEMO_fMATRIX_TYPE* pDest)
{
iNEMO_fMATRIX_TYPE* A;
iNEMO_fMATRIX_TYPE* B;
iNEMO_sMATRIX_TYPE* P;
int i, nCol, nRow;
nCol = iNEMO_MatCol(pSource);
nRow = iNEMO_MatRow(pSource);
if (nCol != nRow)
/* The matrix is not Square */
return (NULL);
A = iNEMO_fMatCreate(nRow, nCol);
if (A == NULL)
return (NULL);
B = iNEMO_fMatCreate(nRow, nCol);
if (B == NULL)
return (NULL);
/* P is a vector matrix */
P = iNEMO_sMatCreate(nRow, 1);
if (P == NULL)
return (NULL);
/* It is to avoid to modify pSource Matrix */
iNEMO_fMatCopy(pSource, A);
/* LU Decomposition and check for Singular Matrix */
if (iNEMO_MatLUP(A, P) == -1)
{
iNEMO_fMatFree(A);
iNEMO_fMatFree(B);
iNEMO_sMatFree(P);
return (NULL);
}
for (i=0; i<nCol; ++i)
{
iNEMO_fMatFill(B, 0.0f);
iNEMO_MatData(B)[i][0] = 1.0f;
iNEMO_MatBackSubs(A, B, P, pDest, i);
}
iNEMO_fMatFree(A);
iNEMO_fMatFree(B);
iNEMO_sMatFree(P);
if (pDest == NULL)
{
return(NULL);
}
else
{
return (pDest);
}
}
