-
Notifications
You must be signed in to change notification settings - Fork 1
/
soumil_env.py
129 lines (106 loc) · 5.48 KB
/
soumil_env.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
# Environment Setup
# Objective: Simulate Motion of the Hyperloop Pod
# Key Components: 1) Equation to Simulate true motion of the Hyperloop Pod
# 2) Equation to model measured Position
# 3) Physics Based Expected Position and Velocity Equation
# Assumptions: - Sample Measured position with 1m standard deviation
# - Sample Measured velocity with 0.1km/hr standard deviation
# - Constant Acceleration
import argparse
import numpy as np
import random
import matplotlib.pyplot as plt
class Sampler:
def __init__(self, standard_deviation = 0):
self.mean = 0
self.standard_deviation = standard_deviation
self.sample = []
def get_sample_value(self, true_value):
self.mean = true_value
self.sample = np.random.normal(self.mean, self.standard_deviation, 100)
return self.sample[np.random.randint(100)]
class KalmanFilter:
def __init__(self, Q = 0.1, R = 0.1, H = 1, A = 1):
self.q = Q
self.r = R
self.h = H
self.a = A
self.number_of_steps = 0
self.p = 0
def setup(self, no_steps, p_value = 1):
self.number_of_steps = no_steps
self.p = p_value
def update(self, calculated_value, measured_value):
temp_p = self.a * self.a * self.p + self.q
kalman_gain = self.h * temp_p / (self.h * self.h * temp_p + self.r)
kalman_value = calculated_value + kalman_gain * (measured_value - self.h * calculated_value)
self.p = (1 - kalman_gain * self.h ) * temp_p
return kalman_value
class Simulation:
def __init__(self, total_time, time_step):
self.total_time = total_time
self.time_step = time_step
self.no_steps = int(total_time / time_step + 1)
self.time = np.linspace(0, self.total_time, self.no_steps)
self.velocity = np.zeros(self.no_steps)
self.true_position = np.zeros(self.no_steps)
self.measured_position = np.zeros(self.no_steps)
self.calculate_position = np.zeros(self.no_steps)
self.expected_velocity = np.zeros(self.no_steps)
self.kalman_position = np.zeros(self.no_steps)
self.velocity_measure = None
self.position_meausre = None
self.error_kalman_true = np.zeros(self.no_steps)
self.error_calculate_true = np.zeros(self.no_steps)
self.error_measure_true = np.zeros(self.no_steps)
self.kalman_filter = KalmanFilter()
def setup(self, expected_velocity, start_time, velocity_deviation, position_deviation):
self.expected_velocity[int(start_time):] = expected_velocity
self.velocity_measure = Sampler(velocity_deviation)
self.position_meausre = Sampler(position_deviation)
self.kalman_filter.setup(self.no_steps)
def sum(self, list):
total = 0
for item in list:
total = total + item
return total
def simulate(self):
plt.figure(1, figsize = (5, 4))
plt.ion()
plt.show()
for i in range(self.no_steps - 1):
self.true_position[i] = self.true_position[i - 1] + self.expected_velocity[i - 1] * self.time_step
self.measured_position[i] = self.position_meausre.get_sample_value(self.true_position[i])
self.velocity[i] = self.velocity_measure.get_sample_value(self.expected_velocity[i])
self.calculate_position[i] = self.calculate_position[i - 1] + self.velocity[i] * self.time_step
self.kalman_position[i] = self.kalman_filter.update(self.calculate_position[i], self.measured_position[i])
self.error_kalman_true[i] = abs(self.kalman_position[i] - self.true_position[i])
self.error_calculate_true[i] = abs(self.calculate_position[i] - self.true_position[i])
self.error_measure_true[i] = abs(self.measured_position[i] - self.true_position[i])
plt.clf()
plt.subplot(2, 1, 1)
plt.plot(self.time[0: i + 1], self.measured_position[0: i + 1], 'b-', linewidth = 1)
plt.plot(self.time[0: i + 1], self.calculate_position[0: i + 1], 'g-', linewidth = 1)
plt.plot(self.time[0: i + 1], self.kalman_position[0: i + 1], 'm-', linewidth = 1)
plt.plot(self.time[0: i + 1], self.true_position[0: i + 1], 'r-', linewidth = 1)
plt.ylabel('Position')
plt.legend(['Measured Position', 'Calculated Position', 'Kalman Position', 'True Position'], loc = 2)
plt.subplot(2, 1, 2)
plt.plot(self.time[0: i + 1], self.error_measure_true[0: i + 1], 'b-', linewidth = 1)
plt.plot(self.time[0: i + 1], self.error_calculate_true[0: i + 1], 'g-', linewidth = 1)
plt.plot(self.time[0: i + 1], self.error_kalman_true[0: i + 1], 'r-', linewidth = 1)
plt.ylabel('Error')
plt.legend(['Measured Error', 'Calculated Error', 'Kalman Error'], loc = 2)
plt.xlabel('Time')
plt.pause(0.1)
plt.savefig("result.png")
print(self.sum(self.error_measure_true))
print(self.sum(self.error_calculate_true))
print(self.sum(self.error_kalman_true))
parser = argparse.ArgumentParser()
parser.add_argument("--sensors", nargs = '+', type = float, help = "velocity_deviation position_deviation")
parser.add_argument("--sim", nargs = '+', type = float, help = "total_time start_time expected_velocity")
args = parser.parse_args()
sim = Simulation(args.sim[0], 1.0)
sim.setup(args.sim[2], args.sim[1], args.sensors[0], args.sensors[1])
sim.simulate()