• 大小: 5KB
    文件类型: .rar
    金币: 1
    下载: 0 次
    发布日期: 2021-06-02
  • 语言: C/C++
  • 标签: 捷联惯导  C语言  

资源简介

捷联惯性导航的C语言实现,姿态更新 速度更新 位置计算

资源截图

代码片段和文件信息

//捷联惯导
#include 
#include 
#include 

#define PI 3.1415926
#define WIE 7.2921158e-5//弧度每秒
#define RAD PI/180
#define G 9.8
#define Re 6.378393e6
#define dRe 0.15678e-6
#define e 0.3367e-2

//void Initial();
void Initial(double C[3][3]double T[3][3]double Q[4]double POS[3]double V[2]double Angl[3]double Wiep[3]double WEPP[2]);
void INITCMAT(double C[3][3]double POS[3]);
void INITQ(double T[3][3]double Q[4]);
void INITWIEP(double C[3][3]);
void INITWEPP(double C[3][3] double V[2]);
void calcG(double C[3][3]double h);
void calcV(double V[2]);
void calcWPBB(double Wpbb[3]double Wibb[3] double T[3][3]double Wepp[3] double Wiep[3]double WEPP[2]);
//void updConvQ(double Wpbb[3]);
void updConvQ(double Wpbb[3]double  Conv_Q[4][4]);
void updateQ(double Wpbb1[3]double Wpbb2[3]double Wpbb3[3]double Q[4]double t1);
void unitQ(double Q[4]);
void calaT(double Q[4]double T[3][3]);
void convacce(double T[3][3]double fp[3]double fb[3]);
void updateVp(double fp[3]double Wiep[3]double Wepp[3]double V[3]double t2);
//void updateVp(double fp[3]double Wiep[3]double Wepp[3]double Init_V[3]double t2);
void calaWEPP(double C[3][3] double V[2]double WEPP[2]);
void updateC(double C[3][3]double WEPP[2]double t2);
//void updateC(double Init_C[3][3]double C[3][3]double Wepp[2]double t2);
void calaWIEP(double Wiep[3]double C[3][3]double Wie[3]);
void calapos(double POS[3]double C[3][3]);
void calczitai(double Angl[3]double T[3][3]double alpha);

void mulit_MAT_VEC1(double mulit[2]double a[2][2]double b[2]);
void mulit_MAT_VEC2(double mulit[3]double a[3][3]double b[3]);
void mulit_MAT_VEC3(double mulit[4]double a[4][4]double b[4]);
void mulit_NUM_VEC1(double mulit[4]double ndouble a[4]);
void mulit_NUM_VEC2(double mulit[3]double ndouble a[3]);
void mulit_NUM_MAT1(double mulit[3][3]double ndouble a[3][3]);
void add_VECTOR1(double sum[3]double a[3]double b[3]);
void add_VECTOR2(double sum[4]double a[4]double b[4]);
void add_mVECTOR(double sum[4]double a[4]double b[4]double c[4]double d[4]double e1[4]);
void sub_VECTOR1(double diff[3]double a[3]double b[3]);
void trans_MATRIX1(double a[3][3]double a_trans[3][3]);
void mulit_MATRIX(double mulit[3][3]double a[3][3]double b[3][3]);
void add_MATRIX(double sum[3][3]double a[3][3]double b[3][3]);
  
void main()

{
double LAT=45*RAD;
    double t1=0.01;
double t2=0.04;
int I=0;
    int mn;
double T[3][3];
    double Wepp[3];
    double WEPP[2];
double Wiep[3];
    double Q[4];
    double fp[3];    
double C[3][3];
    double V[2];
double Wie[3];
double h=0;
double Angl[3];
double alpha=0;
double POS[3];   
double x=0;
    double Wf[18];
    double Wibb1[3]Wibb2[3]Wibb3[3]fb[3];
    double Wpbb1[3]Wpbb2[3]Wpbb3[3];
  
  //Initial();
    Initial(CTQPOSVAnglWiepWEPP);
cout<    cout<

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      20545  2011-05-31 21:12  jielian.cpp

----------- ---------  ---------- -----  ----

                20545                    1


评论

共有 条评论