Diff: kalmanfilter/kalmanfilter.h

Revision:

0:c55310328157

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalmanfilter/kalmanfilter.h	Fri Feb 07 11:34:24 2020 +0000
@@ -0,0 +1,46 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include "defining.h"
+#include "stdlib.h"
+#include "mbed.h"
+
+class kalmanfilter {
+    public:
+            Mat MatInitI(Mat C, int nOrder);                               /*方阵初始化为单位对角阵*/
+            Mat3 Mat3Init(Mat3 C, double dNum);
+            Mat MatInit(Mat C, int nM,int nN, double dNum);
+            Mat3 Mat3InitI(Mat3 C);
+            Mat Diag(cVec V, Mat Mout, int nOrder);
+            Mat MatSum(cMat C1, cMat C2, Mat C, int nM, int nN);           /*矩阵加法*/
+            Mat3 Mat3Sum(cMat3 C1, cMat3 C2, Mat3 C);
+            Mat3 Mat3Sum3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C);
+            Mat MatSum3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN);
+            Mat MatMinus(cMat C1, cMat C2, Mat C, int nM, int nN);         /*矩阵减法*/
+            Mat3 Mat3Minus(cMat3 C1, cMat3 C2, Mat3 C);
+            Mat MatCpy(Mat dest, cMat source, int nM, int nN);             /*矩阵拷贝*/
+            Mat3 Mat3Cpy(Mat3 dest,cMat3 source);
+            Mat MatProd(cMat C1, cMat C2, Mat C, int nM, int nN, int nP);  /*矩阵乘法*/
+            Mat3 Mat3Prod(cMat3 C1, cMat3 C2, Mat3 C);
+            Mat3 Mat3Prod3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C);
+            Mat MatProd3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN, int nP, int nQ);
+            Mat MatTrans(cMat C,Mat CT,int nM,int nN);                     /*矩阵转置*/
+            Mat3 Mat3Trans(cMat3 C,Mat3 CT);
+            Mat MatInv(cMat C,Mat IC,int n);                               /*方阵的逆*/
+            Mat3 Mat3Inv(cMat3 CM,Mat3 ICM);
+            Mat MatInvDiag(cMat C,Mat IC,int n);
+            Vec MVProd(cMat C, cVec V, Vec Vout, int nM, int nN);          /*矩阵与向量乘法*/
+            Vec3 MV3Prod(cMat3 C, cVec3 V, Vec3 Vout);
+            Vec3 VM3Prod(cVec3 V, cMat3 C, Vec3 Vout);
+            Vec VMProd(cVec V, cMat C, Vec Vout, int nM, int nN);
+            Vec VecCpy(Vec dest, cVec source, int nOrder);                 /*向量拷贝*/
+            nVec nVecCpy(nVec dest, cnVec source, int nOrder);
+            Vec VecSum(cVec V1, cVec V2, int nOrder, Vec Vout);            /*向量加法*/
+            nVec nVecSum(cnVec V1, cnVec V2, int nOrder, nVec Vout);
+            Vec VecMinus(cVec V1, cVec V2, int nOrder, Vec Vout);          /*向量减法*/
+            nVec nVecMinus(cnVec V1, cnVec V2, int nOrder, nVec Vout);
+            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, 
+             int n, int m, int q, int p, Vec Xk, Mat Pk, Mat Kk, Mat Pk_, Vec Xk_);/*卡尔曼滤波方程*/   
+    };
+
+#endif
\ No newline at end of file