nominally working ekf

Author: betaBison

gnss_lib_py/algorithms/gnss_filters.py

@@ -10,11 +10,12 @@
 import numpy as np
 
 from gnss_lib_py.parsers.navdata import NavData
+from gnss_lib_py.algorithms.snapshot import solve_wls
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic
 from gnss_lib_py.utils.filters import BaseExtendedKalmanFilter
 
-def solve_ekf_gnss(measurements, init_dict = dict(),
-                   params_dict = dict()):
+def solve_gnss_ekf(measurements, init_dict = None,
+                   params_dict = None):
     """Runs a GNSS Extended Kalman Filter across each timestep.
 
     Runs an Extended Kalman Filter across each timestep and adds a new
@@ -44,21 +45,59 @@ def solve_ekf_gnss(measurements, init_dict = dict(),
                           "x_sv_m","y_sv_m","z_sv_m",
                           ])
 
+    if init_dict is None:
+        init_dict = {}
+
+    if "state_0" not in init_dict:
+        pos_0 = None
+        for _, _, measurement_subset in measurements.loop_time("gps_millis"):
+            pos_0 = solve_wls(measurement_subset)
+            if pos_0 is not None:
+                break
+
+        state_0 = np.zeros((7,1))
+        if pos_0 is not None:
+            state_0[:3,0] = pos_0[["x_rx_m","y_rx_m","z_rx_m"]]
+            state_0[6,0] = pos_0[["b_rx_m"]]
+
+        init_dict["state_0"] = state_0
+
+    if "sigma_0" not in init_dict:
+        sigma_0 = np.eye(init_dict["state_0"].size)
+        init_dict["sigma_0"] = sigma_0
+
+    if "Q" not in init_dict:
+        process_noise = np.eye(init_dict["state_0"].size)
+        init_dict["Q"] = process_noise
+
+    if "R" not in init_dict:
+        measurement_noise = np.eye(1) # gets overwritten
+        init_dict["R"] = measurement_noise
+
+    # initialize parameter dictionary
+    if params_dict is None:
+        params_dict = {}
+
+    if "motion_type" not in params_dict:
+        params_dict["motion_type"] = "constant_velocity"
+
+    if "measure_type" not in params_dict:
+        params_dict["measure_type"] = "pseudorange"
+
     # create initialization parameters.
     gnss_ekf = GNSSEKF(init_dict, params_dict)
 
     states = []
 
     for timestamp, delta_t, measurement_subset in measurements.loop_time("gps_millis"):
-
-        pos_sv_m = measurement_subset[["x_sv_m","y_sv_m","z_sv_m"]].T
+        pos_sv_m = measurement_subset[["x_sv_m","y_sv_m","z_sv_m"]]
         pos_sv_m = np.atleast_2d(pos_sv_m)
 
         corr_pr_m = measurement_subset["corr_pr_m"].reshape(-1,1)
 
         # remove NaN indexes
-        not_nan_indexes = ~np.isnan(pos_sv_m).any(axis=1)
-        pos_sv_m = pos_sv_m[not_nan_indexes]
+        not_nan_indexes = ~np.isnan(pos_sv_m).any(axis=0)
+        pos_sv_m = pos_sv_m[:,not_nan_indexes]
         corr_pr_m = corr_pr_m[not_nan_indexes]
 
         # prediction step
@@ -67,14 +106,15 @@ def solve_ekf_gnss(measurements, init_dict = dict(),
 
         # update step
         update_dict = {"pos_sv_m" : pos_sv_m}
+        update_dict["measurement_noise"] = np.eye(pos_sv_m.shape[1])
         gnss_ekf.update(corr_pr_m, update_dict=update_dict)
 
         states.append([timestamp] + np.squeeze(gnss_ekf.state).tolist())
 
     states = np.array(states)
 
     if states.size == 0:
-        warnings.warn("No valid state estimate computed in solve_ekf_gnss, "\
+        warnings.warn("No valid state estimate computed in solve_gnss_ekf, "\
                     + "returning None.", RuntimeWarning)
         return None
 
@@ -83,11 +123,13 @@ def solve_ekf_gnss(measurements, init_dict = dict(),
     state_estimate["x_rx_m"] = states[:,1]
     state_estimate["y_rx_m"] = states[:,2]
     state_estimate["z_rx_m"] = states[:,3]
-    state_estimate["b_rx_m"] = states[:,4]
+    state_estimate["vx_rx_mps"] = states[:,4]
+    state_estimate["vy_rx_mps"] = states[:,5]
+    state_estimate["vz_rx_mps"] = states[:,6]
+    state_estimate["b_rx_m"] = states[:,7]
 
-    lat,lon,alt = ecef_to_geodetic(state_estimate[["x_rx_m",
-                                                   "y_rx_m",
-                                                   "z_rx_m"]])
+    lat,lon,alt = ecef_to_geodetic(state_estimate[["x_rx_m","y_rx_m",
+                                   "z_rx_m"]].reshape(3,-1))
     state_estimate["lat_rx_deg"] = lat
     state_estimate["lon_rx_deg"] = lon
     state_estimate["alt_rx_deg"] = alt
@@ -118,7 +160,7 @@ def __init__(self, init_dict, params_dict):
         self.motion_type = params_dict.get('motion_type','stationary')
         self.measure_type = params_dict.get('measure_type','pseudorange')
 
-    def dyn_model(self, u, predict_dict=dict()):
+    def dyn_model(self, u, predict_dict=None):
         """Nonlinear dynamics
 
         Parameters
