dansan.cpp

惯性导航原理 单子样和三子样算法比较分析

//比较在高动态条件下三子样与单子样算法的性能。
#include<iostream.h>
#include<math.h>
#define pi 3.1416
const int N=3;
const int M=4;
//三子样与单子样算法的性能
void xingneng(double t,double a,double w,double h)
{
	
	//四元素初始值
	double chushi[M]={cos(a/2),0,sin(a/2)*cos(w*t),sin(a/2)*sin(w*t)};

	//机体坐标系b相对参考坐标系R的角速度
    double Wb[N]={-2*w*sin(a/2)*sin(a/2),-w*sin(a)*sin(w*t),w*sin(a)*cos(w*t)};

	//单子样旋转矢量
    int i;
    double E=2*w*h*sin(a/2)*sin(a/2);
	double F=2*sin(a)*sin(w*h/2);
    double anglezone[N]={-E,F*sin(w*t+w*h/2),F*cos(w*t+w*h/2)};
	double vector1[N];
    for(i=0;i<N;i++)
           vector1[i]=anglezone[i];
	//优化三子样旋转矢量
    int k1=9/20,k2=27/40;
	double a_1[N],b_1[N],d_1[N],b[N],d[N],vector3[N];
	double E1=2/3*w*h*sin(a/2)*sin(a/2);
	double F1=2*sin(a)*sin(w*h/6);
	double anglez1[N]={-E1,-F1*sin(w*t+w*h/6),F1*cos(w*t+w*h/6)};
    double anglez2[N]={-E1,-F1*sin(w*t+w*h/2),F1*cos(w*t+w*h/2)};
    double anglez3[N]={-E1,-F1*sin(w*t+5*w*h/6),F1*cos(w*t+5*w*h/6)};
	double anglez_1[N][N]={{0,F1*cos(w*t+w*h/6),F1*sin(w*t+w*h/6)},{-F1*cos(w*t+w*h/6),0,-E1},{-F1*sin(w*t+w*h/6),E1,0}};
	double anglez_2[N][N]={{0,F1*cos(w*t+w*h/2),F1*sin(w*t+w*h/2)},{-F1*cos(w*t+w*h/2),0,-E1},{F1*sin(w*t+w*h/2),E1,0}};
    
    for(i=0;i<N;i++)
	      a_1[i]=anglez3[i]-anglez1[i];//anglez3减去anglez1
    for(i=0;i<N;i++)
	      b_1[i]=anglez3[0]*anglez_1[0][i]+anglez3[1]*anglez_1[1][i]+anglez3[2]*anglez_1[2][i];//anglez3乘anglez-1
          d_1[i]=a_1[0]*anglez_2[0][i]+a_1[1]*anglez_2[1][i]+a_1[2]*anglez_2[2][i];//a_1乘anglez2
    for(i=0;i<N;i++)
	      b[i]=k1*b_1[i];
          d[i]=k2*d_1[i];
    for(i=0;i<N;i++)
          vector3[i]=anglez1[i]+anglez2[i]+anglez3[i]+b[i]+d[i];

   //由t到t+h，机体坐标系的更新四元素
	
    double qh[M]={1-2*sin(a/2)*sin(a/2)*sin(w*h/2)*sin(w*h/2),-sin(a/2)*sin(a/2)*sin(w*h),
	-sin(a)*sin(w*h/2)*sin(w*t+w*h/2),sin(a)*sin(w*h/2)*cos(w*t+w*h/2)};
//姿态误差四元素

//单子样
	double vector0=vector1[1]*vector1[1]+vector1[2]*vector1[2]+vector1[3]*vector1[3];
	double vector=sqrt(vector0);
     
	double siyuansu1_0=cos(vector/2);
	double siyuansu1_1=(vector1[1]/vector)*sin(vector/2);
	double siyuansu1_2=(vector1[2]/vector)*sin(vector/2);
	double siyuansu1_3=(vector1[3]/vector)*sin(vector/2);


    double dan1=2*(siyuansu1_1*siyuansu1_2-siyuansu1_0*siyuansu1_3);
	double dan2=siyuansu1_0*siyuansu1_0-siyuansu1_1*siyuansu1_1+siyuansu1_2*siyuansu1_2-siyuansu1_3*siyuansu1_3;
	double dan3=2*(siyuansu1_2*siyuansu1_3+siyuansu1_0*siyuansu1_1);
	double dan4=-2*(siyuansu1_1*siyuansu1_3-siyuansu1_0*siyuansu1_2);
	double dan5=siyuansu1_0*siyuansu1_0-siyuansu1_1*siyuansu1_1-siyuansu1_2*siyuansu1_2+siyuansu1_3*siyuansu1_3;
	
	double pianhang1=atan2(dan1,dan2);
	double zongyao1=asin(dan3);
	double hengyao1=atan2(dan4,dan5);
//三子样
	double siyuansu3_0=qh[0]+(1/2)*(qh[1]*vector3[1]+qh[2]*vector3[2]-qh[3]*vector3[3]);
    double siyuansu3_1=qh[1]+(1/2)*(-qh[0]*vector3[1]+qh[3]*vector3[2]-qh[2]*vector3[3]);
	double siyuansu3_2=qh[2]+(1/2)*(-qh[3]*vector3[1]-qh[0]*vector3[2]+qh[1]*vector3[3]);
	double siyuansu3_3=qh[3]+(1/2)*(qh[2]*vector3[1]-qh[1]*vector3[2]-qh[0]*vector3[3]);

    double san1=2*(siyuansu3_1*siyuansu3_2-siyuansu3_0*siyuansu3_3);
	double san2=siyuansu3_0*siyuansu3_0-siyuansu3_1*siyuansu3_1+siyuansu3_2*siyuansu3_2-siyuansu3_3*siyuansu3_3;
	double san3=2*(siyuansu3_2*siyuansu3_3+siyuansu3_0*siyuansu3_1);
	double san4=-2*(siyuansu3_1*siyuansu3_3-siyuansu3_0*siyuansu3_2);
	double san5=siyuansu3_0*siyuansu3_0-siyuansu3_1*siyuansu3_1-siyuansu3_2*siyuansu3_2+siyuansu3_3*siyuansu3_3;
	
	double pianhang3=atan2(san1,san2);
	double zongyao3=asin(san3);
	double hengyao3=atan2(san4,san5);
//单子样和三子样算法性能比较

	cout<<"                "<<"偏航角误差"<<"   "<<"纵摇角误差"<<"   "<<"横摇角误差"<<endl;
	cout<<"单子样旋转矢量法"<<pianhang1<<"      "<<zongyao1<<"     "<<hengyao1<<endl;
	cout<<"三子样旋转矢量法"<<pianhang3<<"      "<<zongyao3<<"     "<<hengyao3<<endl;
}


