hfusion01.h

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

#define M 2              //传感器测量维数
#define N 2              //待估计量维数（高度和高度偏差）
#define K 2350         
#define L 2*K            //待处理数据量
#define H0 0             //机场海拔高度
#define T 1              //系统离散化采样时间

int i,j,k,l,m,n,t;
double inv;
double pn[K][2],x[K][2]; //pn用来存储每一采样时刻被估计量的估计方差，
                         //x用来存储每一采样时刻高度和高度偏差的估计值
double xx[2],z[K][3];
double y[1][2];

static double I[3][3]={{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,10}};            //初始误差均方误差阵

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];