JIELIAN初始对准的c算法源码实现捷联惯导的静基座初始对准联合开发网.rar

JIELIAN初始对准的c算法源码,实现捷联惯导的静基座初始对准联合开发网.rar

/*****************************************************************************/
/**************用于实际系统的组合导航gps量测数据直接从OEM读数****************/
/*****************************************************************************/
#include “math.h“
#include “stdio.h“
#include “stdlib.h“
#include “conio.h“
#define R_e 6.318137e6
#define F 1.0/298.257
#define w_ie 7.292115e-5
#define T 1.0
#define G 9.878049
#define GPSON 1
//#define dd 4
double g[15][6]q[6][6]r[6][6]cbp[3][3];
double R_mR_n;
int comflag;
int   dd;
//double fuyangpianhang;

void init（double x[15]double dx[15]double p[15][15]double h[6][15]）;
void readstrap（double x[15]double a[22]）;
void measure（double x[15]double z[6]double noise[6]double a[22]）;
void transferandp_k（double x[15]double fi[15][15]double p[15][15]double p_k[15][15]）;
void squareer_p_k（double p[15][15]double p_k[15][15]double transition1[15][15]double f[15][15]double fi[15][15]double fi_t[15][15]）;
void squareer_p（double p[15][15]double p_k[15][15]double modulus[15][6]double h[6][15]）;
void gainmodulus（double x[15]double p_k[15][15]double modulus[15][6]double h[6][15]）;
void estate（double dx[15]double z[6]double fi[15][15]double modulus[15][6]double h[6][15]）;
void kalman（double x[15]double dx[15]double z[6]double noise[6]double p[15][15]double h[6][15]double a[22]）;
void tranfs（double f[][15]）;
int  randm（float cfloat dfloat tfloat efloat *rn1float *snfloat *sn1）;
void athwart（double aa[][6]int n）;
void matrixmultiply（double *p1double *p2double *p3int iiint kkint jj）;
void readbf（double a[22]）;
//void output（double dx[15]）;
FILE *in;

void main（void）
{
int ij;
float rn[6]sn[6]ssn[6];
double dx[15]x[15]z[6]noise[6];
double h[6][15]p[15][15]a[22];
double outout[15];
FILE *out;


if（（out=fopen（“result1.dat““w“））==NULL）
	{	puts（“CAN‘T OPEN result.dat to save result data.“）;
		puts（“PROGRAM IS TERMINATED !“）;
		exit（1）;
	}
if（（in=fopen（“rk.dat““r“））==NULL）
	{	puts（“CAN‘T OPEN out.dat to save result data.“）;
		puts（“PROGRAM IS TERMINATED !“）;
		exit（1）;
	}
	
init（xdxph）; 

for（i=0;i<6;i++） z[i]=0.0;
	rn[0]=2.0*（pow（2.014.0）-2.0）+1.0;
	rn[1]=2.0*（pow（2.014.0）-2.0）+2.0;
	rn[2]=2.0*（pow（2.014.0）-2.0）+3.0;
	rn[3]=2.0*（pow（2.014.0）-2.0）+7.0;
	rn[4]=2.0*（pow（2.014.0）-2.0）+5.0;
	rn[5]=2.0*（pow（2.014.0）-2.0）+5.0;

for（i=0;i<400;i++）
{     	randm（10.00.03.00.0&rn[0]&sn[0]&ssn[0]）; noise[0]=sn[0];
	randm（10.00.03.00.0&rn[1]&sn[1]&ssn[1]）; noise[1]=sn[1];
	randm（10.00.03.00.0&rn[2]&sn[2]&ssn[2]）; noise[2]=sn[2];
	randm（0.10.03.00.0&rn[3]&sn[3]&ssn[3]）; noise[3]=sn[3];
	randm（0.10.03.00.0&rn[4]&sn[4]&ssn[4]）; noise[4]=sn[4];
	randm（0.10.03.00.0&rn[5]&sn[5]&ssn[5]）; noise[5]=sn[5];
	
	readbf（a）; 
	readstrap（xa）;
      kalman（xdxznoisepha）;

      fprintf（out“%4d “i）;
	  for（j=0;j<15;j++）
	  { 	outout[j]=dx[j];
	  	outout[0]=outout[0];
	  	outout[1]=outou

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

     文件      60606  2005-07-23 11:06  初始对准\c\WHNOISE.C

     文件      64298  2005-07-23 11:06  初始对准\c\RKs.C

     文件      62768  2005-07-23 11:06  初始对准\c\RK2.C

     文件      61264  2005-07-23 11:06  初始对准\c\RK10.C

     文件      60552  2005-07-23 11:06  初始对准\c\PINGFEI.C

     文件      73630  2005-07-23 11:06  初始对准\c\jingjizhuo.c

     文件      60115  2005-07-23 11:06  初始对准\c\Kalman.c

     文件      63488  2005-07-23 11:06  初始对准\c\JIELIAN.C

     文件      72628  2005-07-23 11:06  初始对准\c\HANGJI1.C

     文件      60198  2005-07-23 11:06  初始对准\c\COMPARE.C

     文件      74645  2005-07-23 11:06  初始对准\c\Comnav36.c

     文件      63684  2005-07-23 11:06  初始对准\c\10.C

     文件      65536  2005-07-23 11:06  初始对准\c\1010.C

     文件      65536  2005-07-23 11:06  初始对准\c\HANGJI.C

     文件      65536  2005-07-23 11:06  初始对准\c\RK1.C

     目录          0  2005-08-16 10:04  初始对准\c

     文件      61370  2005-07-23 11:05  初始对准\matlab\single.m

     文件      60066  2005-07-23 11:05  初始对准\matlab\scatter.m

     文件      61578  2005-07-23 11:05  初始对准\matlab\pwcsstraight3.m

     文件      61380  2005-07-23 11:05  初始对准\matlab\pwcsstraight2.m

     文件      62621  2005-07-23 11:05  初始对准\matlab\pwcsstraidegree.m

     文件      60870  2005-07-23 11:05  初始对准\matlab\pwcspose3.m

     文件      60295  2005-07-23 11:05  初始对准\matlab\pwcspose2.m

     文件      65224  2005-07-23 11:05  初始对准\matlab\sinsMBNA1.m

     文件      60480  2005-07-23 11:05  初始对准\matlab\posematrix1.m

     文件      60060  2005-07-23 11:05  初始对准\matlab\posematrix.m

     文件      61006  2005-07-23 11:05  初始对准\matlab\makeA12.m

     文件      60858  2005-07-23 11:05  初始对准\matlab\makeA101.m

     文件      60840  2005-07-23 11:05  初始对准\matlab\llll.fig

     文件      60536  2005-07-23 11:05  初始对准\matlab\kalmf3.m

............此处省略17个文件信息