捷联惯导解算程序

/* 
1. initialization---- 
2. aCbnitude update----
3. transformation of the specific force resolving axes---
4. velocity update---
5. position update---
*/

#include “stdio.h“
#include “conio.h“
#include “stdlib.h“
#include “math.h“
#define pi  3.141592654
#define T   0.025         /*陀螺采样周期SI units（s） ==================================*/
#define Tk  0.0125        /*加表采样周期SI units（s）===================================*/
#define e   0.003352813   /*地球扁率SI units（1）=======================================*/
#define Re  6378137.0     /*长半轴SI units（m）=========================================*/
#define wie 0.00007292115855   /*地球自转角速度SI units（rad/s）=============================*/
double  C[3][3];          /*位置矩阵===================================================*/
double  P[4];             /*四元数=====================================================*/
double  Cbn[3][3];         /*捷联矩阵===================================================*/
double   faiG;            /*网格航向角SI units（rad）===================================*/
double   theta;           /*俯仰角SI units（rad）=======================================*/
double   gama;            /*倾斜角SI units（rad）=======================================*/
double  Wibb[3][3];       /*陀螺输出数组行为连续三个时刻采样列为XYZ方向输出SI units（rad/s）*/
double  Wepp[3][2];       /*数组行为连续三个时刻采样列为XYZ方向输出SI units（rad/s）=*/
double  Wpbb[3][3];
double  fb[7][3];         /*加速度计输出数组行为连续三个时刻采样列为XYZ方向输出SI units（m/s2）*/
double  fp[7][3];         /*转换到导航坐标系SI units（m/s2）============================*/
double  VV1[3][3];        /*行为连续三个时刻速度，列为0东向E1北向N2天向速度USI units（m/s）*/
double  level_s[3];       /*水平速度一次输出连续三个时刻SI units（m/s）================*/
double  L;                /*纬度SI units（rad）=========================================*/
double  lamed;            /*经度SI units（rad）=========================================*/
double  alpha;            /*游移方位角SI units（rad）===================================*/
double  h;                /*高度SI units（m）===========================================*/
double  g=9.8;



void initial（double *Ldouble *lameddouble *h double *alphadouble  Cbn[][3]
     double  P[4]double *faiGdouble *thetadouble *gamadouble  C[][3]
     double  Wibb[][3]double  fb[][3]）
{
     *L=pi/4;                   /*纬度      */
     *lamed=pi/4;               /*经度      */
     *h=0;                      /*高度      */
     /*初始速度。行为连续三个时刻速度，列为0东向E1北向N2天向速度U*/

/*  1.***************************Initialization************************** 

********************Coarse Alignment******************************

 The main problem of initialization is the solving of Direction Cosin Matrix*/

     Cbn[0][0]=（Wibb[0][2]*fb[0][1]-Wibb[0][1]*fb[0][2]）/（g*wie*cos（*L））;
     Cbn[0][1]=（Wibb[0][2]*fb[0][2]-Wibb[0][2]*fb[0][0]）/（g*wie*cos（*L））;
     Cbn[0][2]=（Wibb[0][1]*fb[0][0]-Wibb[0][0]*fb[0][1]）/（g*wie*cos（*L））;
     Cbn[1][0]=fb[0][0]*tan（*L）/g+Wibb[0][0]/（cos（*L）*wie）;
     Cbn[1]