捷联惯导算法+kalman滤波c程序

#include 
#include 
#include 
#include 
#include 
#include 

const double GRAVITY = 9.8;
const double PI = 3.1415926536l;
const double RPI = PI / 180.;
const double DPI = 180. / PI;
const double WIE = 15.04088 * RPI / 3600.;	//单位rad/s
const double RE = 6.378165e6l;	//单位：m
const double AEE = 1.0 / 298.3;
const double EI = 1e-6;

const int sta_num = 12;
const int mea_num = 3;

double s0 = 65536.0;
double w0 = 2053.0;
double v0 = 13849.0;
double r0 = 0.0;
double t0 EGx;
int m0;

double white（）		//生成白噪声											
{
	t0 = 0.0;
      
	for（int j = 0; j < 12; j++）
	{
		r0 = w0 * r0 + v0;
		m0 = （int）（r0 / s0）;
		r0 = r0 - m0 * s0;
		t0 = t0 + r0 /  s0;
	}
	EGx = t0 - 6.0;
	return（EGx）;
}
//mxn维矩阵w乘以n维向量b得到m维向量a
void vecmul（int m int n double *a double *w double *b）
{
	double aa[20];			//一般维数不超过3维
	int i j;
	for（i = 0; i < m; i++）
	{
		aa[i] = 0.0;
		for（j = 0; j < n; j++）
			aa[i] += （*（w + i*n + j）） * （*（b + j））;
	}
	for（i = 0; i < m; i++）
		*（a + i） = aa[i];
}
//m维向量b与c相加得到向量a
void vecadd（int m double *a double *b double *c）
{
	int i;
	for（i = 0; i < m; i++）
		*（a + i） = *（b + i） + *（c + i）;
}
//m维向量b与c相减得到向量a
void vecsub（int m double *a double *b double *c）
{
	int i;
	for（i = 0; i < m; i++）
		*（a + i） = *（b + i） - *（c + i）;
}
//向量b的装置与向量c相乘得到向量a
// 0   -b2   b1
// b2   0   -b
//-b1   b    0
void cvecmul（double *a double *b double *c）
{
	double bb[3][3];
	bb[0][0] = 0.01;
	bb[0][1] = -*（b + 2）;
	bb[0][2] = *（b + 1）;
	bb[1][0] = *（b + 2）;
	bb[1][1] = 0.01;
	bb[1][2] = -*b;
	bb[2][0] = -*（b + 1）;
	bb[2][1] = *b;
	bb[2][2] = 0.01;
	vecmul（3 3 a （double *）bb c）;
}
//n1xn2矩阵b乘以n2xn3矩阵得到n1xn3矩阵
void mamul（int n1 int n2 int n3 double *a double *b double *c）
{
	double d[20][20];   //一般矩阵相乘维数不超过3
	int i j k;
	for（i = 0; i < n1; i++）
	{
		for（j = 0; j < n3; j++）
		{
			d[i][j] = 0.0;
			for（k = 0; k < n2; k++）
				d[i][j] += （*（b + i * n2 + k）） * （*（c + k * n3 + j））;
		}
	}
	for（i = 0; i < n1; i++）
	{
		for（j = 0; j < n3; j++）
		{
			*（a + i * n3 + j） = d[i][j];
		}
	}
}
//三个矩阵的维数均为n1xn2
void maadd（int n1 int n2 double *a double *b double *c）
{
	int i j;
	for（i = 0; i < n1; i++）
		for（j = 0; j < n2; j++）
			*（a + i * n2 + j） = （*（b + i * n2 + j）） + （*（c + i * n2 + j））;
}
//三个矩阵的维数均为n1xn2
void masub（int n1 int n2 double *a double *b double *c）
{
	int i j;
	for（i = 0; i < n1; i++）
		for（j = 0; j < n2; j++）
			*（a + i * n2 + j） = （*（b + i * n2 + j）） - （*（c + i * n2 + j））;
}
//向量与常数相乘，得到向量a
void avecmul（int n double *a double *b double c）
{
	int i;
	for（i = 0; i < n; i++）
		*（a + i） = *（b + i） * c;
}
//矩阵数乘:a==b （x） c
void amamul（int n1int n2double *adouble *b double c）
{
	int ij;

	for （i=0;i		for （j=0;j			*（a+i*n2+j）=*（b+i*n2+j）*c;
}
//矩阵转置
void maturn（int n1int n2 double *a double *b）
{
	double d[20][20]