receiver.py

while epoch < len(sat_epoch_size) - 1:

    k = sat_epoch_size[epoch]
    m = sat_epoch_size[epoch]

    norm_count = sat_epoch_size[epoch+1]
    sum_count = norm_count - 1
    if init == 0:
      temp = b12(satellite_output.ts[init],pi,sidereal_day)
      # print "b12: x,y,z  ",temp
      x_veh_coords.append(temp[0])
      y_veh_coords.append(temp[1])
      z_veh_coords.append(temp[2])
      print "x,y,z   ",x_veh_coords[epoch],y_veh_coords[epoch],z_veh_coords[epoch]
    # if init > 1:
      # N = []
      # A = []
      # X=[]
      # Y=[]
      # Z=[]

      # dXdx = []
      # dXdy = []
      # dXdz = []

      # dYdx = dXdy
      # dYdy = []
      # dYdz = []

      # dZdx = dXdz
      # dZdy = dYdz
      # dZdz = []

    if epoch != 0:
      # This helps count through each while loop.
      # this is necessary for the sum_count while loop to work correctly
      j = epoch
      sum_count = norm_count - 1 - epoch
      m -= epoch

    print "----------------------------------------------------------"
    print "Counters: epoch | i | j | k | m | norm_count | sum_count: "
    print "           ",epoch,"   ", i," ", j," ", k," ", m,"     ", norm_count,"        ", sum_count
    print "----------------------------------------------------------"

    # Calculate the norms of current epoch
    # print "x_veh_coords[epoch]:  ",x_veh_coords[epoch]
    while k < norm_count: #sat_epoch_size[epoch]:
      # N.append( dec.sqrt( (dec(satellite_output.x[k]) - dec(x_veh_coords[epoch]))**2 + (dec(satellite_output.y[k]) - dec(y_veh_coords[epoch]))**2 + (dec(satellite_output.z[k]) - dec(z_veh_coords[epoch]))**2 ) )
      # print "K: ",k, "Sat: ", satellite_output.i[k]
      N.insert(k, ( dec.sqrt( (dec(satellite_output.x[k]) - dec(x_veh_coords[epoch]))**2 + (dec(satellite_output.y[k]) - dec(y_veh_coords[epoch]))**2 + (dec(satellite_output.z[k]) - dec(z_veh_coords[epoch]))**2 ) ) )
      print "NORM k: ", k, N[k], "Sat: ", satellite_output.i[k]
      # print "x_veh_coords[epoch]:  ",x_veh_coords[epoch]
      # print "satellite_output.x[k]:  ", satellite_output.x[k]
      k += 1
    print "----------------------------------------------------------"

    # Calculate the sumation
    while m < sum_count:
      # print "m:  ",m, sum_count
      A.insert(m, N[m+1+j] - N[m+j] - (speed_of_light)*( satellite_output.ts[m+j] - satellite_output.ts[m+1+j] ) )
      X.insert(m, (dec(satellite_output.x[m+j]) - dec(x_veh_coords[epoch]))/( N[m+j] ) - (dec(satellite_output.x[m+1+j]) - dec(x_veh_coords[epoch]))/( N[m+1+j] ) )
      Y.insert(m, (dec(satellite_output.y[m+j]) - dec(y_veh_coords[epoch]))/( N[m+j] ) - (dec(satellite_output.y[m+1+j]) - dec(y_veh_coords[epoch]))/( N[m+1+j] ) )
      Z.insert(m, (dec(satellite_output.z[m+j]) - dec(z_veh_coords[epoch]))/( N[m+j] ) - (dec(satellite_output.z[m+1+j]) - dec(z_veh_coords[epoch]))/( N[m+1+j] ) )
      # print " SUM m:  ", m, A[m], X[m], Y[m], Z[m], "Sat: ", satellite_output.i[m+j]
      # print "NORM m: ", m, N[m+j], N[m+1+j]

      # Calculate all partial derivatives that are outside of the partial derivative sums.
      dXdx.insert(m, ( N[m+1+j]**2 - (dec(satellite_output.x[m+j]) - dec(x_veh_coords[epoch]))**2 )/N[m+1+j]**3 - ( N[m+j]**2 - (dec(satellite_output.x[m+j]) - dec(x_veh_coords[epoch]))**2 )/N[m+j]**3 )
      dXdy.insert(m, ( (dec(satellite_output.y[m+j]) - dec(y_veh_coords[epoch]))*(dec(satellite_output.x[m+j]) - dec(x_veh_coords[epoch])) )/N[m+j]**3 - ( (dec(satellite_output.y[m+1+j]) - dec(y_veh_coords[epoch]))*(dec(satellite_output.x[m+1+j]) - dec(x_veh_coords[epoch])) )/N[m+1+j]**3 )
      dXdz.insert(m, ( (dec(satellite_output.z[m+j]) - dec(z_veh_coords[epoch]))*(dec(satellite_output.x[m+j]) - dec(x_veh_coords[epoch])) )/N[m+j]**3 - ( (dec(satellite_output.z[m+1+j]) - dec(z_veh_coords[epoch]))*(dec(satellite_output.x[m+1+j]) - dec(x_veh_coords[epoch])) )/N[m+1+j]**3 )
      dYdx = dXdy
      dYdy.insert(m, ( N[m+1+j]**2 - (dec(satellite_output.y[m+j]) - dec(y_veh_coords[epoch]))**2 )/N[m+1+j]**3 - ( N[m+j]**2 - (dec(satellite_output.y[m+j]) - dec(y_veh_coords[epoch]))**2 )/N[m+j]**3 )
      dYdz.insert(m, ( (dec(satellite_output.y[m+j]) - dec(y_veh_coords[epoch]))*(dec(satellite_output.z[m+j]) - dec(z_veh_coords[epoch])) )/N[m+j]**3 - ( (dec(satellite_output.y[m+1+j]) - dec(y_veh_coords[epoch]))*(dec(satellite_output.z[m+1+j]) - dec(z_veh_coords[epoch])) )/N[m+1+j]**3 )
      dZdx = dXdz
      dZdy = dYdz
      dZdz.insert(m, ( N[m+1+j]**2 - (dec(satellite_output.z[m+j]) - dec(z_veh_coords[epoch]))**2 )/N[m+1+j]**3 - ( N[m+j]**2 - (dec(satellite_output.z[m+j]) - dec(z_veh_coords[epoch]))**2 )/N[m+j]**3 )
      # print " m: ",m, dXdx[m]
      # print "Sat[m+j]: ",m+j, satellite_output.i[m+j], "Sat[m+1+j]: ",m+1+j, satellite_output.i[m+1+j]
      # print "dXdx dXdy dXdz  ",m,dXdx[m], dXdy[m], dXdz[m]
      # print "dYdx dYdy dYdz  ",m,dYdx[m], dYdy[m], dYdz[m]
      # print "dZdx dZdy dZdz  ",m,dZdx[m], dZdy[m], dZdz[m]
      m += 1
    # print "----------------------------------------------------------"

    fx = 0

    dfdx = 0
    dfdy = 0
    dfdz = 0

    d2fdx2 = 0
    d2fdxdy = 0
    d2fdxdz = 0

    d2fdydx = d2fdxdy
    d2fdy2 = 0
    d2fdydz = 0

    d2fdzdx = d2fdxdz
    d2fdzdy = d2fdydz
    d2fdz2 = 0

    # Sum all of the 1st and 3nd order partials
    # while i < sum_count:
    for i in range(sum_count):
      dfdx += 2*A[i]*X[i]
      dfdy += 2*A[i]*Y[i]
      dfdz += 2*A[i]*Z[i]
      d2fdx2 += 2*( X[i]**2 + A[i]*dXdx[i] )
      d2fdxdy += 2*( X[i]*Y[i]+A[i]*dXdy[i] )
      d2fdxdz += 2*( X[i]*Z[i]+A[i]*dXdz[i] )
      d2fdydx = d2fdxdy
      d2fdy2 += 2*( Y[i]**2 + A[i]*dYdy[i] )
      d2fdydz += 2*( Y[i]*Z[i]+A[i]*dYdz[i] )
      d2fdzdx = d2fdxdz
      d2fdzdy = d2fdydz
      d2fdz2 += 2*( Z[i]**2 + A[i]*dZdz[i] )

    # Solve J(x)*s = -F(x)
    J = array([ [d2fdx2], [d2fdxdy], [d2fdxdz], [d2fdydx], [d2fdy2], [d2fdydz], [d2fdzdx], [d2fdzdy], [d2fdz2] ], dtype=numpy.dtype(decimal.Decimal))
    J.shape = (3,3)
    J = scipy.matrix(J.copy())
    # print A
    # b = array([X[i], Y[i], Z[i]])
    b = array([ [dfdx], [dfdy], [dfdz] ], dtype=numpy.dtype(decimal.Decimal))
    b.shape = (3,1)
    b = scipy.matrix(b.copy())
    s = linalg.solve(J,-b)
    prev_x_veh_coords = x_veh_coords[epoch]
    prev_y_veh_coords = y_veh_coords[epoch]
    prev_z_veh_coords = z_veh_coords[epoch]
    x_veh_coords[epoch] += dec(s[0,0])
    y_veh_coords[epoch] += dec(s[1,0])
    z_veh_coords[epoch] += dec(s[2,0])
    # print "x, y, z  ", x_veh_coords[epoch], y_veh_coords[epoch], z_veh_coords[epoch]
    prev_X = [prev_x_veh_coords, prev_y_veh_coords, prev_z_veh_coords]
    curr_X = [x_veh_coords[epoch], y_veh_coords[epoch], z_veh_coords[epoch]]
    distance_check = vec_norm(prev_X,curr_X)
    # print "Distance check:  ", distance_check
    if distance_check < 0.01:
      epoch += 1
      init = 1

    init += 1