double one(double a,double H,double w)//计算单子样算法漂移。
{
	double p1;
	p1=pow(a,2)*w*pow(w*H,2)/12;
	return p1;
}
double three(double a,double H,double w)//计算三子样算法漂移。
{
	double p2;
	p2=pow(a,2)*w*pow(w*H,6)/204120;
	return p2;
}
main()
{
    double f,w,a,d,h,H;
	double onemove,onemove_1;
	double threemove,threemove_1;

    w=2*pi;//角速度。
    h=0.024;//更新周期。
    double a0=1.0;//半锥角。
    double t=60.0;//计算时间。
    a=(a0/360)*2*pi;//度转化为弧度。
    xingneng(t,a,w,h);

	
	cout<<"请依次输入锥运动的半锥角和更新周期:"<<endl;
    cin>>d>>h;//输入半锥角和更新周期。
    a=d*2*pi/360;//度转化为弧度。
    H=h/3600;//转换秒为小时。
	cout<<"锥运动的半锥角和更新周期为:"<<d<<"度"<<"    "<<h<<"微秒"<<endl;
	int i;
	for(i=1;i<50;i++)//不同频率下算法漂移。
    {
        f=2*i;
        w=2*pi*f;
	    cout<<"锥运动频率为:"<<f<<"赫兹"<<endl;
	    onemove_1=one(a,H,w);//单位弧度每小时。
	    threemove_1=three(a,H,w);//单位弧度每小时。
        onemove=onemove_1*180/pi;//转化为度每小时。
        threemove=threemove_1*180/pi;//转化为度每小时。 
	    cout<<"单子样算法漂移:"<<onemove<<"度/小时"<<"      "<<"三子样算法漂移:"<<threemove<<"度/小时"<<endl;//输出单子样和三子样的算法漂移
	}
    
	return 0;
}