hfusion01.c

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

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "Hfusion01.h"

FILE *hADC;
char str1[512];
char buf1[32];
char buf2[32];
int f1;
double f2;
int ft1[L];
double fh1[L];

FILE *hALT;
char str2[512];
char buf3[32];
char buf4[32];
int f3;
double f4;
int ft2[L];
double fh2[L];

FILE *hDGPS;
char str3[512];
char buf5[32];
char buf6[32];
int f5;
double f6;
int ft3[K];
double fh3[K];

FILE *hKal;
char str4[512];

void Readdata1();
void Readdata2();
void Readdata3();
//读取试飞数据

void Writedata();
//存储结果

double matrix_add(double **a,double **b,int m,int n,double **c);
double matrix_sub(double **a,double **b,int m,int n,double **c);
double matrix_mul1(double (*a)[2],double (*b)[2],int m,int n,int k,double (*c)[2]);
double matrix_mul2(double (*a)[2],double (*b)[3],int m,int n,int k,double (*c)[3]);
double matrix_mul3(double (*a)[3],double (*b)[2],int m,int n,int k,double (*c)[2]);
double matrix_mul4(double (*a)[3],double (*b)[3],int m,int n,int k,double (*c)[3]);
double matrix_mul5(double (*a)[2],double (*b)[1],int m,int n,int k,double (*c)[1]);
double matrix_mul6(double (*a)[3],double (*b)[1],int m,int n,int k,double (*c)[1]);
double matrix_mul7(double (*a)[3],double (*b)[2],int m,int n,int k,double (*c)[3]);
double matrix_trans(double **a,int m,int n,double **b);
//Kalman滤波中涉及的矩阵运算

main()
{
	Readdata1();
	for(n=0;n<L;n+=2)
	{
		z[m][0] = fh1[n];
		m++;
	}
	//读取大气机测量值并保存在z[K][3]的第1列

	Readdata2();
	for(n=0;n<L;n+=2)
	{
		z[l][1] = fh2[n]+H0;
		l++;
	}
	//读取大气机测量值并保存在z[K][3]的第2列

	Readdata3();
	for(n=0;n<K;n++)
	{
		z[t][2] = fh3[n];
		t++;
	}
	//读取大气机测量值并保存在z[K][3]的第3列

	xx[0] = z[0][1];
	xx[1] = z[0][0]-z[0][1];
	
	for(i=0;i<=N-1;i++)
    {
        pn[0][i]=p[i][i];
        x[0][i]=xx[i];
    }

	for(k=0;k<K;k++)
	{
		for(i=0;i<=N-1;i++)
		{
			xs[i][0]=xx[i];
		}

		matrix_trans((double**)a,2,2,(double**)a1);
        matrix_mul1(a,p,2,2,2,p1);
        matrix_mul1(p1,a1,2,2,2,p2);
		matrix_trans((double**)b,2,2,(double**)b1);
		matrix_mul1(b,q,2,2,2,q1);
		matrix_mul1(q1,b1,2,2,2,q2);
        matrix_add((double**)p2,(double**)q2,2,2,(double**)ps);

        matrix_mul1(c,ps,2,2,2,ps1);
		matrix_trans((double**)c,2,2,(double**)c1); 
		matrix_mul1(ps1,c1,2,2,2,ps2);
		matrix_add((double**)ps2,(double**)r,2,2,(double**)ps3);

        //求二阶矩阵ps3的逆，逆阵保存到f阵
   	    inv=ps3[0][0]*ps3[1][1]-ps3[0][1]*ps3[1][0];
   		f[0][0]=ps3[1][1]/inv;
   		f[1][1]=ps3[0][0]/inv;
   
   		f[0][1]=-ps3[0][1]/inv;
   		f[1][0]=-ps3[1][0]/inv;  
		
        matrix_mul1(ps,c1,2,2,2,ps4);
		matrix_mul1(ps4,f,2,2,2,h);
        

/*      for(j=0;j<=M-1;j++)
		{
			y[0][j]=z[k][j];
		}*/
		y[0][0] = z[k][0];
		y[0][1] = z[k][1];
        
        matrix_trans((double**)y,1,2,(double**)ro);

        matrix_mul5(a,xs,2,2,1,xs1);
		matrix_mul1(c,a,2,2,2,c2);
		matrix_mul5(c2,xs,2,2,1,xs2);    
		matrix_sub((double**)ro,(double**)xs2,2,1,(double**)xs3);
		matrix_mul5(h,xs3,2,2,1,h1);
		

		xx[0]=xs1[0][0]+h1[0][0];
		xx[1]=xs1[1][0]+h1[1][0];
		
		
		matrix_mul1(h,c,2,2,2,h2);
		matrix_sub((double**)I,(double**)h2,2,2,(double**)h3);
		matrix_mul1(h3,ps,2,2,2,p);
        

        for(j=0;j<=N-1;j++)
        {
            pn[k][j]=p[j][j];
            x[k][j]=xx[j];
        } 


        yo[k][0]=x[k][0];
        yo[k][1]=x[k][1];
        yo[k][2]=pn[k][0];
		
		printf("%f  %f  %f\n",yo[k][0],yo[k][1],yo[k][2]);

		Writedata();
	}
}

