File content as of revision 0:c55310328157:

#include"kalmanfilter.h"
#include "math.h"
/*========================= Mat3Init ====================================*/
Mat3 kalmanfilter::Mat3Init(Mat3 C, double dNum){
/*初始化3维方阵所有分量为指定值
  参数：[in]C: 待初始化的3维方阵
        [in]dNum: 指定值
  返回：C
*/

/*需求：
  类型: Mat3
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i,j;
    for (i=0;i<3;i++){
        for(j=0;i<3;j++){
            C[i][j]=dNum;
        }
    }
    return C;
}

/*========================= MatInit ====================================*/
Mat kalmanfilter:: MatInit(Mat C, int nM,int nN, double dNum){
/*初始化矩阵所有分量为指定值
  参数：[in]C: 待初始化矩阵
        [in]nM: 矩阵行数
        [in]nN: 矩阵列数
        [in]dNum: 指定值
  返回：C
*/

/*需求：
  类型: Mat
  宏: MGet
*/      

/*创建时间: 20070530
  最近更改时间：20091103
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i,j;
    for (i=0;i<nM;i++){
        for(j=0;j<nN;j++){
            MGet(C,i,j,nN)=dNum;
        }
    }
    return C;
}

/*========================= Mat3InitI ====================================*/
Mat3 kalmanfilter::Mat3InitI(Mat3 C){
/*初始化3维方阵为单位对角阵
  参数：[in]C: 待初始化的3维方阵
  返回：C
*/

/*需求：
  类型: Mat3
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i,j;
    for (i=0;i<3;i++){
        for(j=0;i<3;j++){
            if (i==j){
                C[i][j]=1.0;
            }else{
                C[i][j]=0.0;
            }
        }
    }
    return C;
}


/*========================= MatInitI ====================================*/
Mat kalmanfilter::MatInitI(Mat C, int nOrder){
/*方阵初始化为单位对角阵
  参数：[in]C: 待初始化的方阵
        [in]nOrder: 方阵维数
  返回：C
*/

/*需求：
  类型: Mat
  宏: MGet
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i,j;
    for (i=0;i<nOrder;i++){
        for(j=0;j<nOrder;j++){
            if (i==j){
                MGet(C,i,j,nOrder)=1.0;
            }else{
                MGet(C,i,j,nOrder)=0.0;
            }
        }
    }
    return C;
}


/*========================= Diag ====================================*/
Mat kalmanfilter::Diag(cVec V, Mat Mout, int nOrder){
/* 通过向量初始化对角方阵
  参数：[in]V: 向量V
        [out]Mout: 待初始化方阵
        [in]nOrder: 向量维数
  返回：Mout
*/

/*需求：
  类型: Mat, cVec
  宏: MGet
*/      

/*创建时间: 20080509
  最近更改时间：20080509
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i,j;
    for (i=0;i<nOrder;i++){
        for(j=0;j<nOrder;j++){
            if (i==j){
                MGet(Mout,i,j,nOrder)=V[i];
            }else{
                MGet(Mout,i,j,nOrder)=0.0;
            }
        }
    }
    return Mout;
}

/******************************************************************************************************************/

/*============================ Mat3Sum=================================*/
Mat3 kalmanfilter::Mat3Sum(cMat3 C1, cMat3 C2, Mat3 C){
/*3*3矩阵加法
  参数：[in]C1:3*3 C1矩阵
        [in]C2:3*3 C2矩阵
        [out]C: C1+C2
  返回：C
*/

/*需求：
  类型: Mat3,cMat3
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 SSTC
*/
    int i,j;    
    for (i=0;i<3;i++)
    {
        for(j=0;j<3;j++) C[i][j] = C1[i][j] + C2[i][j];     
    }
    return C;   
}


/*============================ Mat3Sum3=================================*/
Mat3 kalmanfilter::Mat3Sum3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C){
/*3*3矩阵加法(三个矩阵)
  参数：[in]C1:3*3 C1矩阵
        [in]C2:3*3 C2矩阵
        [in]C3:3*3 C3矩阵
        [out]C: C1+C2+C3
  返回：C
*/

/*需求：
  类型: Mat3,cMat3
*/    

/* 创建时间: 20080410
   最近更改时间：20080410
   Copyright 2005-2010 SSTC
*/
    int i,j;    
    for (i=0;i<3;i++)
    {
        for(j=0;j<3;j++) C[i][j] = C1[i][j] + C2[i][j]+ C3[i][j];       
    }
    return C;   
}


/*============================ MatSum=================================*/
Mat kalmanfilter::MatSum(cMat C1, cMat C2, Mat C, int nM, int nN){
/*矩阵加法
  参数：[in]C1: C1矩阵
        [in]C2: C2矩阵
        [out]C: C1+C2
        [in]nM: 矩阵行数
        [in]nN: 矩阵列数
  返回：C
*/

/*需求：
  类型: Mat,cMat
  宏: MGet
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/

    int i,j;    
    for (i=0;i<nM;i++){
        for(j=0;j<nN;j++){  
            MGet(C,i,j,nN)=MGet(C1,i,j,nN)+MGet(C2,i,j,nN);
        }
    }
    return C;   
}


/*============================ MatSum3=================================*/
Mat kalmanfilter::MatSum3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN){
/*矩阵加法(三个矩阵)
  参数：[in]C1: C1矩阵
        [in]C2: C2矩阵
        [in]C3: C3矩阵
        [out]C: C1+C2+C3
        [in]nM: 矩阵行数
        [in]nN: 矩阵列数
  返回：C
*/

/*需求：
  类型: Mat,cMat
  宏: MGet
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/

    int i,j;    
    for (i=0;i<nM;i++){
        for(j=0;j<nN;j++){  
            MGet(C,i,j,nN)=MGet(C1,i,j,nN)+MGet(C2,i,j,nN)+MGet(C3,i,j,nN);
        }
    }
    return C;   
}


/*************************************************************************************************/

/*============================ Mat3Minus =================================*/
Mat3 kalmanfilter::Mat3Minus(cMat3 C1, cMat3 C2, Mat3 C){
/*3*3矩阵减法
  参数：[in]C1:3*3 C1矩阵
        [in]C2:3*3 C2矩阵
        [out]C: C1-C2
  返回：C
*/

/*需求：
  类型: Mat3,cMat3
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 SSTC
*/
    int i,j;    
    for (i=0;i<3;i++){
        for(j=0;j<3;j++) C[i][j] = C1[i][j] - C2[i][j];     
    }
    return C;   
}


/*============================ MatMinus =================================*/
Mat kalmanfilter::MatMinus(cMat C1, cMat C2, Mat C, int nM, int nN){
/*矩阵减法
  参数：[in]C1: C1矩阵
        [in]C2: C2矩阵
        [out]C: C1-C2
        [in]nM: 矩阵行数
        [in]nN: 矩阵列数
  返回：C
*/

/*需求：
  类型: Mat,cMat
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/
    int i;  
    for (i=0;i<nM*nN;i++){
        C[i]=C1[i]-C2[i];
    }

    return C;   
}
/*********************************************************************************************************/

/*========================= Mat3Cpy ====================================*/
Mat3 kalmanfilter::Mat3Cpy(Mat3 dest,cMat3 source){
/*3*3矩阵拷贝
  参数：[out]dest:3*3目标矩阵
        [in]source:3*3源矩阵
  返回：dest
*/

/*需求：
  类型: Mat3,cMat3
*/      

/*创建时间: 20070527
  最近更改时间：20070527
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    dest[0][0]=source[0][0];
    dest[0][1]=source[0][1];
    dest[0][2]=source[0][2];    
    dest[1][0]=source[1][0];
    dest[1][1]=source[1][1];
    dest[1][2]=source[1][2];    
    dest[2][0]=source[2][0];
    dest[2][1]=source[2][1];
    dest[2][2]=source[2][2];
    return dest;
}


/*========================= MatCpy ====================================*/
Mat kalmanfilter::MatCpy(Mat dest, cMat source, int nM, int nN){
/*矩阵拷贝
  参数：[out]dest: 目标矩阵
        [in]source: 源矩阵
        [in]nM: 矩阵行数
        [in]nN: 矩阵列数
  返回：dest
*/

/*需求：
  类型: Mat,cMat
*/      

/*创建时间: 20080402
  最近更改时间：20080402
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/

    int i;
    for(i=0;i<nM*nN;i++){
        dest[i]=source[i];
    }
    return dest;
}
/***************************************************************************************************/

/*============================ Mat3Prod =================================*/
Mat3 kalmanfilter::Mat3Prod(cMat3 C1, cMat3 C2, Mat3 C){
/*3*3矩阵乘法
  参数：[in]C1:3*3 C1矩阵
        [in]C2:3*3 C2矩阵
        [out]C: C1*C2
  返回：C
*/

/*需求：
  类型：Mat3,cMat3
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 HLW
*/

    int i,j,k;  
    double Ctemp[3][3];
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            Ctemp[i][j] = 0.0;
            for (k=0;k<3;k++){
                Ctemp[i][j] = Ctemp[i][j] + C1[i][k]*C2[k][j]; 
            }
        }
    }
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            C[i][j]=Ctemp[i][j];
        }
    }
    return C;
}


/*============================ Mat3Prod3 =================================*/
Mat3 kalmanfilter::Mat3Prod3(cMat3 C1, cMat3 C2, cMat3 C3, Mat3 C){
/*3*3矩阵乘法(三个矩阵)
  参数：[in]C1:3*3 C1矩阵
        [in]C2:3*3 C2矩阵
        [in]C3:3*3 C3矩阵
        [out]C: C1*C2*C3
  返回：C
*/

/*需求：
  类型：Mat3,cMat3
*/    

/* 创建时间: 20080410
   最近更改时间: 20080410
   Copyright 2005-2010 SSTC
*/

    int i,j,k;  
    double Ctemp[3][3],Ctemp2[3][3];
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            Ctemp[i][j] = 0.0;
            for (k=0;k<3;k++){
                Ctemp[i][j] = Ctemp[i][j] + C1[i][k]*C2[k][j]; 
            }
        }
    }
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            Ctemp2[i][j] = 0.0;
            for (k=0;k<3;k++){
                Ctemp2[i][j] = Ctemp2[i][j] + Ctemp[i][k]*C3[k][j]; 
            }
        }
    }
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            C[i][j]=Ctemp2[i][j];
        }
    }
    return C;
}


