GPS算法的深入研究（C源代码完整） 牛顿定律起到关键性作用！ 源代码, include, double

cuicaifeng

#include <stdio.h>
#include <math.h>
#include <./meschach/matrix.h>
#define C 1.0
double newton(MAT *SAT, VEC *PR, VEC *X0, double s, VEC *X)
{
VEC *X1, *dX, *I, *V;
MAT *H, *Ht, *M, *M1;
VEC *VS;
MAT *SIG, *U, *V2, *Hi;
int i, m;
double n = 1.0, p, gdop = 0.0;
  m = SAT -> m;

  if(m < 4) {
    H = m_get(m, m);
    Ht = m_get(m, m);
    M = m_get(m, m);
    M1 = m_get(m, m);
    dX = v_get(m);
    SIG = m_get(m, m);
    Hi = m_get(m, m);
    VS = v_get(m);
    V2 = m_get(m, m);
  }
  else {
    H = m_get(m, 4);
    Ht = m_get(m, 4);
    M = m_get(4, 4);
    M1 = m_get(4, 4);
    dX = v_get(4);
    SIG = m_get(4, m);
    Hi = m_get(4, m);
    VS = v_get(4);
    V2 = m_get(4, 4);
  }
  I = v_get(m);
  X1 = v_get(4);
  V = v_get(4);
  U = m_get(m, m);

  while(n > s) {

    for(i = 0; i < m; i ) {
      p = sqrt((SAT -> me[0] - X0 -> ve[0]) * (SAT -> me[0] - X0 -> ve[0]) (SAT -> me[1] - X0 -> ve[1]) * (SAT -> me[1] - X0 -> ve[1]) (SAT -> me[2] - X0 -> ve[2]) * (SAT -> me[2] - X0 -> ve[2]));
      if(m < 4) {
        switch(m) {
          case 1: H -> me[0] = C;
                  break;
          case 2: H -> me[0] = (X0 -> ve[2] - SAT -> me[2]) / p;
                  H -> me[1] = C;
                  break;
          case 3: H -> me[0] = (X0 -> ve[1] - SAT -> me[1]) / p;
                  H -> me[1] = (X0 -> ve[2] - SAT -> me[2]) / p;
                  H -> me[2] = C;
                  break;
          default: break;
        }
      }
      else {
        H -> me[0] = (X0 -> ve[0] - SAT -> me[0]) / p;
        H -> me[1] = (X0 -> ve[1] - SAT -> me[1]) / p;
        H -> me[2] = (X0 -> ve[2] - SAT -> me[2]) / p;
        H -> me[3] = C;
      }
    }
    for(i = 0; i < m; i ) {
      p = sqrt((SAT -> me[0] - X0 -> ve[0]) * (SAT -> me[0] - X0 -> ve[0]) (SAT -> me[1] - X0 -> ve[1]) * (SAT -> me[1] - X0 -> ve[1]) (SAT -> me[2] - X0 -> ve[2]) * (SAT -> me[2] - X0 -> ve[2]));
      I -> ve = PR -> ve - (p C * X0 -> ve[3]);
    }

    m_transp(H, Ht);
    m_mlt(Ht, H, M);
    m_inverse(M, M1);

    svd(H, U, V2, VS);
    m_transp(V2, V2);
    m_zero(SIG);
    if(m < 4) {
      for(i = 0; i < m; i )
        SIG -> me = 1.0 / VS -> ve;
    }
    else {
      for(i = 0; i < 4; i )
        SIG -> me = 1.0 / VS -> ve;
    }      
    m_mlt(SIG, U, Hi);
    m_mlt(V2, Hi, SIG);
    mv_mlt(SIG, I, dX);

    if(m < 4) {
      v_copy(X0, X1);
      switch(m) {
        case 1: X1 -> ve[3] = X0 -> ve[3] dX -> ve[0];
                break;
        case 2: X1 -> ve[2] = X0 -> ve[2] dX -> ve[0];
                X1 -> ve[3] = X0 -> ve[3] dX -> ve[1];
                break;
        case 3: X1 -> ve[1] = X0 -> ve[1] dX -> ve[0];
                X1 -> ve[2] = X0 -> ve[2] dX -> ve[1];
                X1 -> ve[3] = X0 -> ve[3] dX -> ve[2];
                break;
        default: break;
      }
    }
    else
      v_add(X0, dX, X1);      

    v_sub(X1, X0, V);
    n = v_norm2(V);
    v_copy(X1, X0);
  }

  v_copy(X1, X);
  /* Cacul du GDOP */
  if (m < 4) {
      for(i = 0; i < m; i )
        gdop = M1 -> me;
  }
  else {
      for(i = 0; i < 4; i )
        gdop = M1 -> me;
  }   
  gdop = sqrt(fabs(gdop));

  v_free(VS);
  v_free(X1);
  v_free(V);
  v_free(I);
  v_free(dX);
  m_free(V2);
  m_free(U);
  m_free(Hi);
  m_free(SIG);
  m_free(M1);
  m_free(M);
  m_free(Ht);
  m_free(H);
  return(gdop);
}