EKF增益系数K

421054972

高维EKF滤波器，增益系数K的计算中，有高维矩阵逆运算，请问如何计算？？

2013的弹子球

Pzz是吧。。。

笑笑我笑了

Gauss消元法啊。

1. CAL_STATUS MATRIX_Inv(MATRIX_TypeDef* A, MATRIX_TypeDef* res)
   
2. {
   
3. 
   
4.    int i, j, k;
   
5. 
   
6.    float m, det = 1, tempBuf[A->nCol*A->nRow];
   
7. 
   
8.    if((A->nRow != res->nRow) || (A->nCol != res->nCol) || (A->nCol != A->nRow))
   
9.         return CAL_ERROR;
   
10. 
    
11.    /*init Buf*/
    
12. 
    
13.    for(i = 0; i < A->nCol*A->nCol; i++)
    
14.        *(res->pData) = 0.0;
    
15. 
    
16.    for(i = 0; i< A->nCol; i++)
    
17.        *(res->pData + i + i*A->nRow) = 1;
    
18. 
    
19.    for(i = 0; i < A->nCol*A->nCol; i++)
    
20.        tempBuf[i] = *(A->pData + i);
    
21. 
    
22.     /*Gauss*/
    
23.    for(i = 0; (i < A->nCol-1); i++)
    
24.    {
    
25.        for(k = i+1; k < A->nCol; k++ )
    
26.        {
    
27.         /*Calculate M*/
    
28.            m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
    
29.         /*Updata Row*/
    
30.            for(j = i; j < A->nRow; j++)
    
31.                *(A->pData + j + k*A->nCol) -= m * *(A->pData + j + i*A->nCol);
    
32.            for(j = 0; j < A->nRow; j++)
    
33.                *(res->pData + j + k*A->nCol) -= m * *(res->pData + j + i*A->nCol);
    
34.        }
    
35. 
    
36.    }
    
37. 
    
38.    /*Check det*/
    
39.    for(i = 0; i< A->nCol; i++)
    
40.        det *= *(A->pData + i + i*A->nRow);
    
41. 
    
42.     if(det < 0.00014)
    
43.         return CAL_ERROR;
    
44. 
    
45.     /*Call Back*/
    
46.    for(i = A->nCol-1; i > 0; i--)
    
47.    {
    
48. 
    
49.         for(k = i-1; k >= 0; k--)
    
50.         {
    
51.             m = *(A->pData + i + k*A->nCol) / *(A->pData + i + i*A->nCol);
    
52.             *(A->pData + i + k*A->nCol) -= m * *(A->pData + i + i*A->nCol);
    
53.             for(j = 0; j < A->nCol; j++)
    
54.                *(res->pData + j + k*A->nCol) -=  m * *(res->pData + j + i*A->nCol);
    
55.         }
    
56.    }
    
57. 
    
58.    for(i = 0; i < A->nRow; i++)
    
59.    {
    
60.        for(k = 0; k < A->nRow; k++ )
    
61.             *(res->pData + k + i*A->nCol) /=  *(A->pData + i + i*A->nCol);
    
62. 
    
63.    }
    
64. 
    
65.    for(i = 0; i < A->nCol*A->nCol; i++)
    
66.        *(A->pData + i) = tempBuf[i];
    
67. 
    
68.    return CAL_OK;
    
69. 
    
70. }

复制代码

矩阵的定义是:

1. typedef struct
   
2. {
   
3.     uint8_t nRow;   //行数
   
4.     uint8_t nCol;   //列数
   
5.     float*  pData;  //数据
   
6. } MATRIX_TypeDef;

复制代码

2013的弹子球

可以考虑不用EKF，不太好算，UKF比较好