Shao Rui
/
Kalman
1
kalmanfilter/kalmanfilter.h@0:c55310328157, 2020-02-07 (annotated)
- Committer:
- shaorui
- Date:
- Fri Feb 07 11:34:24 2020 +0000
- Revision:
- 0:c55310328157
1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shaorui | 0:c55310328157 | 1 | #ifndef KALMANFILTER_H |
shaorui | 0:c55310328157 | 2 | #define KALMANFILTER_H |
shaorui | 0:c55310328157 | 3 | |
shaorui | 0:c55310328157 | 4 | #include "defining.h" |
shaorui | 0:c55310328157 | 5 | #include "stdlib.h" |
shaorui | 0:c55310328157 | 6 | #include "mbed.h" |
shaorui | 0:c55310328157 | 7 | |
shaorui | 0:c55310328157 | 8 | class kalmanfilter { |
shaorui | 0:c55310328157 | 9 | public: |
shaorui | 0:c55310328157 | 10 | Mat MatInitI(Mat C, int nOrder); /*方阵初始化为单位对角阵*/ |
shaorui | 0:c55310328157 | 11 | Mat3 Mat3Init(Mat3 C, double dNum); |
shaorui | 0:c55310328157 | 12 | Mat MatInit(Mat C, int nM,int nN, double dNum); |
shaorui | 0:c55310328157 | 13 | Mat3 Mat3InitI(Mat3 C); |
shaorui | 0:c55310328157 | 14 | Mat Diag(cVec V, Mat Mout, int nOrder); |
shaorui | 0:c55310328157 | 15 | Mat MatSum(cMat C1, cMat C2, Mat C, int nM, int nN); /*矩阵加法*/ |
shaorui | 0:c55310328157 | 16 | Mat3 Mat3Sum(cMat3 C1, cMat3 C2, Mat3 C); |
shaorui | 0:c55310328157 | 17 | Mat3 Mat3Sum3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C); |
shaorui | 0:c55310328157 | 18 | Mat MatSum3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN); |
shaorui | 0:c55310328157 | 19 | Mat MatMinus(cMat C1, cMat C2, Mat C, int nM, int nN); /*矩阵减法*/ |
shaorui | 0:c55310328157 | 20 | Mat3 Mat3Minus(cMat3 C1, cMat3 C2, Mat3 C); |
shaorui | 0:c55310328157 | 21 | Mat MatCpy(Mat dest, cMat source, int nM, int nN); /*矩阵拷贝*/ |
shaorui | 0:c55310328157 | 22 | Mat3 Mat3Cpy(Mat3 dest,cMat3 source); |
shaorui | 0:c55310328157 | 23 | Mat MatProd(cMat C1, cMat C2, Mat C, int nM, int nN, int nP); /*矩阵乘法*/ |
shaorui | 0:c55310328157 | 24 | Mat3 Mat3Prod(cMat3 C1, cMat3 C2, Mat3 C); |
shaorui | 0:c55310328157 | 25 | Mat3 Mat3Prod3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C); |
shaorui | 0:c55310328157 | 26 | Mat MatProd3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN, int nP, int nQ); |
shaorui | 0:c55310328157 | 27 | Mat MatTrans(cMat C,Mat CT,int nM,int nN); /*矩阵转置*/ |
shaorui | 0:c55310328157 | 28 | Mat3 Mat3Trans(cMat3 C,Mat3 CT); |
shaorui | 0:c55310328157 | 29 | Mat MatInv(cMat C,Mat IC,int n); /*方阵的逆*/ |
shaorui | 0:c55310328157 | 30 | Mat3 Mat3Inv(cMat3 CM,Mat3 ICM); |
shaorui | 0:c55310328157 | 31 | Mat MatInvDiag(cMat C,Mat IC,int n); |
shaorui | 0:c55310328157 | 32 | Vec MVProd(cMat C, cVec V, Vec Vout, int nM, int nN); /*矩阵与向量乘法*/ |
shaorui | 0:c55310328157 | 33 | Vec3 MV3Prod(cMat3 C, cVec3 V, Vec3 Vout); |
shaorui | 0:c55310328157 | 34 | Vec3 VM3Prod(cVec3 V, cMat3 C, Vec3 Vout); |
shaorui | 0:c55310328157 | 35 | Vec VMProd(cVec V, cMat C, Vec Vout, int nM, int nN); |
shaorui | 0:c55310328157 | 36 | Vec VecCpy(Vec dest, cVec source, int nOrder); /*向量拷贝*/ |
shaorui | 0:c55310328157 | 37 | nVec nVecCpy(nVec dest, cnVec source, int nOrder); |
shaorui | 0:c55310328157 | 38 | Vec VecSum(cVec V1, cVec V2, int nOrder, Vec Vout); /*向量加法*/ |
shaorui | 0:c55310328157 | 39 | nVec nVecSum(cnVec V1, cnVec V2, int nOrder, nVec Vout); |
shaorui | 0:c55310328157 | 40 | Vec VecMinus(cVec V1, cVec V2, int nOrder, Vec Vout); /*向量减法*/ |
shaorui | 0:c55310328157 | 41 | nVec nVecMinus(cnVec V1, cnVec V2, int nOrder, nVec Vout); |
shaorui | 0:c55310328157 | 42 | Vec Kalman1(cMat Phik_, cMat Gamak_1, cMat Hk, cMat Qk_1, cMat Rk, cMat Pk_1, cVec Xk_1, cVec Zk, cMat Bk_1, cVec Uk_1, |
shaorui | 0:c55310328157 | 43 | int n, int m, int q, int p, Vec Xk, Mat Pk, Mat Kk, Mat Pk_, Vec Xk_);/*卡尔曼滤波方程*/ |
shaorui | 0:c55310328157 | 44 | }; |
shaorui | 0:c55310328157 | 45 | |
shaorui | 0:c55310328157 | 46 | #endif |