/*============================ MatProd =================================*/
Mat kalmanfilter::MatProd(cMat C1, cMat C2, Mat C, int nM, int nN, int nP){
/*矩阵乘法
  参数：[in]C1: C1矩阵(nM*nN)
        [in]C2: C2矩阵(nN*nP)
        [out]C: C1*C2
        [in]nM: C1行数
        [in]nN: C1列数(C2行数)
        [in]nP: C2列数
  返回：C
*/

/*需求：
  类型: Mat,cMat
  头文件: stdlib.h
  宏: MGet
*/    

/* 创建时间: 20080403
   最近更改时间：20080403
   Copyright 2005-2010 SSTC
*/

    int i,j,k;  
    Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nP);
    for (i=0;i<nM;i++){
        for(j=0;j<nP;j++){
            MGet(Ctemp,i,j,nP) = 0.0;
            for (k=0;k<nN;k++){
                MGet(Ctemp,i,j,nP) = MGet(Ctemp,i,j,nP) + MGet(C1,i,k,nN)*MGet(C2,k,j,nP); 
            }
        }
    }
    for (i=0;i<nM;i++){
        for(j=0;j<nP;j++){
            MGet(C,i,j,nP)=MGet(Ctemp,i,j,nP);
        }
    }
    free(Ctemp);
    return C;
}


