position.c

此程序为GPS接收机的源码

            e_rho = sqrt( dx*dx + dy*dy + dz*dz); // estimated range

            // range error == ( (estimated range) - (measured range) ).
            delrho[i] = m_rho[i] - e_rho - b;

            // Now find the relation (delrho = A . delx). The full relation
            // given above is
            //   rho == sqrt((sx - x)^2 + (sy - y)^2 + (sz - z)^2) + b
            //
            // Taking the derivative gets
            //
            //   delrho = (( dx(x - sx) + dy(y -sy) + dz(z - sz) ) / rho) + b
            //
            // Using the basis <dx,dy,dz,b> a row of the matrix (A) referred to
            // previously is
            //   A[i] = <dx/e_rho, dy/e_rho, dz/e_rho, 1>
            //
            // Where [i] refers to the ith satellite, and the matrix (A) has
            // size (n_sats_used x 4). In practice we store only the first
            // three elements of each row, the last element being always (1).
            A[i][0] = dx / e_rho;
            A[i][1] = dy / e_rho;
            A[i][2] = dz / e_rho;
        }

        // Compute the generalized inverse (GI) of (A) in three steps:

        // First:
        //   a00 a01 a02 a03   Form the square matrix:
        //       a11 a12 a13     a[i,j] == Transpose[A].A
        //           a22 a23  (Since this matrix is symmetric,
        //               a33   compute only the upper triangle.)
        //
        // Initialize with values from the first satellite
        a03 = A[0][0];
        a13 = A[0][1];
        a23 = A[0][2];
        a33 = nsats_used; // This is actually the final value
        a00 = a03 * a03; a01 = a03 * a13; a02 = a03 * a23;
        a11 = a13 * a13; a12 = a13 * a23;
        a22 = a23 * a23;

        // fill in the data from the remaining satellites
        for( i=1; i < nsats_used; i++)
        {
            a00 += A[i][0] * A[i][0];
            a01 += A[i][0] * A[i][1];
            a02 += A[i][0] * A[i][2];
            a03 += A[i][0];
            a11 += A[i][1] * A[i][1];
            a12 += A[i][1] * A[i][2];
            a13 += A[i][1];
            a22 += A[i][2] * A[i][2];
            a23 += A[i][2];
        }

        // Second:
        // Solve for the inverse of the square matrix
        // The inverse is also symmetric, so again find only the upper triangle.
        //
        // The algorithm used is supposed to be efficient. It's rather hard
        // to derive, but fairly easy to show correct. It has been checked
        // and appears correct.
        // It uses some temporary variables, some of which could be re-used in
        // place it the optimizer is smart enough.

        inv[0][0]      = a12 * a13;
        a12a13_a11a23  = inv[0][0] - a11 * a23; // done
        inv[0][0]      = a23 * (inv[0][0] + a12a13_a11a23);
        inv[2][2]      = a01 * a03;
        inv[1][2]      = inv[2][2] - a00 * a13;
        inv[1][3]      = a22 * inv[1][2];
        inv[2][2]     += inv[1][2];
        inv[1][2]     *= -a23;
        inv[2][2]     *=  a13;
         det           = a01 * a12;
        a02a13_a01a23  = a02 * a13;
        inv[0][1]      = a03 * a12;
        inv[1][3]     += a02 * (a02a13_a01a23 - a03 * a12) -
                          a23 * (a01 * a02 - a00 * a12); // done
        a02a13_a01a23 -= a01 * a23; // done
        inv[0][1]     += a02a13_a01a23;
        inv[2][3]      = inv[0][1];
        inv[0][1]     *= -a23;
        inv[2][3]     *= -a01;
        inv[0][2]      = a02 * a11 - det;
         det           = a02 * (det - inv[0][2]);
        inv[3][3]      = a02 * a12;
        a02a12_a01a22  = - a01 * a22;
         det          += a01 * a02a12_a01a22;
         det          *= a33;
        a02a12_a01a22 += inv[3][3]; // done
        inv[3][3]     += a02a12_a01a22;
        inv[3][3]     *= a01;
        a13a13a22      = a13 * a22;
        inv[0][1]     += a03 * a13a13a22 + a33 * a02a12_a01a22;
        a12a23         = a12 * a23;
         det          -= (a03 + a03) * (a02 * a12a13_a11a23 -
                            a01 * (a13a13a22 - a12a23));
        a13a13a22     *= a13; // done
        a03a03_a00a33  = a03 * a03;
        inv[0][3]      = a12 * a12;
         det          -= a00 * (a13a13a22 - (a13 + a13) * a12a23 +
                          inv[0][3] * a33 + a11 * (a23 * a23 - a22 * a33));
        inv[0][3]     -= a11 * a22;
        inv[0][0]     -= a13a13a22 + a33 * inv[0][3]; // done
        inv[3][3]     -= a00 * inv[0][3];
         det          += a03a03_a00a33 * inv[0][3] +
                            a02a13_a01a23 * a02a13_a01a23; // done
        inv[0][3]     *= a03;
        inv[0][3]     += a23 * inv[0][2] - a13 * a02a12_a01a22; // done
        inv[0][2]     *= -a33;
        inv[0][2]     += a13 * a02a13_a01a23 - a03 * a12a13_a11a23; // done
        a03a03_a00a33 -= a00 * a33; // done
        inv[1][1]      = a02 * a03;
        inv[2][3]     += a11 * inv[1][1] + a00 * a12a13_a11a23; // done
        inv[1][1]     += inv[1][1];
        inv[1][1]      = a23 * (inv[1][1] - a00 * a23) -
                           a22 * a03a03_a00a33;
        a02a02         = a02 * a02;
        inv[1][1]     -= a33 * a02a02; // done
        inv[3][3]     -= a11 * a02a02; //done
        a01a33         = a01 * a33;
        inv[1][2]     += a12 * a03a03_a00a33 -
                           a02 * (a03 * a13 - a01a33); // done
        inv[2][2]     -= a11 * a03a03_a00a33 + a01 * a01a33; // done
        // 66 multiplies.

        if( det <= 0) // det ought not be less than zero because the matrix
                     // (Transpose[A].A) is the square of a real matrix,
                    // therefore its determinate is the square of a real number.
                   // If det is negative it means the numeric errors are large.
        {
            singular = 1;
            break;
        }
        else
        {
            // Third and final step, (generalized inverse) = (inv.Transpose[A])
            // go ahead and multiply (GI).delrho => (delx)
            // Note that (inv[]) as calculated is actually the matrix of
            // cofactors, so we divide by the determinate (det).

            // Copy over the symmetric elements if (inv[]) this should improve
            // the overall speed of the loop, we're guessing.
            inv[1][0] = inv[0][1];
            inv[2][0] = inv[0][2]; inv[2][1] = inv[1][2];
            inv[3][0] = inv[0][3]; inv[3][1] = inv[1][3]; inv[3][2] = inv[2][3];

            for( i = 0; i < 4; i++) // Over (delx) & rows of (inv[])
            {
                double acc;   // accumulator
                delx[i] = 0;

                for( j = 0; j < nsats_used; j++) // (delrho) & columns of (A)
                {
                    acc = 0;
                    for( k = 0; k < 3; k++) // rows of (A) & columns of (inv[])
                        acc += A[j][k] * inv[i][k];
                    acc += inv[i][3]; // Last column of (A) always = 1
                    delx[i] += acc * delrho[j];
                }
                delx[i] /= det;
            }


            nits++;     // bump number of iterations counter

            x += delx[0]; // correction to position estimates
            y += delx[1];
            z += delx[2];
            b += delx[3]; // correction to time bias

            error = sqrt( delx[0]*delx[0] + delx[1]*delx[1] +
		          delx[2]*delx[2] + delx[3]*delx[3]);
        }
    }
    while( (error > 0.1) && (nits < 10) && (error < 1.e8));
    // TODO, justify these numbers.

    // FIXME WHOAH Nelly, errors can creep out of that while loop, let's catch
    // them here and stop them from affecting the clock bias. If the solution
    // doens't converge to less than 0.1 meters, toss it.
    if( !singular && (error < 0.1))
    {
        result.valid = 1;

# if 0 // NOT CURRENTLY USED ANYWHERE - should it be?
        // Range residuals after position fix.
        for( i = 0; i < nsats_used; i++)
            pr2[sat_position[i].channel].residual = delrho[i];
#endif
    }
    else
    {
        result.valid = 0;
#if 0
        result.x = rec_pos_xyz.x;
        result.y = rec_pos_xyz.y;
        result.z = rec_pos_xyz.z;
#endif
    }

    result.x = x;
    result.y = y;
    result.z = z;
    result.b = b;
    result.n = nsats_used;
    result.error = error;
    result.positioning = 1;

    return ( result);
}


