Refactoring to include multiple examples

.gitignore

@@ -1 +1,2 @@
 kalman_filter/__pycache__
+kalman_filter/*.pyc

kalman_filter/__main__.py

@@ -1,78 +1,10 @@
-from .kalman_filter import *
-import matplotlib.pyplot as plt
-
-
-"""
-Second order LTI-system:
-x_{n+1} = x_n + dt*v_n
-v_{n+1} = v_n
-
-F = [[1, 1],
-     [0, 1]]
-
-
-No known control inputs:
-G = 0
-
-
-Measured values are the states:
-H = [[1, 0],
-     [0, 1]]
-
-
-The reset of the parameters are empirically determined
-"""
+from . import gps_measurement as gps
+from . import second_order_system as sos
 
 
 def main():
-    # Simulation parameter definitions
-
-    n_iterations = 500
-    std_dist     = [30, 0.5]
-    x0_actual    = [50, 3]
-    
-    x_actual      = np.zeros([n_iterations, 2])
-    x_kalman      = np.zeros([n_iterations, 2])
-    x_measurement = np.zeros([n_iterations, 2])
-
-
-    # Kalman Filter parameter definition
-
-    x0 = [100, 50]
-    P0 = [[10, 0], [0, 5]]
-    H  = [[1, 0], [0, 1]]
-    Q  = [[0.2, 0], [0, 2]]
-    F  = [[1, 1], [0, 1]]
-    G  = 0
-    R  = [[std_dist[0]**2, 0], [0, std_dist[1]**2]] 
-
-    x_actual[0] = x0_actual
-    x_kalman[0] = x0
-
-    
-    # Simulation
-
-    f = KalmanFilter(x0, P0, H, Q, F, G)
-
-
-    for i in range(1, n_iterations):
-        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
-
-        f.step(measurement, R, 0)
-
-        x_actual[i]      = np.dot(F, x_actual[i-1])
-        x_kalman[i]      = f.x
-        x_measurement[i] = measurement
-
-
-    # Show results
-
-    t = np.linspace(0, n_iterations, n_iterations)
-    
-    plt.plot(t, x_actual[:, 0])
-    plt.plot(t, x_kalman[:, 0])
-    plt.plot(t, x_measurement[:, 0])
-    plt.show()
+    gps.simulate()
+#    sos.simulate()
 
 if __name__ == '__main__':
     main()

kalman_filter/gps_measurement.py

@@ -0,0 +1,108 @@
+from .kalman_filter import *
+import matplotlib.pyplot as plt
+import csv
+import geopy.distance
+
+
+def get_lat_long(filename):
+    result = []
+
+    with open(filename) as csvfile:
+        reader = csv.reader(csvfile, delimiter=',')
+
+        for row in reader:
+            try:
+                lat = float(row[1])
+                long = float(row[2])
+                result.append([lat, long])
+            except:
+                pass
+
+        result = np.array(result)
+        return result
+
+def lat_long_to_km(x1, x2):
+    result = []
+
+    for i in range(0, len(x1)):
+        result.append(geopy.distance.geodesic(x1[i], x2[i]).km)
+
+    return np.array(result)
+
+
+
+def simulate():
+    # Read data
+    
+    x0_actual     = [48.993552704, 8.371038159]
+    x_measurement = get_lat_long('/home/atsouchlos/git/kalman_filter/res/29_03_2022_Unterreut_8_Karlsruhe.csv')
+
+    
+    # Simulation parameter definitions
+
+    n_iterations = len(x_measurement)
+    lat_std_dev  = np.std(x_measurement[:, 0])
+    long_std_dev = np.std(x_measurement[:, 1])
+    std_dev      = [lat_std_dev/2, long_std_dev/2]
+
+    print("Computed stdandard deviation:")
+    print(std_dev)
+    
+    x_actual      = np.zeros([n_iterations, 2])
+    x_kalman      = np.zeros([n_iterations, 2])
+
+
+
+    # Kalman Filter parameter definition
+
+    x0 = [48.99355275, 8.37103818]
+    P0 = [[10, 0], [0, 5]]
+    H  = [[1, 0], [0, 1]]
+    Q  = [[5e-14, 0], [0, 5e-14]]
+    F  = [[1, 0], [0, 1]]
+    G  = 0
+    R  = [[std_dev[0]**2, 0], [0, std_dev[1]**2]] 
+
+    x_actual[0] = x0_actual
+    x_kalman[0] = x0
+
+    
+    # Simulation
+
+    f = KalmanFilter(x0, P0, H, Q, F, G)
+
+
+    for i in range(1, n_iterations):
+        f.step(x_measurement[i], R, 0)
+
+        x_actual[i]      = np.dot(F, x_actual[i-1])
+        x_kalman[i]      = f.x
+
+
+    # Show results
+
+    t = np.linspace(0, n_iterations, n_iterations)
+   
+    fig1 = plt.figure("Measured vs Filtered vs Actual")
+    ax1 = fig1.add_axes([0.05,0.05,0.4,0.9])
+    ax1.plot(t, x_actual[:, 0], label="Actual Latitude")
+    ax1.plot(t, x_kalman[:, 0], label="Kalman Filter Latitude")
+    ax1.plot(t, x_measurement[:, 0], label="Measured Latitude")
+    ax1.legend()
+
+    ax2 = fig1.add_axes([0.55,0.05,0.4,0.9])
+    ax2.plot(t, x_actual[:, 1], label="Actual Longitude")
+    ax2.plot(t, x_kalman[:, 1], label="Kalman Filter Longitude")
+    ax2.plot(t, x_measurement[:, 1], label="Measured Longitude")
+    ax2.legend()
+
+    x_error_kalman      = lat_long_to_km(x_actual, x_kalman)
+    x_error_measurement = lat_long_to_km(x_actual, x_measurement)
+
+    fig2 = plt.figure("Error in Position")
+    ax3 = fig2.add_axes([0.1,0.1,0.8,0.8])
+    ax3.plot(t, x_error_kalman*1000, label="Error in estimated position in m")
+    ax3.plot(t, x_error_measurement*1000, label="Error in measured position in m")
+    ax3.legend()
+    
+    plt.show()

kalman_filter/second_order_system.py

@@ -0,0 +1,76 @@
+from .kalman_filter import *
+import matplotlib.pyplot as plt
+
+
+"""
+Second order LTI-system:
+x_{n+1} = x_n + dt*v_n
+v_{n+1} = v_n
+
+F = [[1, 1],
+     [0, 1]]
+
+
+No known control inputs:
+G = 0
+
+
+Measured values are the states:
+H = [[1, 0],
+     [0, 1]]
+
+
+The reset of the parameters are empirically determined
+"""
+
+
+def simulate():
+    # Simulation parameter definitions
+
+    n_iterations = 500
+    std_dist     = [30, 0.5]
+    x0_actual    = [50, 3]
+    
+    x_actual      = np.zeros([n_iterations, 2])
+    x_kalman      = np.zeros([n_iterations, 2])
+    x_measurement = np.zeros([n_iterations, 2])
+
+
+    # Kalman Filter parameter definition
+
+    x0 = [100, 50]
+    P0 = [[10, 0], [0, 5]]
+    H  = [[1, 0], [0, 1]]
+    Q  = [[0.2, 0], [0, 2]]
+    F  = [[1, 1], [0, 1]]
+    G  = 0
+    R  = [[std_dist[0]**2, 0], [0, std_dist[1]**2]] 
+
+    x_actual[0] = x0_actual
+    x_kalman[0] = x0
+
+    
+    # Simulation
+
+    f = KalmanFilter(x0, P0, H, Q, F, G)
+
+
+    for i in range(1, n_iterations):
+        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
+
+        f.step(measurement, R, 0)
+
+        x_actual[i]      = np.dot(F, x_actual[i-1])
+        x_kalman[i]      = f.x
+        x_measurement[i] = measurement
+
+
+    # Show results
+
+    t = np.linspace(0, n_iterations, n_iterations)
+    
+    plt.plot(t, x_actual[:, 0])
+    plt.plot(t, x_kalman[:, 0])
+    plt.plot(t, x_measurement[:, 0])
+    plt.show()
+