position.c

此程序为GPS接收机的源码

// position.c Process pseudoranges and ephemeris into position
// Copyright (C) 2005  Andrew Greenberg
// Distributed under the GNU GENERAL PUBLIC LICENSE (GPL) Version 2 (June 1991).
// See the "COPYING" file distributed with this software for more information.

#include <cyg/kernel/kapi.h>
#include <math.h>
#include "position.h"
#include "constants.h"
#include "display.h"        // logging function
#include "ephemeris.h"
#include "gp4020.h"
#include "gp4020_io.h"
#include "log.h"
#include "pseudorange.h"
#include "switches.h"
#include "time.h"

/******************************************************************************
 * Defines
 ******************************************************************************/

// WGS-84 defines
#define OMEGA_E     7.2921151467E-5  // Earth rotation rate [r/s]
#define WGS84_A         6378137.0    // Earth semimajor axis [m]
#define WGS84_B         6356752.3142 // Earth semiminor axis [m]

// Other defines

#define SQRTMU      19964981.8432    // [(m^3/2)/s] sqrt(G * M(Earth))

#define F_RC -4.442807633E-10 // Relativistic correction: -2*sqrt(mu)/c^2
                              // (BB p133)

/******************************************************************************
 * Globals
 ******************************************************************************/

cyg_sem_t  position_semaphore;

unsigned short pr2_data_fresh;
// unsigned short positioning;

position_t  receiver_pos;
llh_t       receiver_llh;

azel_t  sat_azel[N_CHANNELS];
xyzt_t  sat_pos_by_ch[N_CHANNELS];

// temporarily globals for debugging

satpos_t sat_position[N_CHANNELS];
double   m_rho[N_CHANNELS];

/******************************************************************************
 * Statics (Module variables)
 ******************************************************************************/


//static xyzt_t   rec_pos_xyz;

/******************************************************************************
 * Signal from pseudorange thread that there aren't enough satellites to be
 * calculating position. Clear things as necessary.
 ******************************************************************************/
void
clear_position( void)
{
    receiver_pos.positioning = 0;
}

/******************************************************************************
 * Translate ECEF to LLH coordinates.
 *
 * Based on equations found in Hoffman-Wellinhoff
 ******************************************************************************/

static llh_t
ecef_to_llh( xyz_t pos)
{
    double p,n,thet,esq,epsq;
    double a2,b2,slat,clat,sth,sth3,cth,cth3; // Save some float math.
    llh_t result;

    a2 = WGS84_A * WGS84_A;
    b2 = WGS84_B * WGS84_B;

    p = sqrt( pos.x * pos.x + pos.y * pos.y);
    thet = atan( pos.z * WGS84_A / ( p * WGS84_B));
    esq = 1.0 - b2 / a2;
    epsq = a2 / b2 - 1.0;

    sth = sin( thet);
    cth = cos( thet);

    sth3 = sth * sth * sth;
    cth3 = cth * cth * cth;

    result.lat = atan( (pos.z + epsq * WGS84_B * sth3)
                        / (p - esq * WGS84_A * cth3));
    result.lon = atan2( pos.y, pos.x);
    clat = cos( result.lat);
    slat = sin( result.lat);
    n = a2 / sqrt( a2 * clat * clat + b2 * slat * slat);
    result.hgt = p / clat - n;

    return( result);
}


/******************************************************************************
 * calculate_position
 * Given satellite locations and pseudorange measurements, return an estimated
 * receiver position based purely on the geometry and Earth's rotation rate.
 *
 * The initial estimated position comes from the (rec_pos_xyz) structure, the
 * pseduorange measurements from (m_rho), measured range,
 *
 * If the position cannot be determined, the position returned will be
 * <000>, the center of the Earth.
 *****************************************************************************/
