EKF增益系数K
高维EKF滤波器，增益系数K的计算中，有高维矩阵逆运算，请问如何计算？？ Pzz是吧。。。 Gauss消元法啊。CAL_STATUS MATRIX_Inv(MATRIX_TypeDef* A, MATRIX_TypeDef* res)
{
int i, j, k;
float m, det = 1, tempBuf;
if((A->nRow != res->nRow) || (A->nCol != res->nCol) || (A->nCol != A->nRow))
return CAL_ERROR;
/*init Buf*/
for(i = 0; i < A->nCol*A->nCol; i++)
*(res->pData) = 0.0;
for(i = 0; i< A->nCol; i++)
*(res->pData + i + i*A->nRow) = 1;
for(i = 0; i < A->nCol*A->nCol; i++)
tempBuf = *(A->pData + i);
/*Gauss*/
for(i = 0; (i < A->nCol-1); i++)
{
for(k = i+1; k < A->nCol; k++ )
{
/*Calculate M*/
m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
/*Updata Row*/
for(j = i; j < A->nRow; j++)
*(A->pData + j + k*A->nCol) -= m * *(A->pData + j + i*A->nCol);
for(j = 0; j < A->nRow; j++)
*(res->pData + j + k*A->nCol) -= m * *(res->pData + j + i*A->nCol);
}
}
/*Check det*/
for(i = 0; i< A->nCol; i++)
det *= *(A->pData + i + i*A->nRow);
if(det < 0.00014)
return CAL_ERROR;
/*Call Back*/
for(i = A->nCol-1; i > 0; i--)
{
for(k = i-1; k >= 0; k--)
{
m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
*(A->pData + i + k*A->nCol) -= m * *(A->pData + i + i*A->nCol);
for(j = 0; j < A->nCol; j++)
*(res->pData + j + k*A->nCol) -=m * *(res->pData + j + i*A->nCol);
}
}
for(i = 0; i < A->nRow; i++)
{
for(k = 0; k < A->nRow; k++ )
*(res->pData + k + i*A->nCol) /=*(A->pData + i + i*A->nCol);
}
for(i = 0; i < A->nCol*A->nCol; i++)
*(A->pData + i) = tempBuf;
return CAL_OK;
}
矩阵的定义是:
typedef struct
{
uint8_t nRow; //行数
uint8_t nCol; //列数
float*pData;//数据
} MATRIX_TypeDef;
可以考虑不用EKF，不太好算，UKF比较好
页:
[1]