ljz.h

C语言常用数值算法

#include "stdlib.h"
#include "stdio.h"
#include "math.h"
#define PI 3.1415926535897932

// Each B-file starts with a file header struct
// The size of a rawstruct header = 90 bytes
//--------------------------------
struct FileHeaderstruct
{
  char  version[10];		// ASCII: set to "Version:".
  unsigned char raw_verson;	// Current version set to 1.??
  char  rcvr_type[10];		// ASCII Receiver type: L-XII, M XII,and LM-XII ( Z-XII ?? ).
  char  chan_ver[10];		// ASCII channel software version.
  char  nav_ver[10];		// ASCII NAV software version.
  short capability;		// Capability of receiver:
         				//	1 -> L1 only (L1),
        				//	2 -> L1/L2 (codeless L2)(L2C).
    	    			//	3 -> L1/L2 (P code)(L2P). ??
  long  reserved;		// Spare.
  char  num_obs_types;		// the same as capability??
  char  spare[42];		// Spare.
} Header;
//----------
// The size of PosStruct = 67 bytes
//--------------------------------
struct PosStruct
{
  char    sitename[4];// 4-character site name (operator entered),
  				// note: last TCHAR may not '0'!!
  double   rcv_time;		// Epoch receive time (sec)
  double   navx;		// Station position: ECEF-Y (meters)
  double   navy;		// ECEF-Y (meters)
  double   navz;		// ECEF-Z (meters)
  float    navxdot;		// Velocity in ECEF-X coordinate (m/sec)
  float    navydot;		// Velocity in ECEF-Y coordinate (m/sec)
  float    navzdot;		// Velocity in ECEF-Z coordinate (m/sec)
  double   navt;		// Receiver Clock offset (meters)
  double   navtdot;		// Rate of Receiver Clock offset (m/sec)
  unsigned short pdop;		// PDOP
  char     num_sats;		// numbers of satellites in the epoch
} PosData;
//-----------------
// The size of RawDataStruct  = (31) bytes
//--------------------------------
struct RawDataStruct
{ 
  double        rawrange;      /* SV raw range: raw transmit time is this   */
                               /*  value subtracted from receive time (sec) */
                               /*  (receive time rounded to nearest         */
                               /*  millisecond)                             */
  float          smth_corr;     /* Smoothing correction for ranges (meters)  */
  unsigned short smth_count;    /* Number of data points in smoothing.       */
  char           polarity_known;/* RESERVED                                  */
  unsigned char  warning;       /* Warning flag (BIT flags):                 */
            			       /*  Bit 1 ==> RESERVED                       */
                               /*  Bit 2 ==> RESERVED                       */
                               /*  Bit 3 ==> Carrier phase questionable.    */
                               /*  Bit 4 ==> Code phase questionable.       */
                               /*  Bit 5 ==> RESERVED                       */
                               /*  Bit 6 ==> PW tracking method used.       */
                               /*  Bit 7 ==> Possilbe loss of lock.         */
                               /*  Bit 8 ==> Loss of lock occured.          */
  unsigned char  goodbad;       /* Another health indicator:                 */
                               /*  22 ==> Code and carrier measured.        */
			       /*  23 ==> Same as 22 but additionally, nav  */
			       /*          message obtained but measurement */
                               /*          was not used in position         */
			       /*          computation.                     */
                               /*  24 ==> Same as 23 but codephase was used */
                               /*          in position computation.         */
  unsigned char ireg;          /* Signal to noise.                          */
  char         qa_phase;      /* QA phase check (0.001 cycles).            */
  int       doppler;       /* SV raw doppler. (0.0001Hz)                 */
  double    carphase;      /* Full carrier phase (cycles).              */
} RawData[3];
//---------------
// Each epoch has a SatStruct per SV
// The size of L1 rawdata struct = (35 * nav.num_sats) bytes
// The size of L2C rawdata struct = (66 * nav.num_sats) bytes
// The size of L2P rawdata struct = (97 * nav.num_sats) bytes
//---------------
struct SatStruct
{
  unsigned char svprn;		// SV PRN number
  unsigned char elevation;	// Satellite elevation angle (degrees)
  unsigned char azimuth;  	// Satellite azimuth (degrees)
  unsigned char chnind;   	// Channel index (id)
} SatData;
//define the struct of data
//对称正定矩阵求逆
  int bssgj(a,n)
  int n;
  double a[];
  { int i,j,k,m;
    double w,g,*b;
    b=malloc(n*sizeof(double));
    for (k=0; k<=n-1; k++)
      { w=a[0];
        if (fabs(w)+1.0==1.0)
          { free(b); printf("fail\n"); return(-2);}
        m=n-k-1;
        for (i=1; i<=n-1; i++)
          { g=a[i*n]; b[i]=g/w;
            if (i<=m) b[i]=-b[i];
            for (j=1; j<=i; j++)
              a[(i-1)*n+j-1]=a[i*n+j]+g*b[j];
          }
        a[n*n-1]=1.0/w;
        for (i=1; i<=n-1; i++)
          a[(n-1)*n+i-1]=b[i];
      }
    for (i=0; i<=n-2; i++)
    for (j=i+1; j<=n-1; j++)
      a[i*n+j]=a[j*n+i];
    free(b);
    return(2);
  }
//矩阵相乘，a(m,n)*b(n,k)=c(m,k)
  void brmul(a,b,m,n,k,c)
  int m,n,k;
  double a[],b[],c[];
  { int i,j,l,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { u=i*k+j; c[u]=0.0;
        for (l=0; l<=n-1; l++)
          c[u]=c[u]+a[i*n+l]*b[l*k+j];
      }
    return;
  }