/******************************************************************************
 * Calculate a az/el for each satellite given a reference position
 * (Reference taken from "here.h".)
 ******************************************************************************/
static azel_t
satellite_azel( xyzt_t satpos)
{
    double xls, yls, zls, tdot, satang, xaz, yaz, az;
    azel_t result;

// What are these?
#define north_x 0.385002966431406
#define north_y 0.60143634005935
#define north_z 0.70003360254707
#define east_x  0.842218174688889
#define east_y -0.539136852963804
#define east_z  0
#define up_x -0.377413913446142
#define up_y -0.589581022958081
#define up_z 0.71410990421991

    // DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
    xls = satpos.x - HERE_X;
    yls = satpos.y - HERE_Y;
    zls = satpos.z - HERE_Z;

    tdot = (up_x * xls + up_y * yls + up_z * zls) /
            sqrt( xls * xls + yls * yls + zls * zls);

    if ( tdot >= 1.0 )
        satang = PI_OVER_2;
    else if ( tdot <= -1.0 )
        satang = - PI_OVER_2;
    else
        satang = asin( tdot);

    xaz = east_x * xls + east_y * yls;
    yaz = north_x * xls + north_y * yls + north_z * zls;

    if (xaz != 0.0 || yaz != 0.0)
    {
        az = atan2( xaz, yaz);
        if( az < 0)
            az += TWO_PI;
    }
    else
        az = 0.0;

    result.el = satang;
    result.az = az;

    return (result);
}


/*******************************************************************************
FUNCTION SatPosEphemeris(double t, unsigned short n)
RETURNS  eceft

INPUT   t  double   coarse time of week in seconds
        n  char     satellite prn

PURPOSE

     THIS SUBROUTINE CALCULATES THE SATELLITE POSITION
     BASED ON BROADCAST EPHEMERIS DATA

     R    - RADIUS OF SATELLITE AT TIME T
     Crc  - RADIUS COSINE CORRECTION TERM
     Crs  - RADIUS SINE   CORRECTION TERM
     SLAT - SATELLITE LATITUDE
     SLONG- SATELLITE LONGITUDE