@@ -134,6 +176,9 @@ def dyn_model(self, u, predict_dict=dict()):
         new_x : np.ndarray
             Propagated state
         """
+        if predict_dict is None:
+            predict_dict = {}
+
         A = self.linearize_dynamics(predict_dict)
         new_x = A @ self.state
         return new_x
@@ -178,7 +223,7 @@ def measure_model(self, update_dict):
             raise NotImplementedError
         return z
 
-    def linearize_dynamics(self, predict_dict=dict()):
+    def linearize_dynamics(self, predict_dict=None):
         """Linearization of dynamics model
 
         Parameters
@@ -194,6 +239,9 @@ def linearize_dynamics(self, predict_dict=dict()):
             Dictionary of prediction parameters.
         """
 
+        if predict_dict is None:
+            predict_dict = {}
+
         # uses delta_t from predict_dict if exists, otherwise delta_t
         # from the class initialization.
         delta_t = predict_dict.get('delta_t', self.delta_t)
@@ -213,12 +261,14 @@ def linearize_measurements(self, update_dict):
         Parameters
         ----------
         update_dict : dict
-            Update dictionary containing satellite positions with key 'pos_sv_m'
+            Update dictionary containing satellite positions with key
+            ``pos_sv_m``.
 
         Returns
         -------
         H : np.ndarray
-            Jacobian of measurement model, dimension M x N
+            Jacobian of measurement model, of dimension
+            #measurements x #states
         """
         if self.measure_type == 'pseudorange':
             pos_sv_m = update_dict['pos_sv_m']

gnss_lib_py/algorithms/snapshot.py

