main.cpp
上传用户：wyp_nj
上传日期：2022-05-03
资源大小：484k
文件大小：2k
- // 本程序实现matrix类
- // 对matrix类的定义
- #include "matrix.h"
- #include "DR_GPS_EKF.h"
- void main(void)
- {
- double axdr,aydr; //DR得到加速度
- double dyaw; //DR得到的航向角速率
- double longps0=116.0666672/180*pi; //初始经度 ？？
- double latgps0=39.14999941/180*pi; //初始纬度 ？？
- double longps,latgps; //GPS得到的经纬度
- double vegps,vngps; //GPS得到的位置、速度
- double yaw=0; //航向角的初值(初始位置决定)？？
- double dr[7]={0,0,0,0,0,0,0}; //DR导航数据(位置速度角度)初值为0
- double gps[6]={0,0,0,0,0,0}; //GPS导航数据(位置速度角度)初值为0
- FILE *fpdr,*fdr; //打开DR数据文件的指针
- FILE *fpgps,*fgps; //打开GPS数据文件的指针
- FILE *fekf; //写入Kalman滤波数据的指针
- if((fpgps=fopen("gps.txt","rb"))!=NULL) //打开DR数据文件
- cout<<"can open the file gps.txt"<<"n"; //打开GPS数据文件
- if((fpdr=fopen("dr.txt","rb"))!=NULL)
- cout<<"can open the file dr.txt"<<"n";
- if((fdr=fopen("DR_.txt","w"))!=NULL)
- cout<<"can open the file DR.txt"<<"n";
- if((fgps=fopen("GPS_.txt","w"))!=NULL)
- cout<<"can open the file GPS.txt"<<"n";
- if((fekf=fopen("EKF.txt","w"))!=NULL)
- cout<<"can open the file EKF.txt"<<"n";
-
- while( !feof( fpdr ) ) //采6000组数据
- {
- fscanf(fpgps,"%lf %lf %lf %lf",&longps,&latgps,&vegps,&vngps);//读入GPS的采集数据:东北向加速度和经纬度
- GPS(gps,vegps,vngps,longps,latgps,longps0,latgps0);//改变GPS导航数据(位置速度)
- fprintf(fgps,"%lf %lf %lf %lfn",gps[0],gps[1],gps[2],gps[3],gps[4],gps[5]);
- fscanf(fpdr,"%lf %lf %lf",&dyaw,&axdr,&aydr );//读入DR的采集数据:X、Y轴上的加速度和航向角速率
- DR(dr,axdr*g,aydr*g,dyaw); //改变DR导航数据(位置速度加速度角度)
- fprintf(fdr,"%lf %lf %lf %lf %lfn",dr[0],dr[1],dr[2],dr[3],dr[6]);
- EKF(dr,gps,ekf,longps0,latgps0); //Kalman函数改变组合导航数据和DR数据
- fprintf(fekf,"%lf %lf %lf %lf %lf %lf %lf %lf %lfn",
- ekf[0],ekf[1],ekf[2],ekf[3],ekf[4],
- ekf[5],ekf[6],ekf[7],ekf[8]);
- //？(); //调用画图函数
- }
- fclose( fpdr ); //关闭dr数据文件
- fclose( fpgps ); //关闭gps数据文件
- fclose( fpdr ); //关闭DR数据文件
- fclose( fgps ); //关闭GPS数据文件
- fclose( fekf); //关闭EKF数据文件
- }
English
