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 Create an Empty Matrix of Short Int
* @param nRow : Number of Rows
* @param nCol : Number of columns
* @param nType : specifies as to fill the Matrix
* @retval Pointer to iNEMO_sMATRIX_TYPE
* @par Functions called:
* None
*/
iNEMO_fMATRIX_TYPE *iNEMO_fMatCreateInit(int nRow, int nCol, int nType)
{
iNEMO_fMATRIX_TYPE* pTmp;
int i, j;
if(nRow==0 || nCol==0)
return(NULL);
if ((pTmp = iNEMO_fMatCreate( nRow, nCol )) != NULL)
{
switch (nType)
{
case iNEMO_ZERO_MATRIX:
case iNEMO_IDEN_MATRIX:
case iNEMO_ONES_MATRIX:
for (i=0; i < nRow; i++)
for (j=0; j < nCol; j++)
{
/* Fills of ones */
if (nType == iNEMO_ONES_MATRIX)
{
iNEMO_MatData(pTmp)[i][j] = 1.0f;
continue;
}
/* Fills as Identity Matrix */
if (nType == iNEMO_IDEN_MATRIX)
{
if (i==j)
{
iNEMO_MatData(pTmp)[i][j] = 1.0f;
continue;
}
}
/* Fills of zeros */
iNEMO_MatData(pTmp)[i][j] = 0.0f;
}
break;
}
return (pTmp);
}
else
return (NULL);
}
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);
}
}
