matrix_tools.c

GPS卫星导航接收机的仿真程序。用C语言实现

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <conio.h>
#include <assert.h>

#include	"simulator.h"
#include	"Matrix_Tools.h"


//******************************************************************************
//**	Function:	create_cacode												
//**	Input:		Prn number													  
//**	Tasks:		Generate C/A code for a SV specified by prn	
//**	Output:		C/A code																	  
//******************************************************************************
void create_cacode(char svcode[], int prn)
{

	int g2shift_vector[37] = {5,6,7,8,17,18,139,140,141,251,252,254,255,256,257,258,469,470,471,472,473,474,509,512,513,514,515,516,859,860,861,862,863,950,947,948,950};
	int reg[10];
	int g1[1023];
	int g2[1023];
	int g2tmp[1023];
	int g2_shift;
	int i;
	int j;
	int save1;

	if ((prn <= 0) | (prn > 37))
		printf("invalid prn: must be between 1 and 37");

	prn--;

	g2_shift = g2shift_vector[prn];

	/************* Generate G1 code **************************/
	for(i=0;i< 10;i++)
 		reg[i] = -1;

	for(i =0; i<1023;i++)
	{
		g1[i] = reg[9];
		save1 = reg[2] * reg[9];
		for(j = 8;j>=0; j--)
  			reg[j+1] = reg[j];
		reg[0] = save1;
	}
	/************end of G1 code ***********************/

	/************* Generate G2 code **************************/
	for(i=0;i< 10;i++)
 		reg[i] = -1;

	for(i =0; i<1023;i++)
	{
		g2[i] = reg[9];
		save1 = reg[1] * reg[2] * reg[5]* reg[7] * reg[8] * reg[9];
		for(j = 8;j>=0; j--)
	  	reg[j+1] = reg[j];
		reg[0] = save1;
	}

	/****** shift G2 code **********************/

	for(i=0;i<1023;i++)
	{
		j = i + g2_shift;
		if (j >= 1023)
  			j = j - 1023;
		g2tmp[j] = g2[i];
	}

	for(i=0;i<1023;i++)
		svcode[i] = (char) g1[i] * g2tmp[i];

return;

}


double **
dalloc2d(unsigned nrows, unsigned ncols)
/* Allocates space for a 'nrows' by 'ncols' matrix and returns the
 *	double pointer to that matrix */
{
//	long size;
	double **ppd;
	unsigned u;

	/* ppd is a pointer to pointer to double */
	/* it points to the first element in an array of pointers to double */

	ppd = (double **) malloc ((size_t) nrows * sizeof(double *));

	if(ppd == NULL)
	{
		fprintf(stderr,"Memory allocation error...Program over!");
		exit(1);
	}

	/* ppd[0] is a pointer to double */
	/* it is the first element in the array of pointers to double */
	/* it points to the first double in the memory we have allocated */

	ppd[0] = (double *) malloc ((size_t) nrows * ncols * sizeof(double));

	if(ppd[0] == NULL)
	{
		fprintf(stderr,"Memory allocation error...Program over!");
		exit(1);
	}

	for (u=1; u<nrows; u++)
		ppd[u] = ppd[0] + u * ncols;

	dclear2d(ppd, nrows, ncols);

	return ppd;					/* return the pointer */
}


double *
dalloc1d(unsigned size)
/* Allocates space for a 'nrows' by 'ncols' matrix and returns the
 *	double pointer to that matrix */
{
	double *pd;

	pd = (double *) malloc ((size_t) size * sizeof(double));

	if(pd == NULL)
	{
		fprintf(stderr,"Memory allocation error...Program over!");
		exit(1);
	}

	dclear1d( pd, size);

	return pd;					/* return the pointer */
}


void	
dfree1d(double * pd)
{
	free (pd);
}


void
dfree2d(double **ppd)
/* releases the memory allocated to a dynamic array that was created
 * by dalloc2d.  This function correctly deallocates all memory. */
{
	/* DO NOT CHANGE THE ORDER OF THESE STATEMENTS! */
	free(ppd[0]);				/* free data memory */
	free(ppd);					/* free array of pointers */
}


void
dclear2d(double **A, unsigned nrows, unsigned ncols)
/* Sets all elements in matrix A (dimensions nrows X ncols) to zero */
{


	unsigned i, j;

	for (i=0; i<nrows; i++) 		/* fill the array in a portable way */
		for (j=0; j<ncols; j++)
			A[i][j] = 0.0;
}


void
dmult2d(double **A, unsigned ar, unsigned ac, double **B, unsigned br, unsigned
		bc, double **C, unsigned mode)
