/*?
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]