@@ -102,9 +102,8 @@ def solve_wls(measurements, weight_type = None,
     state_estimate["z_rx_m"] = states[:,3]
     state_estimate["b_rx_m"] = states[:,4]
 
-    lat,lon,alt = ecef_to_geodetic(state_estimate[["x_rx_m",
-                                                   "y_rx_m",
-                                                   "z_rx_m"]])
+    lat,lon,alt = ecef_to_geodetic(state_estimate[["x_rx_m","y_rx_m",
+                                   "z_rx_m"]].reshape(3,-1))
     state_estimate["lat_rx_deg"] = lat
     state_estimate["lon_rx_deg"] = lon
     state_estimate["alt_rx_deg"] = alt

gnss_lib_py/utils/filters.py

@@ -18,7 +18,7 @@ class BaseFilter(ABC):
     ----------
     state_0 : np.ndarray
         Initial state estimate
-    P : np.ndarray
+    sigma_0 : np.ndarray
         Current uncertainty estimated for state estimate (2D covariance)
     """
 
@@ -62,7 +62,7 @@ def __init__(self, init_dict, params_dict):
         self.R = init_dict['R']
         self.params_dict = params_dict
 
-    def predict(self, u=None, predict_dict=dict()):
+    def predict(self, u=None, predict_dict=None):
         """Predict the state of the filter given the control input
 
         Parameters
@@ -73,15 +73,17 @@ def predict(self, u=None, predict_dict=dict()):
             Additional parameters needed to implement predict step
         """
         if u is None:
-            u = np.zeros((1,self.state_dim))
+            u = np.zeros((self.state_dim,1))
+        if predict_dict is None:
+            predict_dict = {}
         assert _check_col_vect(u, np.size(u)), "Control input is not a column vector"
         self.state = self.dyn_model(u, predict_dict)  # Can pass parameters via predict_dict
         A = self.linearize_dynamics(predict_dict)
         self.sigma = A @ self.sigma @ A.T + self.Q
         assert _check_col_vect(self.state, self.state_dim), "Incorrect state shape after prediction"
         assert _check_square_mat(self.sigma, self.state_dim), "Incorrect covariance shape after prediction"
 
-    def update(self, z, update_dict=dict()):
+    def update(self, z, update_dict=None):
         """Update the state of the filter given a noisy measurement of the state
 
         Parameters
@@ -91,9 +93,16 @@ def update(self, z, update_dict=dict()):
         update_dict : dict
             Additional parameters needed to implement update step
         """
+        if update_dict is None:
+            update_dict = {}
+
+        # uses process_noise from update_dict if exists, otherwise use
+        # process_noise from the class initialization.
+        measurement_noise = update_dict.get('measurement_noise', self.R)
+
         assert _check_col_vect(z, np.size(z)), "Measurements are not a column vector"
         H = self.linearize_measurements(update_dict)  # Can pass arguments via update_dict
-        S = H @ self.sigma @ H.T + self.R
+        S = H @ self.sigma @ H.T + measurement_noise
         K = self.sigma @ H.T @ np.linalg.inv(S)
         z_expect = self.measure_model(update_dict)  # Can pass arguments via update_dict
         assert _check_col_vect(z_expect, np.size(z)), "Expected measurements are not a column vector"
@@ -105,25 +114,25 @@ def update(self, z, update_dict=dict()):
         assert _check_square_mat(self.sigma, self.state_dim), "Incorrect covariance shape after update"
 
     @abstractmethod
-    def linearize_dynamics(self, predict_dict=dict()):
+    def linearize_dynamics(self, predict_dict=None):
         """Linearization of system dynamics, should return A matrix
         """
         raise NotImplementedError
 
     @abstractmethod
-    def linearize_measurements(self, update_dict=dict()):
+    def linearize_measurements(self, update_dict=None):
         """Linearization of measurement model, should return H matrix
         """
         raise NotImplementedError
 
     @abstractmethod
-    def measure_model(self, update_dict=dict()):
+    def measure_model(self, update_dict=None):
         """Non-linear measurement model
         """
         raise NotImplementedError
 
     @abstractmethod
-    def dyn_model(self, u, predict_dict=dict()):
+    def dyn_model(self, u, predict_dict=None):
         """Non-linear dynamics model
         """
         raise NotImplementedError
@@ -135,7 +144,7 @@ class BaseKalmanFilter(BaseExtendedKalmanFilter):
     model
     """
 
-    def dyn_model(self, u, predict_dict=dict()):
+    def dyn_model(self, u, predict_dict=None):
         """Linear dynamics model
 
         Parameters
@@ -149,12 +158,14 @@ def dyn_model(self, u, predict_dict=dict()):
         -------
         new_x : State after propagation
         """
+        if predict_dict is None:
+            predict_dict = {}
         A = self.linearize_dynamics(predict_dict)
         B = self.get_B(predict_dict)
         new_x = A @ self.state + B @ u
         return new_x
 
-    def measure_model(self, update_dict=dict()):
+    def measure_model(self, update_dict=None):
         """Linear measurement model
 
         Parameters
@@ -166,12 +177,14 @@ def measure_model(self, update_dict=dict()):
         -------
         z_expect : Measurement expected for current state
         """
+        if update_dict is None:
+            update_dict = {}
         H = self.linearize_measurements(update_dict)
         z_expect = H @ self.state
         return z_expect
 
     @abstractmethod
-    def get_B(self, predict_dict=dict()):
+    def get_B(self, predict_dict=None):
         """Map from control to state, should return B matrix
         """
         raise NotImplementedError
@@ -207,7 +220,7 @@ def __init__(self, init_dict, params_dict):
             self.N_sig = int(2 * self.state_dim + 1)
         self.params_dict = params_dict
 
-    def predict(self, u, predict_dict=dict()):
+    def predict(self, u, predict_dict=None):
         """Predict the state of the filter given the control input
 
         Parameters
@@ -218,6 +231,9 @@ def predict(self, u, predict_dict=dict()):
             Additional parameters needed to implement predict step
         """
 
+        if predict_dict is None:
+            predict_dict = {}
+
         N = self.state_dim
         N_sig = self.N_sig
         x_t_tm = np.zeros((N, N_sig))
@@ -238,7 +254,7 @@ def predict(self, u, predict_dict=dict()):
         assert _check_col_vect(self.state, self.state_dim), "Incorrect state shape after prediction"
         assert _check_square_mat(self.sigma, self.state_dim), "Incorrect covariance shape after prediction"
 
-    def update(self, z, update_dict=dict()):
+    def update(self, z, update_dict=None):
         """Update the state of the filter given a noisy measurement of the state
 
         Parameters
@@ -248,6 +264,9 @@ def update(self, z, update_dict=dict()):
         update_dict : dict
             Additional parameters needed to implement update step
         """
+        if update_dict is None:
+            update_dict = {}
+
         assert _check_col_vect(z, np.size(z)), "Measurements are not a column vector"
         N = self.state_dim
         N_sig = self.N_sig
@@ -309,13 +328,13 @@ def inv_U_transform(self, W, x_t_tm):
         return np.expand_dims(mu, axis=1), S
 
     @abstractmethod
-    def measure_model(self, x, update_dict=dict()):
+    def measure_model(self, x, update_dict=None):
         """Non-linear measurement model
         """
         raise NotImplementedError
 
     @abstractmethod
-    def dyn_model(self, x, u, predict_dict=dict()):
+    def dyn_model(self, x, u, predict_dict=None):
         """Non-linear dynamics model
         """
         raise NotImplementedError