Shao Rui
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
--- /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
--- /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
--- /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
--- /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);
+
+ }
+
--- /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