-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrobot.py
290 lines (230 loc) · 12 KB
/
robot.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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from IPython.display import HTML
np.random.seed(1) ## for repeatability
class Robot:
def __init__(self, start_pos, start_orientation, motor_noise_std, gps_noise_std, imu_noise_std):
# Defining state
self.true_x = np.array([[start_pos[0, 0]], [start_pos[1, 0]], [np.radians(float(start_orientation))]])
self.orientation_rad = np.radians(float(start_orientation))
self.position = np.array(start_pos, dtype=float) # Robot's position array [x, y].T
# Kalman filter necessities
self.A = np.eye(3)
self.x_m = np.array([[start_pos[0, 0]], [start_pos[1, 0]], [np.radians(float(start_orientation))]])
self.P_m = np.eye(3) # initial covariance matrix TODO: find some sense in initial covariance matrix
# Process noise covariance matrix (Q)
self.Q = np.diag([motor_noise_std**2, motor_noise_std**2, motor_noise_std**2])
# Measurement noise covariance matrix (R)
self.R = np.diag([gps_noise_std**2, gps_noise_std**2, imu_noise_std**2])
self.u_x = float(0)
self.u_theta = float(0)
# Measurement matrix (H)
self.H = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
# Control input model
self.B = np.eye(3)
self.motor_noise_std = motor_noise_std # Standard deviation for motion noise
self.gps_noise_std = gps_noise_std
self.imu_noise_std = imu_noise_std
self.z = self.true_x
# Rotation PID parameters
self.Kp_rot = 0.9
self.Ki_rot = 0.01
self.Kd_rot = 0.000
self.previous_error_rot = 0.0
self.integral_rot = 0.0
self.previous_orientation = self.x_m[2, 0]
self.integral_rot_max = 1.0
self.integral_rot_min = - 1.0
self.dt = 0.1
# pos PID parameters
self.Kp_pos = 0.5
self.Ki_pos = 0.0
self.Kd_pos = 0.005
self.previous_error_pos = 0.0
self.integral_pos = 0.0
def calculate_distance_to_target(self, target_pos):
return np.linalg.norm(target_pos - self.x_m)
def calculate_estimated_angle_to_target(self, target_pos):
"""
Calculates the angle from the current position to the target position.
"""
delta_x = target_pos[0, 0] - self.x_m[0, 0]
delta_y = target_pos[1, 0] - self.x_m[1, 0]
angle_rad = np.arctan2(delta_y, delta_x) # Angle in radians
return angle_rad
# PREDICTION USING KALMAN FILTER
def predict(self):
#self.move_step_pid_u(target_pos) TODO: Integrate the u directly in the moving function
self.x_p = self.x_m + self.B @ np.array([[self.u_x*np.cos(self.x_m[2,0])],
[self.u_x*np.sin(self.x_m[2,0])],
[self.u_theta]])
self.P_p = self.A @ self.P_m @ (self.A).T + self.Q
# UPDATING KNOWING NEW INFORMATION
def update(self):
S = self.H @ self.P_p @ (self.H).T + self.R
K = self.P_p @ (self.H).T @ np.linalg.inv(S)
self.x_m = self.x_p + K @ (self.z - self.H @ self.x_p)
self.P_m = self.P_p - self.P_p @ (self.H).T @ np.linalg.inv(S) @ self.H @ self.P_p
def rotate_towards(self, target_pos):
current_error = self.calculate_estimated_angle_to_target(target_pos) - self.x_m[2,0]
self.integral_rot += current_error
derivative = current_error - self.previous_error_rot
rotation_amount = self.Kp_rot * current_error + self.Ki_rot * self.integral_rot + self.Kd_rot * derivative
self.x_m[2, 0] += rotation_amount
self.x_m[2, 0] = self.x_m[2, 0] % 2*np.pi
self.previous_error_rot = current_error
def rot_step_pid_u(self, target_pos):
#print(self.calculate_estimated_angle_to_target(target_pos))
current_error = self.calculate_estimated_angle_to_target(target_pos) - self.x_m[2,0]
#print("Target_pos is", target_pos, "angle error is", current_error*180/3.14)
#print('current_error is ', current_error)
self.integral_rot += current_error
self.integral_rot = max(min(self.integral_rot, self.integral_rot_max), self.integral_rot_min)
# Calculate derivative based on measurement change
current_orientation = self.x_m[2,0]
derivative = (current_orientation - self.previous_orientation) / self.dt
self.previous_orientation = current_orientation
self.u_theta = self.Kp_rot * current_error + self.Ki_rot * self.integral_rot - self.Kd_rot * derivative
#print(self.u_theta)
def move_step_pid_u(self, target_pos):
# Calculate the Euclidean distance to the target as the error
current_error_pos = np.sqrt((self.x_m[0,0] - target_pos[0,0])**2 + (self.x_m[1,0] - target_pos[1,0])**2)
# Update the integral of the error
self.integral_pos += current_error_pos
# Calculate the derivative of the error
derivative_pos = current_error_pos - self.previous_error_pos
# PID control for step size based on distance to target
step_size = self.Kp_pos * current_error_pos + self.Ki_pos * self.integral_pos + self.Kd_pos * derivative_pos
# Speed adjustment
# You may need to adjust the max_speed and min_speed based on your robot's capabilities and testing
max_speed = 20.0 # Maximum speed limit
min_speed = 0.05 # Minimum speed to prevent too slow movements
# Ensure the step size is within the min and max speed limits
step_size = max(min(step_size, max_speed), min_speed)
self.u_x = step_size
self.previous_error_pos = current_error_pos
def move_towards(self, target_pos):
"""
Moves the robot towards the target position.
"""
# PID control for step size based on distance to target
current_error_pos = target_pos[0, 0] + target_pos[1, 0] - (self.position[0] + self.position[1])
self.position += self.u_x * np.array([np.cos(self.orientation_rad), np.sin(self.orientation_rad)])
self.previous_error_pos = current_error_pos
def process_noise(self):
return np.array([[np.random.normal(0, np.sqrt(self.Q[0,0]))],
[np.random.normal(0, np.sqrt(self.Q[1,1]))],
[np.random.normal(0, np.sqrt(self.Q[2,2]))]])
def gps_measure(self):
"""
Gives measured information on [x, y] position of the robot with measurement inaccuracy
"""
#print(self.z)
#print(self.z[:2, 0])
#print(self.true_x[:2, 0])
self.z[0:2] = self.true_x[0:2] + np.array([[np.random.normal(0,self.gps_noise_std)],
[np.random.normal(0,self.gps_noise_std)]])
def imu_sensor(self):
"""
Simulates the sensing of the orientation with added noise.
"""
self.z[2, 0] = self.true_x[2,0] + np.random.normal(0, self.imu_noise_std)
def step_run(self, target_pos) -> np.ndarray:
if np.sqrt((self.x_m[0,0] - target_pos[0,0])**2 + (self.x_m[1,0] - target_pos[1,0])**2) < 0.5:
print(f"Reached target or very close to it !")
else:
# Update control inputs using PID
self.rot_step_pid_u(target_pos)
self.move_step_pid_u(target_pos)
# Apply process noise and move the robot
process_noise = self.process_noise()
self.true_x = self.A @ self.true_x + process_noise + self.B @ np.array([[self.u_x*np.cos(self.x_m[2,0])],
[self.u_x*np.sin(self.x_m[2,0])],
[self.u_theta]])
# Simulate sensor measurements
self.gps_measure()
self.imu_sensor()
# Perform Kalman filter prediction and update
self.predict()
self.update()
def run(self, target_pos, steps=100, plot = False):
estimated_positions = [self.x_m[:2, 0].copy()] # Kalman filter estimates
true_positions = [self.true_x[:2, 0].copy()] # True positions
gps_positions = [self.x_m[:2, 0].copy()] # Unfiltered estimates (for comparison)
for k in range(steps):
# Update control inputs using PID
self.rot_step_pid_u(target_pos)
self.move_step_pid_u(target_pos)
# Apply process noise and move the robot
process_noise = self.process_noise()
self.true_x = self.A @ self.true_x + process_noise + self.B @ np.array([[self.u_x*np.cos(self.x_m[2,0])],
[self.u_x*np.sin(self.x_m[2,0])],
[self.u_theta]])
# Simulate sensor measurements
self.gps_measure()
self.imu_sensor()
# Perform Kalman filter prediction and update
self.predict()
self.update()
# Store the positions
estimated_positions.append(self.x_m[:2, 0].copy())
true_positions.append(self.true_x[:2, 0].copy())
gps_positions.append(self.z[:2, 0].copy()) # Assuming 'self.position' is the unfiltered estimate
# Check for completion
#print("Delta x", abs(self.x_m[0,0] - target_pos[0,0]))
#print("Delta y", abs(self.x_m[1,0] - target_pos[1,0]))
if np.sqrt((self.x_m[0,0] - target_pos[0,0])**2 + (self.x_m[1,0] - target_pos[1,0])**2) < 0.5:
print(f"Reached target or very close to it in: {k} steps!")
break
if plot:
#print(gps_positions)
# Visualization
plt.figure(figsize=(12, 6))
plt.plot(np.array(true_positions)[:, 0], np.array(true_positions)[:, 1], 'k-', label='True Position')
plt.plot(np.array(estimated_positions)[:, 0], np.array(estimated_positions)[:, 1], 'b--', label='Kalman Filter Estimate')
plt.plot(np.array(gps_positions)[:, 0], np.array(gps_positions)[:, 1], 'r:', label='Unfiltered Estimate')
plt.plot(target_pos[0], target_pos[1], 'go', label='Target')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.title('Robot Navigation with Kalman Filter')
plt.legend()
plt.grid(True)
plt.show()
return estimated_positions, true_positions, gps_positions
# Example usage
if __name__ == '__main__':
start_pos = np.array([[8.5],
[3.9]])
robot = Robot(start_pos=start_pos, start_orientation=0, motor_noise_std=0.00020,
gps_noise_std=0.00008, imu_noise_std= 0.000005)
target_pos = np.array([[17],
[87]])
steps = 200
plotting = True
positions, __, __ = robot.run(target_pos=target_pos, steps=steps, plot = plotting)
# Set up the figure for animation again
# Setup the figure and axis for the plot and sliders
fig, ax = plt.subplots()
# Setup plot limits and initial plot elements
ax.set_xlim((-1, 100))
ax.set_ylim((-1, 100))
line, = ax.plot([], [], 'o-', lw=2)
target, = ax.plot(target_pos[0, 0], target_pos[1, 0], 'rx', markersize=10)
# Initialize animation
def init():
line.set_data([], [])
target.set_data([target_pos[0]], [target_pos[1]])
return (line, target)
# Define animation function
def animate(i):
xdata = [pos[0] for pos in positions[:i+1]]
ydata = [pos[1] for pos in positions[:i+1]]
line.set_data(xdata, ydata)
return (line,)
# Animate
anim = animation.FuncAnimation(fig, animate, init_func=init,
frames=len(positions), interval=200, blit=False)
plt.show()