Reorganize directory structure

examples/gps_measurement.py

@@ -0,0 +1,130 @@
+from filter.kalman_filter import LinearKalmanFilter
+import matplotlib.pyplot as plt
+import csv
+import numpy as np
+import geopy.distance
+
+
+def read_csv_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)
+
+
+"""
+A stationary system is considered (x1=latitude, x2=longitude):
+x1_{n+1} = x1_n
+x2_{n+1} = x2_n
+
+F = [[1, 0],
+     [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(kalman_filter_t):
+    # Read data
+    
+    x0_actual     = [48.993552704, 8.371038159]
+    x_measurement = read_csv_lat_long('examples/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 = kalman_filter_t(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()

examples/second_order_system.py

@@ -0,0 +1,76 @@
+from filter.kalman_filter import LinearKalmanFilter
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+"""
+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(kalman_filter_t):
+    # 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 = kalman_filter_t(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()