void Readdata1()
{
	int i,j;
	int ch;
	int bGetLine;

	if((hADC = fopen("hADC.txt","r"))==NULL)
	{
		printf("cannot open the file!\n");
		return;
	}	
	
	j = 0;
	while(!feof(hADC))
	{
		i = 0;
		bGetLine = 1;
		while(1)
		{
			ch = fgetc(hADC);
			if(ch=='\n' || ch==EOF)
			{
				break;
			}
			else
			{
				str1[i] = (char)ch;
				if(bGetLine)
				{
					if(((str1[i]>='0') && (str1[i]<='9')) || (str1[i]=='.'))
					{
						bGetLine = 0;
						i++;
					}
				}
				else
				{
					i++;
				}
			}
		}
		if(i != 0)
		{
			str1[i] = '\0';
			buf1[0] = '\0';
			buf2[0] = '\0';
			sscanf(str1,"%[^	]%s",buf1,buf2);
			f1=atoi(buf1);
			f2=atof(buf2);
			ft1[j]=f1;
			fh1[j]=f2;
		}
		j++;
	}
	fclose(hADC);
}

void Readdata2()
{
	int i,j;
	int ch;
	int bGetLine;

	if((hALT = fopen("hALT.txt","r"))==NULL)
	{
		printf("cannot open the file!\n");
		return;
	}	
	
	j = 0;
	while(!feof(hALT))
	{
		i = 0;
		bGetLine = 1;
		while(1)
		{
			ch = fgetc(hALT);
			if(ch=='\n' || ch==EOF)
			{
				break;
			}
			else
			{
				str2[i] = (char)ch;
				if(bGetLine)
				{
					if(((str2[i]>='0') && (str2[i]<='9')) || (str2[i]=='.'))
					{
						bGetLine = 0;
						i++;
					}
				}
				else
				{
					i++;
				}
			}
		}
		if(i != 0)
		{
			str2[i] = '\0';
			buf3[0] = '\0';
			buf4[0] = '\0';
			sscanf(str2,"%[^	]%s",buf3,buf4);
			f3=atoi(buf3);
			f4=atof(buf4);
			ft2[j]=f3;
			fh2[j]=f4;
		}
		j++;
	}
	fclose(hALT);
}

void Readdata3()
{
	int i,j;
	int ch;
	int bGetLine;

	if((hDGPS = fopen("hDGPS.txt","r"))==NULL)
	{
		printf("cannot open the file!\n");
		return;
	}	
	
	j = 0;
	while(!feof(hDGPS))
	{
		i = 0;
		bGetLine = 1;
		while(1)
		{
			ch = fgetc(hDGPS);
			if(ch=='\n' || ch==EOF)
			{
				break;
			}
			else
			{
				str3[i] = (char)ch;
				if(bGetLine)
				{
					if(((str3[i]>='0') && (str3[i]<='9')) || (str3[i]=='.'))
					{
						bGetLine = 0;
						i++;
					}
				}
				else
				{
					i++;
				}
			}
		}
		if(i != 0)
		{
			str3[i] = '\0';
			buf5[0] = '\0';
			buf6[0] = '\0';
			sscanf(str3,"%[^	]%s",buf5,buf6);
			f5=atoi(buf5);
			f6=atof(buf6);
			ft3[j]=f5;
			fh3[j]=f6;
		}
		j++;
	}
	fclose(hDGPS);
}

void Writedata()
{
	int i;

	if((hKal = fopen("hKal.txt","w"))==NULL)
	{
		printf("cannot open the file!\n");
		return;
	}
	
	for(i=0;i<K;i++)
	{
		sprintf(str4,"%d	%f	%f	%f	%f\t\n", ft3[i], yo[i][0], yo[i][1], yo[i][2], z[i][0]);
		fwrite(str4, 1, strlen(str4), hKal);
	}
	fclose(hKal);
}


double matrix_add(double **a,double **b,int m,int n,double **c)
{
    int i,j;
    for(i=0;i<=m-1;i++)
        for(j=0;j<=n-1;j++)
            *((double*)c+n*i+j)=*((double*)a+n*i+j)+*((double*)b+n*i+j);
    return(1);
}


double matrix_sub(double **a,double **b,int m,int n,double **c)
{
    int i,j;
    for(i=0;i<=m-1;i++)
        for(j=0;j<=n-1;j++)
            *((double*)c+n*i+j)=*((double*)a+n*i+j)-*((double*)b+n*i+j);
    return(1);
}


double matrix_mul1(double (*a)[2],double (*b)[2],int m,int n,int k,double (*c)[2])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}


double matrix_mul2(double (*a)[2],double (*b)[3],int m,int n,int k,double (*c)[3])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}


double matrix_mul3(double (*a)[3],double (*b)[2],int m,int n,int k,double (*c)[2])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}


double matrix_mul4(double (*a)[3],double (*b)[3],int m,int n,int k,double (*c)[3])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}


double matrix_mul5(double (*a)[2],double (*b)[1],int m,int n,int k,double (*c)[1])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}


double matrix_mul6(double (*a)[3],double (*b)[1],int m,int n,int k,double (*c)[1])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}

double matrix_mul7(double (*a)[3],double (*b)[2],int m,int n,int k,double (*c)[3])
{
    int i,j,l;
    for(i=0;i<m;i++)
    {
        for(j=0;j<k;j++)
        {
            *(((double*)(c+i))+j)=0.0;
            for(l=0;l<n;l++)
			{
				*(*(c+i)+j)+=(*(*(a+i)+l))*(*(*(b+l)+j));
			}
		}
	}
	return(1);
}

double matrix_trans(double **a,int m,int n,double **b)
{
    int i,j;
    for(i=0;i<=m-1;i++)
        for(j=0;j<=n-1;j++)
            *((double*)b+m*j+i)=*((double*)a+n*i+j);
    return(1);
}