diff --git a/.gitignore b/.gitignore index 709c80b..413bade 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ kalman_filter/__pycache__ +kalman_filter/*.pyc diff --git a/kalman_filter/__main__.py b/kalman_filter/__main__.py index 6f540f3..1b69c31 100644 --- a/kalman_filter/__main__.py +++ b/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() diff --git a/kalman_filter/gps_measurement.py b/kalman_filter/gps_measurement.py new file mode 100644 index 0000000..7ca77d0 --- /dev/null +++ b/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() diff --git a/kalman_filter/second_order_system.py b/kalman_filter/second_order_system.py new file mode 100644 index 0000000..35d91c7 --- /dev/null +++ b/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() +