hfusion0.c

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

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

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

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

FILE *hKal;
char str3[512];

FILE *hKalh;
char str4[512];

FILE *hKalbh;
char str5[512];

FILE *hKalp;
char str6[512];

void Readdata1();
void Readdata2();

void Writedata();
void Writedata1();
void Writedata2();
void Writedata3();

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_trans(double **a,int m,int n,double **b);


	
main()
{
	Readdata1();
	Readdata2();


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

	for(n=0;n<=K;n++)
	{
		z[m][0]=fh2[n];
		m++;
	}


	for(n=0;n<=L;n+=4)	
	{			
		z[l][1]=fh1[n]+H0;
		l++;
	}


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

        
        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,3,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();
	Writedata1();
	Writedata2();
	Writedata3();
}


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

	if((hALT = fopen("hALT2.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
			{
				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(hALT);
}


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

	if((hADC = fopen("hADC2.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
			{
				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(hADC);
}


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(str3,"%d	%f	%f	%f\t\n", ft2[i], yo[i][0], yo[i][1], yo[i][2]);
		fwrite(str3, 1, strlen(str3), hKal);
	}
	fclose(hKal);
}


void Writedata1()
{
	int i;

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


void Writedata2()
{
	int i;

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


void Writedata3()
{
	int i;

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


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_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);
}