position_t
calculate_position( unsigned short nsats_used)
{
    position_t result;
    double x, y, z, b;  // estimated receiver position and clock bias
    double alpha;       // Angle of Earth rotation during signal transit
    //double sa,ca;       // Sin( alpha), Cos( alpha)
    double e_rho;       // Estimated Range

    unsigned short i, j, k; // indices

    // matrix declarations
    // (Is it smart to reserve N_CHANNELS when we need (nsats_used)?
    //  Since there's only one instance maybe these should be globals?)
    double delrho[N_CHANNELS]; // vector of per-sat. estimated range error
    double A[N_CHANNELS][3];   // linearized relation of position to pseudorange
    double inv[4][4];          // inverse of matrix (a[i,j] == Transpose[A].A)
    double delx[4];            // incremental position correction
    double  a00, a01, a02, a03,  // a[i,j], intermediate matrix for
                 a11, a12, a13,  // generalized inverse calculation.
                      a22, a23,  // Only 10 elements due to symmetry.
                           a33;  // Don't loop, so no indexing.
    double det; // matrix determinate

#if 0
    const char idx[4][4] = // For symmetric, indexed matrices such as inv[]
        {   {0,1,2,3},     //  we store only their upper triangle.
            {1,4,5,6},     // Normal indexed access is possible like so:
            {2,5,7,8},     //   a[i][j] => a[idx[i][j]]
            {3,6,8,9}
        }
    // This is not clearly better. It's probably faster to copy the 6 elements
    // over and save the indirect look-up in the inner loop
    // (about (16 * nsats_used) look-ups).
#endif

    double dx, dy, dz;          // vector from receiver to satellite (delx)
    xyz_t  satxyz[N_CHANNELS];  // sat position in ECEF at time of reception

    unsigned short nits = 0;    // Number of ITerationS

    double         error = 1000;        // residual calculation error
    unsigned short singular = 0;        // flag for matrix inversion
    result.valid = 0;

    b = 0.0;  // [m] local estimate of clock bias, initially zero
             // Note, clock bias is defined so that:
            //     measured_time == true_time + clock_bias

//    nsats_used  = 4;
    // make local copy of current position estimate (a speed optimization?)
    x = HERE_X;
    y = HERE_Y;
    z = HERE_Z;
//     x = rec_pos_xyz.x;
//     y = rec_pos_xyz.y;
//     z = rec_pos_xyz.z;

    for( i = 0; i < nsats_used; i++)
    {
        // The instantaneous origin of a satellite's transmission, though fixed
        // in inertial space, nevertheless varies with time in ECEF coordinates
        // due to Earth's rotation. Here we find the position of each satellite
        // in ECEF at the moment our receiver latched its signal.

        // The time-delay used here is based directly on the measured
        // time-difference. Perhaps some sort of filtering should be used.
        // If a variation of this calculation were included in the iteration,
        // more accuracy could be gained at some added computational expense.

        // Note that we may want to correct for the clock bias here, but
        // perhaps it's better if that's already folded into m_rho by
        // the time we get here.

        // alpha is the angular rotation of the earth since the signal left
        // satellite[i]. [radians]
        alpha = m_rho[i] * OMEGA_E / SPEED_OF_LIGHT;

        // Compute the change in satellite position during signal propagation.
        // Exact relations:
        //   sa = sin( alpha);
        //   ca = cos( alpha);
        //   dx = (sat_position[i].x * ca - sat_position[i].y * sa) - x;
        //   dy = (sat_position[i].y * ca + sat_position[i].x * sa) - y;
        //   dz = sat_position[i].z - z;
        // Make the small angle approximation
        //   cos( alpha) ~= 1 and sin( alpha) ~= alpha.
        satxyz[i].x = sat_position[i].x + sat_position[i].y * alpha;
        satxyz[i].y = sat_position[i].y - sat_position[i].x * alpha;
        satxyz[i].z = sat_position[i].z;
        // TODO, check the error in this approximation.
    }


/**************************************
 * Explanation of the iterative method:
 *
 * We make measurements of the time of flight of satellite signals to the
 * receiver (delta-t). From this we calculate a distance known as a pseudo-
 * range. The pseudorange differs from the true distance because of atmospheric
 * delay, receiver clock error, and other smaller effects. The error sources
 * other than clock error are dealt with elsewhere. The precise clock error can
 * only be found during this position calculation unless an external and very
 * accurate time source is used, or the carrier "integer ambiguity" can be
 * resolved. Since neither of the latter is usually the case, we solve for the
 * receiver "clock bias" as part of this position calculation. To be precise,
 * We define our pseudorange to be:
 *
 *   rho[i] == sqrt( (sx[i]-x)^2 + (sy[i]-y)^2 + (sz[i]-z)^2) + b
 *
 * Where (rho[i]) is the pseudorange to the ith satellite, (<sx,sy,sz>[i]) are
 * the ECEF (Earth Centered Earth Fixed) rectangular coordinates of the ith
 * satellite which are known from orbital calculations based on the local time
 * and ephemeris data, and (<xyz>) is the current estimate of the receiver
 * position in ECEF coordinates, (b)[meters] is the remaining clock-error
 * (bias) after performing all relevant corrections, and (^2) means to square
 * the quantity in parenthesis.
 *
 * By differentiation of (rho), a linear relation can be found between a change
 * in receiver position (delx), and the change in pseudorange (delrho).
 * Written as a matrix, this relation can be inverted and the receiver position
 * correction (delx) found.  Though approximate, the linearized process can
 * be iterated until a sufficiently accurate position is found.
 *
 * The linear relation in matrix form can be written:
 *
 *   delrho == A . delx
 *
 * Where (delrho) is a column vector representing the change in pseudoranges
 * resulting from a change in receiver position vector (delx), and (A) is
 * the matrix expressing the linear relation.
 *
 * To calculate a receiver position correction, first calculate the
 * pseudoranges based on the estimated receiver position and the known satellite
 * positions, call this (e_rho), and subtract this from the measured
 * pseudoranges:
 *
 *   delrho = m_rho - e_rho
 *
 * Knowing (delrho) and (A) the correction (delx) can be solved for, and
 * the corrected estimate of position can then be found:
 *
 *   (corrected-x) = (previous-x) + delx
 *
 * In the iterative method, (corrected-x) is used as the new value of the
 * estimated receiver position, and the whole correction process repeated.
 * Iteration is successful when the size of the correction is sufficiently small.
 *
 **************************************/

    do
    {   // Iterate until an accurate solution is found

        // Use some locals to avoid re-computing terms.
        // The names somewhat indicate the terms contained.
        double a01a33, a02a02, a12a23, a13a13a22,
            a03a03_a00a33, a02a12_a01a22, a02a13_a01a23, a12a13_a11a23;

        for( i = 0; i < nsats_used; i++)
        {
            // Working in the ECEF (Earth Centered Earth Fixed) frame, find the
            // difference between the position of satellite[i] and the
            // receiver's estimated position at the time the signal was
            // received.
            dx = x - satxyz[i].x;
            dy = y - satxyz[i].y;
            dz = z - satxyz[i].z;