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);
}
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;
}
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);
}
}
