# 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
        self.velocity_error = None  # m/s

        # .. some derived values in more useful reference frames
        self.position_llh = None    # LLH
        self.velocity_enu = None    # ENU
        self.heading = None         # degrees
        self.ground_speed = None    # m/s
        self.vertical_speed = None  # m/s

    def observation_function_without_altitude(self, state, *, positions):
        """Kalman filter observation function.

        Given state (position,...) and a list of N receiver positions,
        return 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-1)

        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-1] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range

        return obs


# 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

        self.velocity_error = None  # m/s

        # .. some derived values in more useful reference frames
        self.position_llh = None    # LLH
        self.velocity_enu = None    # ENU
        self.heading = None         # degrees
        self.ground_speed = None    # m/s
        self.vertical_speed = None  # m/s

    def observation_function_without_altitude(self, state, *, positions):
        """Kalman filter observation function.

        Given state (position,...) and a list of N receiver positions,
        return 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-1)

        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-1] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range

        return obs


    def observation_function_with_altitude(self, state, *, positions):