/*============================ MatProd3 =================================*/
Mat kalmanfilter::MatProd3(cMat C1, cMat C2, cMat C3, Mat C, int nM, int nN, int nP, int nQ){
/*矩阵乘法(三个矩阵)
  参数：[in]C1: C1矩阵(nM*nN)
        [in]C2: C2矩阵(nN*nP)
        [in]C3: C3矩阵(nP*nQ)
        [out]C: C1*C2*C3
        [in]nM: C1行数
        [in]nN: C1列数(C2行数)
        [in]nP: C2列数(C3行数)
        [in]nQ: C3列数
  返回：C
*/

/*需求：
  类型: Mat,cMat
  头文件: stdlib.h
  宏: MGet
*/    

/* 创建时间: 20080410
   最近更改时间：20080410
   Copyright 2005-2010 SSTC
*/

    int i,j,k;  
    Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nP);
    Mat Ctemp2=(Mat)malloc(sizeof(double)*nM*nQ);
    for (i=0;i<nM;i++){
        for(j=0;j<nP;j++){
            MGet(Ctemp,i,j,nP) = 0.0;
            for (k=0;k<nN;k++){
                MGet(Ctemp,i,j,nP) = MGet(Ctemp,i,j,nP) + MGet(C1,i,k,nN)*MGet(C2,k,j,nP); 
            }
        }
    }
    for (i=0;i<nM;i++){
        for(j=0;j<nQ;j++){
            MGet(Ctemp2,i,j,nQ) = 0.0;
            for (k=0;k<nP;k++){
                MGet(Ctemp2,i,j,nQ) = MGet(Ctemp2,i,j,nQ) + MGet(Ctemp,i,k,nP)*MGet(C3,k,j,nQ); 
            }
        }
    }
    for (i=0;i<nM;i++){
        for(j=0;j<nQ;j++){
            MGet(C,i,j,nQ)=MGet(Ctemp2,i,j,nQ);
        }
    }
    free(Ctemp);
    free(Ctemp2);
    return C;
}
/***************************************************************************************************/

