利用严恭敏老师工具箱测试磁力计初始对准方案

组合导航初始对准方案是个很值得研究的问题，这里浅介绍下用严老师代码中的磁力计来进行初始航向对比，诸君可以作为参考。ppt中的视频在我主页视频里有
结果如下：都是进行转化的

代码使用的严老师的c++代码：

void SingleGps::PosAttInit(CVect3 &att_init,u8 alignTime_s,bool &isInit,CVect3 &posInit,bool &isAHRS)
{
	cout<<"进行初始化了"<<endl;
	CQuat qnb0(1,0,0,0); att_init=O31;
	IMU9 imuData; GpsDataAddTime gpsData;//
	double gpsYaw=0.0,accBuf[3]={0.0},magBuf[3]={0.0},gyroBuf[3]={0.0},posBuf[3]={0.0};
	int cnt = alignTime_s / TS,tmp=cnt;
	bool hasGpsData = false,hasImuData = false;//标志位
	
	ros::Time start_time = ros::Time::now();
	ros::Duration total_time(alignTime_s); // 3 seconds
	do
	{
		hasImuData = checkIMUData(imuData);
        hasGpsData = checkGPSData(gpsData);
		//cout<<fixed<<setprecision(8)<<"磁力计："<<imuData.mag_x<<" "<<imuData.mag_y<<" "<<imuData.mag_z<<endl;
		int count=0;
		if(hasImuData)//获取imu9数据
		{
			accBuf[0]+=imuData.acc_x;accBuf[1]+=imuData.acc_y;accBuf[2]+=imuData.acc_z;
			gyroBuf[0]+=imuData.gyro_x;gyroBuf[1]+=imuData.gyro_y;gyroBuf[2]+=imuData.gyro_z;
			magBuf[0]+=imuData.mag_x;magBuf[1]+=imuData.mag_y;magBuf[2]+=imuData.mag_z;
			tmp--;
			hasImuData=false;
		}
		if(hasGpsData && newGpsDataAvailable && gpsData.gpsdata.fix==4)
		{
			posBuf[0]=gpsData.gpsdata.latitude;
			posBuf[1]=gpsData.gpsdata.longitude;
			posBuf[2]=gpsData.gpsdata.altitude;
		}
		
	}while(tmp);
	ros::Time end_time = ros::Time::now();
	ros::Duration duration = end_time - start_time;
	cout<<"初始化所用时间："<<duration.toSec()<<endl;
	// //获取几秒数据的平均值
	CVect3 Acc = CVect3(accBuf) / cnt;
    //陀螺仪数据
    CVect3 Gyro= CVect3(gyroBuf) / cnt;
	//磁力计数据
	CVect3 Mag = CVect3(magBuf) / cnt;
	//位置
	posInit=CVect3(posBuf);//获取初始状态
	cout<<fixed<<setprecision(8)<<"初始化的位置："<<posInit.i<<" "<<posInit.j<<endl;
	//将其他方向安装的IMU转换为载体“右前上”方向 FRD是当前坐标系，这个函数是把它转化为右前上坐标系
	//IMURFU(&Mag,1,"RFU");
    //进行椭球拟合
   	Mag = (Mag-b)*A;//A为软磁校正，b为硬磁校正
	//初始姿态获取
	att_init.i=asin(Acc.j/G0);
	att_init.j=-atan2(Acc.i,Acc.k);
	Mag = a2qua(att_init) * Mag;

	double yaw = atan2Ex(Mag.i,Mag.j) + declination*DEG;  //可以直接调用老师的 magyaw函数
	if(yaw>M_PI)        yaw = yaw - 2*M_PI;
	else if(yaw<-M_PI)  yaw = yaw + 2*M_PI;
	att_init.k = yaw;
	isInit=true;//已经完成了初始化
	isAHRS=true;
}

截取了我的代码，这里运用了严老师的代码，因为是ros环境，数据输入啥的是用的回调函数来的，可以根据自己系统来。可以看下大致思路。
需要指出的是，如果磁力计不进行椭球拟合，效果会差很多。拟合代码我结合了rosbag的特征和西工大朱同学的代码，如下：
可以提供一下下面的代码作为参考，请放置在psins工具箱里，有不少函数需要用的严老师工具箱里的东西。

具体代码我已经传到自己首页，自己免费下载就成，只要换成自己采集到的数据就成，可以rosbag也可以是txt啥形式，自己改下就成
可以看下c++代码，是完全依赖磁力计，抗干扰能力奇差，所以实际落地会比较困难