mathsubs.c

GPS导航定位程序

 * Entries 0 to 128 correspond to angles 0 to pi/2 in steps of
 * 1/128*pi.
 ***************************************************************************/

static int sinarray[129] = 
{0,402,804,1206,1608,2009,2410,2811,3212,3612,4011,4410,4808,5205,5602,5998,
 6393,6786,7179,7571,7962,8351,8739,9126,9512,9896,10278,10659,11039,11417,
 11793,12167,12539,12910,13279,13645,14010,14372,14732,15090,15446,15800,
 16151,16499,16846,17189,17530,17869,18204,18537,18868,19195,19519,19841,
 20159,20475,20787,21096,21403,21705,22005,22301,22594,22884,23170,23452,
 23731,24007,24279,24547,24811,25072,25329,25582,25832,26077,26319,26556,
 26790,27019,27245,27466,27683,27896,28105,28310,28510,28706,28898,29085,
 29268,29447,29621,29791,29956,30117,30273,30424,30571,30714,30852,30985,
 31113,31237,31356,31470,31580,31685,31785,31880,31971,32057,32137,32213,
 32285,32351,32412,32469,32521,32567,32609,32646,32678,32705,32728,32745,
 32757,32765,32767};

/****************************************************************************    
* Function: long SinApprox(long x)
*
* Returns sin(x) in the range +-1 scaled by (2^15 - 1).
*
* Input: x - input argument scaled by 2^13.
*
* Output: None.
*
* Return Value: The sine scaled by (2^15 - 1).
****************************************************************************/
long SinApprox(long x)
{
    long index;                        /* Index into the arctangent array. */
    int sign;                                   /* The sign of the result. */

    x = x%51471L;             /* Get x in the range +-2*pi scaled by 2^13. */

    if(x<-25735L)               /* Get x in the range +-pi scaled by 2^13. */
	x += 51471L;
    else if(x>25735L)
	x -= 51471L;
    
    if(x>=0)                    /* Get x in the range 0-pi scaled by 2^13 .*/
	sign = 1;
    else
    {
	sign = -1;
	x = -x;
    }

    if(x>12867L)                             /* Get x in the range 0-pi/2. */
	x = 25735L - x;

    index = (x*129L)/12868L;   

    return((long)(sign*sinarray[index]));
}

/****************************************************************************    
* Function: long CosApprox(long x)
*
* Returns the cos(x) in the range +-1 scaled by (2^15 - 1).
*
* Input: x - the input argument in the range +-pi scaled by 2^13.
*
* Output: None.
*
* Return Value: The cosine scaled by (2^15 - 1).
****************************************************************************/
long CosApprox(long x)
{
    /* Use the identity cos(x) = sin(pi/2 - x) */
   
    x = 12868L - x;
    return(SinApprox(x));
}

/****************************************************************************    
* Function: int CompareInt(const void *a, const void *b)
*
* Compares 2 integers. The comparison is reverse because it is used to sort
* a table of satellites into descending order according to their elevation
* angles using the Borland library routine, qsort().
*
* Input: a - pointer to the input argument.
*        b - pointer to the input argument.
*
* Output: None.
*
* Return Value: Negative if a>b, zero if a=b, positive if a<b.
****************************************************************************/
int CompareInt(const void *a, const void *b)
{
    return(*(int *)b-*(int *)a);
}

/****************************************************************************    
* Function: unsigned long ISquare(int i)
*
* Square a 16-bit integer to produce a 32-bit unsigned long. This routine
* is in assembler because not all compiler version produce the optimised
* code below.
*
* Input: i - the input argument.
*
* Output: None.
*
* Return Value: The argument squared.
****************************************************************************/
#pragma warn -rvl                    /* Disable 'no return value' warning. */
unsigned long ISquare(int i)
{
    _AX = i;
    asm{
	imul ax
    }
}
#pragma warn .rvl                     /* Enable 'no return value' warning. */

/****************************************************************************    
* Function: int InverseMatrix(int dimension, double matrix[])
*
* Invert an n x n matrix using Gauss-Jordan elimination with column shifting
* to maximize pivot elements.  The caller may treat the matrix as 2-D, but
* it is actually passed as a 1-D array since C does not have dynamic array
* dimensions (subscripting is done internally as c[i][j] = c[n*i+j]).
*
* Input: matrix - n x n nonsingular matrix, which does have to be symmetric.
*        dimension - the dimension of the input matrix.
*
* Output: matirx - the dimension by dimension inverse matrix.
*
* Return Value: Indicate if inverse computed or singular matrix.
****************************************************************************/
int InverseMatrix(int dimension, double matrix[])
{
    int column_swap[20];         /* Used to keep track of column swapping. */
    int l,k,m;                        /* Used for row and column indexing. */

    double pivot;
    double determinate;
    double swap;                         /* Used to swap element contents. */

    determinate = 1.0;

    for(l=0;l<dimension;l++)
	column_swap[l] = l;

    for(l=0;l<dimension;l++)
    {
	pivot = 0.0;
	m = l;

	/* Find the element in this row with the largest absolute value -
	   the pivot. Pivoting helps avoid division by small quantities. */

	for(k=l;k<dimension;k++)
	{
	    if(fabs(pivot) < fabs(matrix[dimension*l+k]))
	    {
		m = k;
		pivot = matrix[dimension*l+k];
	    }
	}

	/* Swap columns if necessary to make the pivot be the leading
	   nonzero element of the row. */

	if(l!=m)
	{
	    k = column_swap[m];
	    column_swap[m] = column_swap[l];
	    column_swap[l] = k;
	    for(k=0;k<dimension;k++)
	    {
		swap = matrix[dimension*k+l];
		matrix[dimension*k+l] = matrix[dimension*k+m];
		matrix[dimension*k+m] = swap;
	    }
	}

	/* Divide the row by the pivot, making the leading element
	   1.0 and multiplying the determinant by the pivot. */

	matrix[dimension*l+l] = 1.0;
	determinate = determinate * pivot;   /* Determinant of the matrix. */
	if(fabs(determinate) < 1.0E-6)
	    return(SINGULAR);      /* Pivot = 0 therefore singular matrix. */
	for(m=0;m<dimension;m++)
	    matrix[dimension*l+m] /= pivot;

	/* Gauss-Jordan elimination.  Subtract the appropriate multiple
	   of the current row from all subsequent rows to get the matrix
	   into row echelon form. */

	for(m=0;m<dimension;m++)
	{
	    if(l==m)
		continue;
	    pivot = matrix[dimension*m+l];
	    if(pivot==0.0)
		continue;
	    matrix[dimension*m+l] = 0.0;
	    for(k=0;k<dimension;k++)
		matrix[dimension*m+k] -= (pivot*matrix[dimension*l+k]);
	}
    }

    /* Swap the columns back into their correct places. */

    for(l=0;l<dimension;l++)
    {
	if(column_swap[l]==l)
	    continue;

	/* Find out where the column wound up after column swapping. */

	for(m=l+1;m<dimension;m++)
	    if(column_swap[m]==l)
		break;

	column_swap[m] = column_swap[l];
	for(k=0;k<dimension;k++)
	{
	    pivot = matrix[dimension*l+k];
	    matrix[dimension*l+k] = matrix[dimension*m+k];
	    matrix[dimension*m+k] = pivot;
	}
	column_swap[l] = l;
    }
    return(NONSINGULAR);
}