Shao Rui
/
Kalman
1
Revision 0:c55310328157, committed 2020-02-07
- Comitter:
- shaorui
- Date:
- Fri Feb 07 11:34:24 2020 +0000
- Commit message:
- 1
Changed in this revision
diff -r 000000000000 -r c55310328157 defining.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/defining.h Fri Feb 07 11:34:24 2020 +0000 @@ -0,0 +1,80 @@ +#ifndef __defining__ +#define __defining__ + +/*===============================================================*/ +/*define type +*/ + + +/*bool type*/ +enum Bool_{ + False=0, + True=1, +}; +typedef enum Bool_ Bool; + + +/*矩阵,向量,四元数*/ +typedef double (*Mat3)[3]; +typedef double *Vec3,*Quat,*Vec,*Mat,*Point,*Point3; +typedef const double (*cMat3)[3]; +typedef const double *cVec3,*cQuat,*cVec,*cMat,*cPoint,*cPoint3; +typedef int (*nMat3)[3]; +typedef int *nVec3,*nQuat,*nVec,*nMat,*nPoint,*nPoint3; +typedef const int (*cnMat3)[3]; +typedef const int *cnVec3,*cnQuat,*cnVec,*cnMat,*cnPoint,*cnPoint3; +/*复数*/ +typedef struct Complex_{ + double real; + double img; +}Complex; + + +/*===============================================================*/ +/*宏定义 +*/ +#ifndef NULL +#define NULL 0 +#endif +#ifndef MAX_PATH +#define MAX_PATH 260 +#endif +/* +#ifndef UNUSED +#define UNUSED(x) (x=x) +#endif */ +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef max +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef MGet +#define MGet(p,i,j,n) (p[(i)*(n)+(j)]) /*n列矩阵的(i,j)元素*/ +#endif + + +/*===============================================================*/ +/*数学常数宏 + pi - 圆周率 + pi2 - pi的两倍 + ee - 自然对数底 +*/ + +/*创建时间: 20061122 + 最近更改时间:20070525 + Copyright 2006-2010 SSTC +*/ +#ifndef pi +#define pi 3.1415926535897932384626433832795 +#endif +#ifndef pi2 +#define pi2 6.2831853071795864769252867665590 +#endif +#ifndef ee +#define ee 2.7182818284590455348848081484903 +#endif + + + +#endif
diff -r 000000000000 -r c55310328157 kalmanfilter/kalmanfilter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalmanfilter/kalmanfilter.cpp Fri Feb 07 11:34:24 2020 +0000 @@ -0,0 +1,1243 @@ +#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; +} \ No newline at end of file
diff -r 000000000000 -r c55310328157 kalmanfilter/kalmanfilter.h --- /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
diff -r 000000000000 -r c55310328157 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 07 11:34:24 2020 +0000 @@ -0,0 +1,75 @@ +#include "mbed.h" +#include "stdio.h" +#include "kalmanfilter.h" +kalmanfilter kalman_filter; + +int main() { + /*----------------------------------------------*/ + /*------ Kalman:KF3 ---------*/ + /*----------------------------------------------*/ + double Tint=0.5; //步长 + double qq=20.0; //状态噪声; + double r=15.0*15.0;//量测噪声 + double x0=0; //初角度估计 + double v0=20.0; //初始角速度估计 + //double z[]={ 9.2720, 13.513, 6.1300, 35.376, 76.399, 53.196, 71.301, 61.941, 94.043, 100.70, 112.13, 138.60, 128.03, 185.08, 159.94, 169.56, 191.93, 177.63, 206.30, 212.89, 183.21, 259.21, 233.50, 282.24, 264.88, 238.05, 301.91, 323.77, 324.30, 349.54, 341.17, 357.74, 387.27, 341.74, 329.06, 377.47, 306.21, 299.88, 287.88, 269.47, 268.20, 295.22, 198.59, 230.01, 267.01, 197.88, 182.84, 199.56, 147.22, 141.72, 141.17, 111.40, 139.92, 108.83, 77.893, 89.428, 63.567, 71.060, 52.668, 38.430, 12.085, -.90100, 24.456, 8.9650, 48.318, 24.895, 77.915, 68.884, 38.276, 102.70, 83.452, 165.39, 131.92, 131.94, 162.00, 148.20, 170.82, 182.15, 157.87, 214.37, 228.80, 264.62, 234.54, 248.40, 279.18, 288.44, 284.99, 288.35, 260.88, 386.52, 343.78, 339.11, 360.26, 374.39, 342.58, 314.23, 325.68, 318.40, 276.20, 271.67, 284.77, 249.61, 252.33, 217.99, 233.09, 268.20, 121.97, 172.35, 171.80, 180., 158.09, 140.76, 132.06, 120.83, 89.714, 114.43, 75.168, 84.507, 132.58, 6.2840, 37.705, 27.114, -69.126, 13.381, -5.0320, 11.470, 85.737, 48.845, 30.234, 48.604, -2.5710, 142.25, 108.70, 103.38, 110.79, 68.467, 159.41, 166.86, 174.31, 167.37, 119.62, 271.54, 220.01, 229.39, 221.68, 174.84, 319.50, 274.70, 269.74, 299.71, 282.64, 352.31, 324.82, 300.52, 424.20, 356.93, 368.81, 352.97, 318.94, 338.38, 320.71, 306.19, 278.28, 264.44, 283.42, 234.25, 230.12, 244.88, 227.77, 212.85, 191.38, 192.90, 248.25, 155.24, 150.95, 196.96, 73.301, 112.10, 150.75, 30.608, 74.905, 110.57, -3.6260, 39.705, 83.189, -23.950, 10.042, 16.216, 31.992, 15.359, 16.260, 15.623, 16.699, 15.513, 33.552, 15.359, 15.601}; + //int lengthz=197; + double z[]={1.659,17.561,31.413,43.155,67.151,105.516,103.745,132.845,138.935,162.869,161.13,205.885,218.191,232.747,275.592,268.136,371.82,298.458,328.422,319.787,369.346,311.032,317.027,292.319,255.571,267.894,216.225,223.599,198.355,175.846,173.86,126.815,124.489,140.198,64.702,72.812,46.116,47.203,-18.359,11.883,37.706,54.226,58.315,61.125,118.907,117.955,156.83,137.986,177.09,205.236,214.884,222.625,208.478,277.71,268.287,308.305,337.767,373.236,367.02,372.394,340.542,323.503,324.125,270.119,313.02,201.609,225.589,232.815,185.559,190.431,114.25,130.986,131.814,110.624,86.737,54.18,65.865,-20.491,1.34,-13.512,78.877,54.287,80.24,79.791,86.722,105.775,132.421,159.287,167.547,191.528,221.675,243.396,244.226,238.432,284.327,295.521,321.958,327.083,358.257}; + int lengthz=99; + + + int i; + FILE* fout; + double Zk[1]; + double Phik_[3][3]; + double Gamak_1[]={1.0000, 0.0000, 0.0000, + 0.0000, 1.0, 0.0000, + 0.0000, 0.0000, 1.000}; + double Hk[]={1.0000, 0.0, 0.0}; + double Qk_1[3][3]; + double Rk[1][1]; + double Pk_1[]={ .12960e7, 0., 0., + 0., .12960e-1, 0., + 0., 0., .12960e-4}; + double Xk_1[3]; + double Bk_1[3][1]; + double Uk_1[]={0.0}; + double Xk[3]; + double Pk[3*3]; + double Kk[3*1]; + double Pk_[3*3]; + double Xk_[3]; + int n=3;int m=1; + int q=3;int p=1; + + Phik_[0][0]=1.0; Phik_[0][1]=Tint; Phik_[0][2]=Tint*Tint/2.0; + Phik_[1][0]=0.0; Phik_[1][1]=1.0; Phik_[1][2]=Tint; + Phik_[2][0]=0.0; Phik_[2][1]=0.0; Phik_[2][2]=1.0; + + Qk_1[0][0]=pow(Tint,6)/36.0*qq; Qk_1[0][1]=pow(Tint,5)/12.0*qq; Qk_1[0][2]=pow(Tint,4)/6.0*qq; + Qk_1[1][0]=pow(Tint,5)/12.0*qq; Qk_1[1][1]=pow(Tint,4)/4.0*qq; Qk_1[1][2]=pow(Tint,3)/2.0*qq; + Qk_1[2][0]=pow(Tint,4)/6.0*qq; Qk_1[2][1]=pow(Tint,3)/2.0*qq; Qk_1[2][2]=pow(Tint,2)*qq; + Rk[0][0]=r; + Xk_1[0]=x0; Xk_1[1]=v0; Xk_1[2]=0; + Bk_1[0][0]=pow(Tint,3)/6.0;Bk_1[1][0]=pow(Tint,2)/2.0;Bk_1[2][0]=Tint; + + //输出 + fout=fopen("KF3.txt","w"); + for (i=0;i<=lengthz;i++){ + Zk[0]=z[i]; + kalman_filter.Kalman1((Mat)Phik_, (Mat)Gamak_1, (Mat)Hk, (Mat)Qk_1, (Mat)Rk, (Mat)Pk_1, (Vec)Xk_1, (Vec)Zk, (Mat)Bk_1, (Vec)Uk_1, + n, m, q, p, (Vec)Xk, (Mat)Pk, (Mat)Kk, (Mat)Pk_, (Vec)Xk_); + + kalman_filter.VecCpy((Vec)Xk_1,(Vec)Xk,n); + kalman_filter.VecCpy((Vec)Pk_1,(Vec)Pk,n*n); + + printf("%.5f\n",Xk[0]); + fprintf(fout,"%.5f\n",Xk[0]); + + + + } + fclose(fout); + + } +
diff -r 000000000000 -r c55310328157 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Feb 07 11:34:24 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file