/* multiplies two double arrays (a * b) of dimensions ar,ac and br,bc
 * respectively and stores the product in a third double array c passed
 * to the function.  The array c should have dimensions ar,bc.  It does
 * not check, in any way, the magnitude of the results or the bounds of
 * your arrays, so overflow or invalid memory accesses can happen and
 * trash the program, in a very real and legally binding sense.
 * The mode indicated whether a normal multiplication occurs or indices
 * are swapped to permit multiplication of transposes. */
{
	unsigned i, j, k;			/* array indices */


	switch (mode)
	{
	default:
	case AA:					/* normal multiplication */

		dclear2d(C, ar, bc);	/* zero array C */

		for (i=0; i<ar; i++)		/* multiply each row in a... */
			for (j=0; j<bc; j++) {	/* by each column in b... */
				for (k=0; k<ac; k++) 		/* and assign the sum to the */
					C[i][j] +=  A[i][k] * B[k][j]; /* matching element in c */
			}
		break;
	case ATA:					/* use transpose of A */

		dclear2d(C, ac, bc);	/* zero array C */

		for (i=0; i<ac; i++)		/* multiply each row in a... */
			for (j=0; j<bc; j++) {	/* by each column in b... */
				for (k=0; k<ar; k++) 		/* and assign the sum to the */
					C[i][j] +=  A[k][i] * B[k][j]; /* matching element in c */
			}
		break;

	case AAT:					/* use transpose of B */

		dclear2d(C, ar, br);	/* zero array C */

		for (i=0; i<ar; i++)		/* multiply each row in a... */
			for (j=0; j<br; j++) {	/* by each column in b... */
				for (k=0; k<ac; k++) 		/* and assign the sum to the */
					C[i][j] +=  A[i][k] * B[j][k]; /* matching element in c */
			}
		break;
	}
}


void
dmult2d1d(double **A, unsigned ar, unsigned ac, double *B, unsigned br,
		  double *C)
/* multiplies a double array a of dimensions ar,ac and a vector b of dimension
 * br respectively and stores the product in a third double vector c passed to
 * the function.  The vector c should have dimension ar.  It does not check, in
 * any way, the magnitude of the results or the bounds of your arrays, so
 * overflow or invalid memory accesses can happen and trash the program, in a
 * very real and legally binding sense. */
{
	unsigned i, j;			/* array indices */

	dclear1d(C, ar);			/* zero vector C */

	assert(ac==br);

	for (i=0; i<ar; i++)		/* multiply each row in a... */
		for (j=0; j<br; j++) 	/* by each "row" in b... */
				C[i] +=  A[i][j] * B[j];
}


void
dadd1d(double *A, double *B, unsigned size, double *C)
/* adds two 1-dimensional arrays, A and B, of type double, places
 * the result in a third array C.  Very little error checking is done
 * and the programmer should be careful to only pass valid arrays.
 * All three arrays should be of the same size. */
{
	unsigned i;

	for (i=0; i<size; i++)
		C[i] = A[i]+B[i];
}


void
dadd2d(double **A, double **B, unsigned nrow, unsigned ncol, double **C)
/* adds two 2-dimensional arrays, A and B, of type double, places
 * the result in a third array C.  Very little error checking is done
 * and the programmer should be careful to only pass valid arrays.
 * All three arrays should be of the same size. */
{
	unsigned i,j;

	for (i=0; i<nrow; i++)
		for(j=0; j<ncol; j++)
			C[i][j] = A[i][j]+B[i][j];
}


void
dscale1d(double *A, unsigned size, double scalar)
/* multiplies all elements in vector A by 'scalar' */
{
	unsigned i;

	for(i=0;i<size;i++)
		A[i] = A[i]*scalar;
}


void
dcopy1d(double *source, double *destination, unsigned size)
/* copies 'source' into 'destination' */
{
	unsigned i;

	dclear1d(destination,size);
	for(i=0;i<size;i++)
		destination[i] = source[i];
}


void
dcopy2d(double **source, double **destination, unsigned nrow, unsigned ncol)
/* copies 'source' into 'destination' */
{
	unsigned i,j;

	dclear2d(destination,nrow,ncol);
	for(i=0;i<nrow;i++)
		for(j=0;j<ncol;j++)
			destination[i][j] = source[i][j];
}


void
dclear1d(double *A, unsigned ncols)
/* sets all elements of a double vector to zero */
{
	unsigned j;

		for (j=0; j<ncols; j++)	/* fill the vector in a portable way */
			A[j] = 0.0;
}


void
iclear1d(int *A, unsigned ncols)
/* sets all elements of a double vector to zero */
{
	unsigned j;

		for (j=0; j<ncols; j++)	/* fill the vector in a portable way */
			A[j] = 0;
}


int
dinvert2d(double **A, unsigned n)
/* inverts a matrix of doubles by Cholesky decomposition.
 * on successful exit, the normals are destroyed but the whole 
 * inverse is returned.
 * returns diagonal on which singularity occurred or zero
 * if successful
 *
 * n is the size of the square matrix if the offset were 1
 * instead of zero  i.e. the size you would dimension if Fortrash
 *
 * Author: 				Bryan Townsend
 * Modifications:		Darren Cosandier
 *							Terry Labach
 *							Mark Petovello (June 24, 1994)
 *
 * Returns:		SUCCESS if inversion was successful
 *					ERROR# if an error is reached
 *							 (where '#' ranges from 1 to 5)
 *
 *				Any of the following errors cause a program exit:
 *				Invalid matrix dimension
 *				Matrix not positive definite
 *				Singularity
 *				Can't take square root of a negative number
 *				Can't divide by zero												*/
{
	int i, j, k;


	if (n==0)					/* array must have SOME elements */
		return(ERROR1);

								/* Check for positive definiteness */
	for (i=0; i<(int)n; i++) {