/*============================ Mat3Trans =================================*/
Mat3 kalmanfilter::Mat3Trans(cMat3 C,Mat3 CT){
/*3*3矩阵转置
  参数：[in]C: 3*3 C矩阵
        [out]CT: C的转置
  返回：CT
*/

/*需求：
  类型：Mat3,cMat3
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 SSTC
*/

    int i,j;
    double Ctemp[3][3];
    for(i=0;i<3;i++){
        for(j=0;j<3;j++){
            Ctemp[i][j] = C[j][i];
        }
    }
    for(i=0;i<3;i++){
        for(j=0;j<3;j++){
            CT[i][j] = Ctemp[i][j];
        }
    }
    return CT;
}


/*============================ MatTrans =================================*/
Mat kalmanfilter::MatTrans(cMat C,Mat CT,int nM,int nN){
/*矩阵转置
  参数：[in]C: C矩阵
        [out]CT: C的转置
        [in]nM: C矩阵行数
        [in]nN: C矩阵列数
  返回：CT
*/

/*需求：
  类型：Mat,cMat
  头文件：stdlib.h
  宏：MGet
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/

    int i,j;
    Mat Ctemp=(Mat)malloc(sizeof(double)*nM*nN);
    for(i=0;i<nN;i++){
        for(j=0;j<nM;j++){
            MGet(Ctemp,i,j,nM)=MGet(C,j,i,nN);  
        }
    }
    for(i=0;i<nM*nN;i++){
        CT[i] = Ctemp[i];
    }
    free(Ctemp);
    return CT;
}
/**************************************************************************************************/

/*============================ MatInv =================================*/
Mat kalmanfilter::MatInv(cMat C,Mat IC,int n){
/*方阵的逆
  参数：[in]C:方阵C
        [out]IC:方阵C的逆
        [in]n:方阵C的维数
  返回：IC,若不可逆或失败则返回NULL
*/

/*需求：
  头文件:math.h,stdlib.h
  类型:Mat,cMat
  宏:NULL
*/    

/* 创建时间: 20070615
   最近更改时间: 20070615
   Copyright 2005-2010 SSTC
*/

    int i,j,k,l;
    double *cpyC;

    cpyC=(double*)malloc(n*n*sizeof(double));
    if (cpyC==NULL){
        return NULL;
    }

    for (i=0;i<n*n;i++){
        cpyC[i]=C[i];
    }
    /* 单位阵*/
    for (i=0; i<n; i++){
        for (j=0; j<n; j++){    
                *(IC+i*n+j) = 0.0;
            }
        *(IC+i*n+i) = 1.0;
    }
    
    /* 化上三角阵*/
    for (j=0; j<n; j++) {   
        if(fabs(*(cpyC+j*n+j))>1.0e-25){/* C[j][j]不等于0*/        
            /* IC阵的第j行除以C[j][j]*/
            for(k=0; k<n; k++){
                *(IC+j*n+k) /= *(cpyC+j*n+j);
            }
            /* C阵的第j行除以C[j][j]*/
            for(k=n-1; k>=j; k--){
                *(cpyC+j*n+k) /= *(cpyC+j*n+j);
            }
            
            for(i=j+1; i<n; i++){
                /* IC阵的第i行 - C[i][j]*IC阵的第j行*/
                for(k=0; k<n; k++){
                    *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
                }
                /* C阵的第i行 - C[i][j]*C阵的第j行*/
                for (k=n-1; k>=j; k--){
                    *(cpyC+i*n+k) -= *(cpyC+i*n+j) * *(cpyC+j*n+k);
                }
            }
        }else if (j<n-1){           
            for(l=j+1; l<n; l++){
                /* 若C阵第j行后的C[l][j]不等于0，第j行加上第l行*/
                if (fabs(*(cpyC+l*n+j)) > 1.0e-25)  {
                    for (k=0; k<n; k++){
                        *(IC+j*n+k) += *(IC+l*n+k);
                    }
                    for (k=n-1; k>=j; k--){
                        *(cpyC+j*n+k) += *(cpyC+l*n+k);
                    }
                    /* IC阵的第j行除以C[j][j]*/
                    for (k=0; k<n; k++){
                        *(IC+j*n+k) /= *(cpyC+j*n+j);
                    }
                    /* C阵的第j行除以C[j][j]*/
                    for (k=n-1; k>=j; k--){
                        *(cpyC+j*n+k) /= *(cpyC+j*n+j);
                    }
                    /* 第i行 - C[i][j]*第j行*/
                    for (i=j+1; i<n; i++){
                        for (k=0; k<n; k++){
                            *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
                        }
                        for (k=n-1; k>=j; k--){
                            *(cpyC+i*n+k) -= *(cpyC+i*n+j) * *(cpyC+j*n+k);
                        }
                    }
                    break;
                }
            }
            
            if (l == n){ /* C[l][j] 全等于0*/          
                free(cpyC);
                return NULL;   /*  矩阵的行列式为零，不可求逆*/
            }
        }else{  /* C[n][n]等于0*/     
            free(cpyC);
            return NULL;    /*  矩阵的行列式为零，不可求逆*/
        }
    }
    /* 化成单位阵*/
    for(j=n-1; j>=1; j--){
        for(i=j-1; i>=0; i--){
            for(k=0; k<n; k++){
                *(IC+i*n+k) -= *(cpyC+i*n+j) * *(IC+j*n+k);
            }
            *(cpyC+i*n+j) = 0;
        }
    }
    free(cpyC);
    return IC;
}


/*============================ Mat3Inv =================================*/
Mat3 kalmanfilter::Mat3Inv(cMat3 CM,Mat3 ICM){
/*3*3矩阵的逆
  参数：[in]CM:3*3 CM矩阵
        [out]ICM:3*3 ICM矩阵
  返回：ICM,若不可逆则返回NULL
*/

/*需求：
  类型：Mat3,cMat3
  宏：NULL, EPS
  头文件: math.h
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 SSTC
*/


    double det;
    double Ctemp[3][3];
    int i,j;
    det=CM[0][0]*CM[1][1]*CM[2][2]+CM[0][1]*CM[1][2]*CM[2][0]+CM[1][0]*CM[2][1]*CM[0][2]-CM[0][2]*CM[1][1]*CM[2][0]-CM[0][1]*CM[1][0]*CM[2][2]-CM[2][1]*CM[1][2]*CM[0][0];

    if (fabs(det)<1.0e-16){
        return NULL;
    }   
    
    Ctemp[0][0] =  (CM[1][1]*CM[2][2] - CM[2][1]*CM[1][2])/det;
    Ctemp[0][1] = -(CM[0][1]*CM[2][2] - CM[2][1]*CM[0][2])/det;
    Ctemp[0][2] =  (CM[0][1]*CM[1][2] - CM[1][1]*CM[0][2])/det;
    Ctemp[1][0] = -(CM[1][0]*CM[2][2] - CM[2][0]*CM[1][2])/det;
    Ctemp[1][1] =  (CM[0][0]*CM[2][2] - CM[2][0]*CM[0][2])/det;
    Ctemp[1][2] = -(CM[0][0]*CM[1][2] - CM[1][0]*CM[0][2])/det;
    Ctemp[2][0] =  (CM[1][0]*CM[2][1] - CM[2][0]*CM[1][1])/det;
    Ctemp[2][1] = -(CM[0][0]*CM[2][1] - CM[2][0]*CM[0][1])/det;
    Ctemp[2][2] =  (CM[0][0]*CM[1][1] - CM[0][1]*CM[1][0])/det; 
    for (i=0;i<3;i++){
        for(j=0;j<3;j++){
            ICM[i][j]=Ctemp[i][j];
        }
    }
    return ICM;
}


/*============================ MatInvDiag =================================*/
Mat kalmanfilter::MatInvDiag(cMat C,Mat IC,int n){
/*方阵的对角逆
  参数：[in]C:方阵C
        [out]IC:方阵C的对角逆
        [in]n:方阵C的维数
  返回：IC,对角元素取倒数，其他元素置0,对角元素若为0则仍保持0
*/

/*需求：
  头文件:math.h,stdlib.h
  类型:Mat,cMat
  宏:MGet
*/    

/* 创建时间: 20091104
   最近更改时间: 20091104
   Copyright 2005-2010 SSTC
*/

    int i,j;    
    for (i=0;i<n;i++){
        for (j=0;j<n;j++){
            if(i!=j){
                MGet(IC,i,j,n)=0;
            }else{
                if (MGet(C,i,j,n)!=0){
                    MGet(IC,i,j,n)=1.0/MGet(C,i,j,n);
                }else{
                    MGet(IC,i,j,n)=0;
                }
            }
        }
    }
    return IC;
}

/*****************************************************************************************************************************/

/*============================ MV3Prod =================================*/
Vec3 kalmanfilter::MV3Prod(cMat3 C, cVec3 V, Vec3 Vout){
/*3*3矩阵与3维向量乘法
  参数：[in]C:3*3 C矩阵
        [in]V:3维向量V
        [out]Vout: C*V(V视为列向量)
  返回：Vout
*/

/*需求：
  类型: Mat3,Vec3,cMat3,cVec3
*/    

/* 创建时间: 20070528
   最近更改时间：20070528
   Copyright 2005-2010 SSTC
*/
    double Vtemp[3];
    int i;  
    for (i=0;i<3;i++){
        Vtemp[i] = C[i][0]*V[0] + C[i][1]*V[1] + C[i][2]*V[2];  
    }
    Vout[0]=Vtemp[0];
    Vout[1]=Vtemp[1];
    Vout[2]=Vtemp[2];
    return Vout;
}


/*============================ VM3Prod =================================*/
Vec3 kalmanfilter::VM3Prod(cVec3 V, cMat3 C, Vec3 Vout){
/*3维向量与3*3矩阵乘法
  参数：[in]V:3维向量V
        [in]C:3*3 C矩阵        
        [out]Vout: V*C(V视为行向量)
  返回：Vout
*/

/*需求：
  类型: Mat3,Vec3,cMat3,cVec3
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/
    double Vtemp[3];
    int i;  
    for (i=0;i<3;i++){
        Vtemp[i] = C[0][i]*V[0] + C[1][i]*V[1] + C[2][i]*V[2];  
    }
    Vout[0]=Vtemp[0];
    Vout[1]=Vtemp[1];
    Vout[2]=Vtemp[2];
    return Vout;
}


/*============================ MVProd =================================*/
Vec kalmanfilter::MVProd(cMat C, cVec V, Vec Vout, int nM, int nN){
/*矩阵与向量乘法
  参数：[in]C: C矩阵
        [in]V: 向量V
        [out]Vout: C*V(V视为列向量)
        [in]nM: C行数
        [in]nN: C列数,即V的维数
  返回：Vout
*/

/*需求：
  类型: Mat,Vec,cMat,cVec
  宏: MGet
  头文件: stdlib.h
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/
    Vec Vtemp=(Vec)malloc(sizeof(double)*nM);
    int i,j;    
    for (i=0;i<nM;i++){
        Vtemp[i]=0.0;
        for(j=0;j<nN;j++){
            Vtemp[i] = Vtemp[i]+MGet(C,i,j,nN)*V[j];    
        }
    }
    for (i=0;i<nM;i++){
        Vout[i]=Vtemp[i];
    }
    free(Vtemp);
    return Vout;
}


/*============================ VMProd =================================*/
Vec kalmanfilter::VMProd(cVec V, cMat C, Vec Vout, int nM, int nN){
/*向量与矩阵乘法
  参数：[in]V: 向量V
        [in]C: 矩阵C
        [out]Vout: V*C(V视为行向量)
        [in]nM: C行数,即V的维数
        [in]nN: C列数
  返回：Vout
*/

/*需求：
  类型: Mat,Vec,cMat,cVec
  宏: MGet
  头文件: stdlib.h
*/    

/* 创建时间: 20080402
   最近更改时间：20080402
   Copyright 2005-2010 SSTC
*/

    Vec Vtemp=(Vec)malloc(sizeof(double)*nN);
    int i,j;    
    for (i=0;i<nN;i++){
        Vtemp[i]=0.0;
        for(j=0;j<nM;j++){
            Vtemp[i] = Vtemp[i]+MGet(C,j,i,nN)*V[j];    
        }
    }
    for (i=0;i<nN;i++){
        Vout[i]=Vtemp[i];
    }
    free(Vtemp);
    return Vout;
}
/**************************************************************************************************************/

/*========================= VecCpy =================================*/
Vec kalmanfilter::VecCpy(Vec dest, cVec source, int nOrder){
/*向量拷贝
  参数：[out]dest: 目标向量
        [in]source: 源向量
        [in]nOrder: 向量维数
  返回：dest
*/

/*需求：
  类型: Vec,cVec
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        dest[i]=source[i];
    }
    return dest;
}


/*========================= nVecCpy =================================*/
nVec kalmanfilter::nVecCpy(nVec dest, cnVec source, int nOrder){
/*整型向量拷贝
  参数：[out]dest: 目标向量
        [in]source: 源向量
        [in]nOrder: 向量维数
  返回：dest
*/

/*需求：
  类型: nVec,cnVec
*/      

/*创建时间: 20080512
  最近更改时间：20080512
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        dest[i]=source[i];
    }
    return dest;
}
/*********************************************************************************************************/

/*========================= VecSum ====================================*/
Vec kalmanfilter::VecSum(cVec V1, cVec V2, int nOrder, Vec Vout){
/*向量加法
  参数：[in]V1: 向量1
        [in]V2: 向量2
        [in]nOrder: 向量1与向量2的维数
        [out]Vout: 向量1与向量2的和
  返回：Vout
*/

/*需求：
  类型: Vec,cVec
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        Vout[i]=V1[i]+V2[i];
    }
    return Vout;
}


/*========================= nVecSum ====================================*/
nVec kalmanfilter::nVecSum(cnVec V1, cnVec V2, int nOrder, nVec Vout){
/*整型向量加法
  参数：[in]V1: 整型向量1
        [in]V2: 整型向量2
        [in]nOrder: 向量1与向量2的维数
        [out]Vout: 向量1与向量2的和
  返回：Vout
*/

/*需求：
  类型: nVec,cnVec
*/      

/*创建时间: 20080512
  最近更改时间：20080512
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        Vout[i]=V1[i]+V2[i];
    }
    return Vout;
}
/*******************************************************************************************************************/

/*========================= VecMinus =================================*/
Vec kalmanfilter::VecMinus(cVec V1, cVec V2, int nOrder, Vec Vout){
/*向量减法
  参数：[in]V1: 向量1
        [in]V2: 向量2
        [in]nOrder: 向量1与向量2的维数
        [out]Vout: 向量1与向量2的差
  返回：Vout
*/

/*需求：
  类型: Vec,cVec
*/      

/*创建时间: 20070530
  最近更改时间：20070530
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        Vout[i]=V1[i]-V2[i];
    }
    return Vout;
}


/*========================= nVecMinus =================================*/
nVec kalmanfilter::nVecMinus(cnVec V1, cnVec V2, int nOrder, nVec Vout){
/*整型向量减法
  参数：[in]V1: 整型向量1
        [in]V2: 整型向量2
        [in]nOrder: 向量1与向量2的维数
        [out]Vout: 向量1与向量2的差
  返回：Vout
*/

/*需求：
  类型: nVec,cnVec
*/      

/*创建时间: 20080512
  最近更改时间：20080512
  Copyright 2006-2010 SSTC,HLW
  Email: nostalgica@163.com
*/
    int i;
    for (i=0;i<nOrder;i++){
        Vout[i]=V1[i]-V2[i];
    }
    return Vout;
}

/*******************************************************************************************************************/

/*========================= Kalman1 ====================================*/
Vec kalmanfilter::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_){
/*离散卡尔曼滤波的一步递推,系统方程：X(k+1)=Φ(k+1,k)X(k)+B(k)U(k)+Γ(k)W(k),Z(k+1)=H(k+1)X(k+1)+V(k+1)
  参数：[in]Phik_: Φ(k+1,k);n*n,n为阶数
        [in]Gamak_1: Γ(k);n*q,q为驱动噪声W(k)维数
        [in]Hk: H(k+1);m*n,m为观测维数
        [in]Qk_1: Q(k);q*q,驱动噪声W(k)协方差阵
        [in]Rk: R(k+1);m*m,观测噪声V(k+1)协方差阵
        [in]Pk_1: P(k,k);n*n
        [in]Xk_1: X(k,k);n维向量
        [in]Zk: Z(k+1);m维向量
        [in]Bk_1: B(k);n*p,p为输入维数
        [in]Uk_1: U(k);p维向量
        [in]n: 系统阶数
        [in]m: 观测维数
        [in]p: 输入维数
        [in]q: 驱动噪声维数
        [out]Xk: X(k+1,k+1);n维向量,若不需输出设为NULL
        [out]Pk: P(k+1,k+1);n*n,若不需输出设为NULL
        [out]Kk: K(k+1);n*m,若不需输出设为NULL
        [out]Pk_: P(k+1,k);n*n,若不需输出设为NULL
        [out]Xk_: X(k+1,k);n维向量,若不需输出设为NULL
  返回：Xk,若不成功,返回NULL
*/

/*需求：
  函数: MatTrans, MatProd, MatSum, MatInv, MatInitI, MatMinus, MVProd, VecSum, VecMinus, VecCpy, MatCpy
  头文件: stdlib.h
  宏: NULL
*/ 

/* 创建时间: 20080402
   最近更改时间：20080403
   Copyright 2005-2010 SSTC,HLW
   Email: nostalgica@163.com
*/

    int nmax=max(max(max(n,m),p),q);
    Mat tCn=(Mat)malloc(sizeof(double)*nmax*nmax);
    Mat tCn2=(Mat)malloc(sizeof(double)*nmax*nmax);
    Mat tCn3=(Mat)malloc(sizeof(double)*nmax*nmax);
    Mat tCn4=(Mat)malloc(sizeof(double)*nmax*nmax);
    Mat tCn5=(Mat)malloc(sizeof(double)*nmax*nmax);
    Vec tV=(Vec)malloc(sizeof(double)*nmax);
    Vec tV2=(Vec)malloc(sizeof(double)*nmax);
    
    /*Pk_=Phik_*Pk_1*Phik_'+Gamak_1*Qk_1*Gamak_1';
    Kk=Pk_*Hk'*(Hk*Pk_*Hk'+Rk)^(-1);
    I=eye(order);
    Pk=(I-Kk*Hk)*Pk_*(I-Kk*Hk)'+Kk*Rk*Kk';
    Xk_=Phik_*Xk_1+Bk_1*Uk_1;
    Xk=Xk_+Kk*(Zk-Hk*Xk_);
    */

    MatTrans(Phik_,tCn,n,n);
    MatProd(Pk_1,tCn,tCn2,n,n,n);
    MatProd(Phik_,tCn2,tCn,n,n,n);          
    MatTrans(Gamak_1,tCn2,n,q);
    MatProd(Qk_1,tCn2,tCn3,q,q,n);
    MatProd(Gamak_1,tCn3,tCn2,n,q,n);
    MatSum(tCn,tCn2,tCn,n,n);/*tCn:Pk_*/
    
    MatTrans(Hk,tCn2,m,n);
    MatProd(Hk,MatProd(tCn,tCn2,tCn3,n,n,m),tCn4,m,n,m);
    MatSum(tCn4,Rk,tCn3,m,m);
    if (MatInv(tCn3,tCn3,m)==NULL){
        return NULL;
    }
    MatProd(tCn,MatProd(tCn2,tCn3,tCn4,n,m,m),tCn2,n,n,m);/*tCn2:Kk*/
    MatInitI(tCn5,n);

    MatMinus(tCn5,MatProd(tCn2,Hk,tCn3,n,m,n),tCn3,n,n);
    MatTrans(tCn3,tCn4,n,n);
    MatProd(tCn3,MatProd(tCn,tCn4,tCn4,n,n,n),tCn3,n,n,n);

    MatProd(tCn2,MatProd(Rk,MatTrans(tCn2,tCn4,n,m),tCn4,m,m,n),tCn4,n,m,n);
    MatSum(tCn3,tCn4,tCn3,n,n);/*tCn3:Pk*/
    MVProd(Phik_,Xk_1,tV,n,n);
    MVProd(Bk_1,Uk_1,tV2,n,p);
    VecSum(tV,tV2,n,tV);/*tV:Xk_*/
    VecMinus(Zk,MVProd(Hk,tV,tV2,m,n),m,tV2);
    VecSum(tV,MVProd(tCn2,tV2,tV2,n,m),n,tV2);/*tV2:Xk*/

    
    if (Xk!=NULL){
        VecCpy(Xk,tV2,n);
    }
    if (Pk!=NULL){
        MatCpy(Pk,tCn3,n,n);
    }
    if (Kk!=NULL){
        MatCpy(Kk,tCn2,n,m);
    }
    if (Pk_!=NULL){
        MatCpy(Pk_,tCn,n,n);
    }
    if (Xk_!=NULL){
        VecCpy(Xk_,tV,n);
    }
    
    free(tCn);
    free(tCn2);
    free(tCn3);
    free(tCn4);
    free(tCn5);
    free(tV);
    free(tV2);
    return Xk;
}