Merge pull request #6 from PyGNC/orbit_estimator_timing

Orbit estimator prediction frequency change

flight/pygnc/algorithms/OrbitEstimator/orbit_ekf.py

@@ -1,7 +1,7 @@
 import autograd.numpy as np
 import brahe
 from scipy.linalg import sqrtm
-
+from scipy.linalg import qr
 from .ekf_core import EKFCore
 
 
@@ -160,3 +160,24 @@ def initialize_state(self, state_eci):
         # initial state
         x0 = np.concatenate([state_eci, np.zeros(3), 1e-3 * np.ones(3)])
         super().initialize_state(x0)
+
+    def predict(self, dt):
+        self.x, self.F = super().predict(dt)
+
+    def update(self, y):
+        """
+        EKF update step with no prediction
+        """
+        # innovation step
+        Z, C = self.innovation(y, self.x, self.F)
+
+        # calculate kalman gain
+        L = self.kalman_gain(self.F, C)
+
+        # update the state
+        self.x = self.x + L @ Z
+
+        e = np.vstack((self.F @ (np.identity(12) - L @ C).T, self.sqrt_R @ L.T))
+
+        # update the square root covariance
+        _, self.F = qr(e, mode="economic")

flight/pygnc/common/zmq_messaging.py

@@ -78,7 +78,6 @@ def __init__(
         self.subscriber.connect(f"tcp://localhost:{port}")
 
     def receive(self, block=True):
-
         block_flag = 0 if block else zmq.NOBLOCK
 
         try:

flight/pygnc/tasks/orbit_estimator.py

@@ -10,23 +10,33 @@
 from ..algorithms.OrbitEstimator import OrbitEKF
 
 
+def predict_orbit_ekf(orbit_ekf, prev_epoch, dt=5.0):
+    orbit_ekf.predict(dt)
+    new_epoch = prev_epoch + dt  # addition of seconds is defined in brahe
+    return new_epoch
+
+
 def update_orbit_ekf(orbit_ekf, gps_message, prev_epoch=None):
     # get position and velocity state from gps
     state_measurement_ecef = np.concatenate(
         (gps_message.position, gps_message.velocity)
     )
+
     measurement_epoch = transformations.gps_week_milliseconds_to_brahe_epoch(
         gps_message.gps_week, gps_message.gps_milliseconds
     )
+
     state_measurement_eci = brahe.frames.sECEFtoECI(
         measurement_epoch, state_measurement_ecef
     )
+
     if prev_epoch is None:
         # need to initialize ekf state with first measurement
         orbit_ekf.initialize_state(state_measurement_eci)
     else:
         dt = measurement_epoch - prev_epoch
-        orbit_ekf.update(state_measurement_eci, dt)
+        orbit_ekf.predict(dt)
+        orbit_ekf.update(state_measurement_eci)
 
     return measurement_epoch
 
@@ -49,7 +59,6 @@ def send_orbit_estimate_message(
 
 def main():
     # print("Orbit Estimator Task")
-
     oem_pub = zmqMessagePublisher(messages.OrbitEstimateMessage)
 
     # instantiate orbit estimator
@@ -59,15 +68,26 @@ def main():
     batch_data = data_parsing.unpack_batch_sensor_gps_file_to_messages_iterable(
         pygnc_config.batch_sensor_gps_filepath
     )
+
+    # We predict the next state every 5 sec
+    # Every 25 sec, we process the measurement packet with an EKF update
     packet_count = 0
     prev_epoch = None
+    in_between_prediction_dt = 5
+
+    # estimates = []
     for bd in batch_data:
         # print(f"Packet count = {packet_count}")
         sensor_messages, gps_message = bd
         prev_epoch = update_orbit_ekf(orbit_ekf, gps_message, prev_epoch)
         send_orbit_estimate_message(oem_pub, prev_epoch, orbit_ekf, sensor_messages[-1])
         packet_count += 1
 
+        # estimates.append(orbit_ekf.x)
+        for i in range(in_between_prediction_dt - 1):
+            prev_epoch = predict_orbit_ekf(orbit_ekf, prev_epoch)
+            # estimates.append(orbit_ekf.x)
+
     # print("Batch orbit estimation completed")
 
     # print("Final state estimate:")

scenario_generator/scenario_generator.jl

@@ -215,11 +215,26 @@ function generate_scenario(
 
 end
 
-state_hist, time_hist, measurement_history = simulate_scenario()
+state_hist, time_hist, measurement_history = simulate_scenario(batch_length_s=3 * 60 * 60)
+
 measurements_to_batch_file(
     measurement_history,
-    1 * 60 * 60, # number of seconds of sensor data to provide
+    3 * 60 * 60, # number of seconds of sensor data to provide
     25, # batch sample period for gps measurements
     5, # batch sample period for other sensor data
-    joinpath("..", "scenarios", "default_scenario"),
-)
+    joinpath("scenarios", "default_scenario"),
+)
+
+"""using DelimitedFiles
+writedlm("state_hist.txt", state_hist, ',')
+writedlm("time_hist.txt", time_hist, ',')
+writedlm("measurement_hist.txt", measurement_history, ',')"""
+
+
+"""gg = []
+n= size(measurement_history, 1)
+for i=1:n
+    push!(gg, [measurement_history[i].gps_position; measurement_history[i].gps_velocity])
+end
+
+g = Matrix(transpose(hcat(gg...)))"""