# Path: mlat/geodesy.py
# WGS84_A = 6378137.0
# WGS84_F = 1.0/298.257223563
# WGS84_B = WGS84_A * (1 - WGS84_F)
# WGS84_ECC_SQ = 1 - WGS84_B * WGS84_B / (WGS84_A * WGS84_A)
# WGS84_ECC = math.sqrt(WGS84_ECC_SQ)
# SPHERICAL_R = 6371e3
#     N = WGS84_A / math.sqrt(1 - WGS84_ECC_SQ * math.sin(lat)**2)
# def llh2ecef(llh):
# def ecef2llh(ecef):
# def greatcircle(p0, p1):
# def ecef_distance(p0, p1):
# 
# Path: mlat/profile.py
# def trackcpu(f, **kwargs):
# def dump_cpu_profiles(tofile=None):
# def trackcpu(f, name=None, **kwargs):
#     def cpu_measurement_wrapper(*args, **kwargs):
# def dump_cpu_profiles(tofile=sys.stderr):


import math
import numpy
import pykalman.unscented
import functools
import logging
from mlat import geodesy, constants, profile
        return an altitude observation and N-1 pseudorange observations; the
        pseudoranges are relative to the first receiver's pseudorange."""

        x, y, z = state[0:3]

        n = len(positions)
        obs = numpy.zeros(n)

        _, _, obs[0] = geodesy.ecef2llh((x, y, z))

        rx, ry, rz = positions[0]
        zero_range = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5

        for i in range(1, n):
            rx, ry, rz = positions[i]
            obs[i] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range

        return obs

    def _update_derived(self):
        """Update derived values from self._mean and self._cov"""

        self.position = self._mean[0:3]
        self.velocity = self._mean[3:6]

        pe = numpy.trace(self._cov[0:3, 0:3])
        self.position_error = 1e6 if pe < 0 else math.sqrt(pe)
        ve = numpy.trace(self._cov[3:6, 3:6])
        self.velocity_error = 1e6 if ve < 0 else math.sqrt(ve)


# Path: mlat/geodesy.py
# WGS84_A = 6378137.0
# WGS84_F = 1.0/298.257223563
# WGS84_B = WGS84_A * (1 - WGS84_F)
# WGS84_ECC_SQ = 1 - WGS84_B * WGS84_B / (WGS84_A * WGS84_A)
# WGS84_ECC = math.sqrt(WGS84_ECC_SQ)
# SPHERICAL_R = 6371e3
#     N = WGS84_A / math.sqrt(1 - WGS84_ECC_SQ * math.sin(lat)**2)
# def llh2ecef(llh):
# def ecef2llh(ecef):
# def greatcircle(p0, p1):
# def ecef_distance(p0, p1):
# 
# Path: mlat/profile.py
# def trackcpu(f, **kwargs):
# def dump_cpu_profiles(tofile=None):
# def trackcpu(f, name=None, **kwargs):
#     def cpu_measurement_wrapper(*args, **kwargs):
# def dump_cpu_profiles(tofile=sys.stderr):


# Path: mlat/server/kalman.py
import math
import numpy
import pykalman.unscented
import functools
import logging
from mlat import geodesy, constants, profile

        return an altitude observation and N-1 pseudorange observations; the
        pseudoranges are relative to the first receiver's pseudorange."""

        x, y, z = state[0:3]

        n = len(positions)
        obs = numpy.zeros(n)

        _, _, obs[0] = geodesy.ecef2llh((x, y, z))

        rx, ry, rz = positions[0]
        zero_range = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5

        for i in range(1, n):
            rx, ry, rz = positions[i]
            obs[i] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range

        return obs

    def _update_derived(self):
        """Update derived values from self._mean and self._cov"""

        self.position = self._mean[0:3]
        self.velocity = self._mean[3:6]

        pe = numpy.trace(self._cov[0:3, 0:3])
        self.position_error = 1e6 if pe < 0 else math.sqrt(pe)
        ve = numpy.trace(self._cov[3:6, 3:6])
        self.velocity_error = 1e6 if ve < 0 else math.sqrt(ve)


        lat, lon, alt = self.position_llh = geodesy.ecef2llh(self.position)