File content as of revision 0:c55310328157:

#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