hfusion0.h

无人机高度基于matlab卡尔曼滤波程序 数据融合

#define M 2
#define N 2
#define L 12192
#define K 3048
#define H0 1100
#define T 0.5
	
int i,j,k,l,m,n;
double inv;
double pn[K][2],x[K][2]; //pn用来存储每一采样时刻被估计量的估计方差，
//                            x用来存储每一采样时刻高度和高度偏差的估计值
double xx[2]={1750,30},z[K][2];       //xx为初始高度和初始高度偏差，z为每一时刻的传感器测量值
double y[1][2];
static double I[2][2]={{1,0},{0,1}};
static double a[2][2]={{1,0},{0,1}};     //一步转移矩阵
static double b[2][2]={{T,0},{0,T}};
static double c[2][2]={{1,1},{1,0}};     //量测矩阵
static double q[2][2]={{36/T,0},{0,9/T}};    //系统噪声方差矩阵
static double r[2][2]={{25/T,0},{0,16/T}};   //量测噪声方差矩阵
static double p[2][2]={{40,0},{0,20}};   //初始误差均方误差阵

double xs[2][1],ps[2][2];
        
double a1[N][N],p1[N][N],p2[N][N],b1[N][N],q1[N][N],q2[N][N];

double c1[N][M],ps1[M][N],ps2[M][M],ps3[M][M],f[M][M];

double ps4[N][M],h[N][M];

//double zz[M][L];

double ro[M][1];

double xs1[N][1],c2[M][N],xs2[M][1],xs3[M][1],h1[M][1];

double h2[N][N],h3[N][N];

double yo[K][3];