Skip to content

Commit 510d27d

Browse files
botprofbotprof
committed
Added simple EKF with GPS example
1 parent 0055f6e commit 510d27d

File tree

3 files changed

+473
-3
lines changed

3 files changed

+473
-3
lines changed

‎README.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,12 @@ Filename | Description
6363
[control_approx_linearization.py](control_approx_linearization.py) | Trajectory tracking for a differential drive vehicle using control by approximate linearization.
6464
[dynamic_extension_tracking.py](dynamic_extension_tracking.py) | Trajectory tracking for a differential drive vehicle using feedback linearization with dynamic extension.
6565
[MPC_linear_tracking.py](MPC_linear_tracking.py) | Trajectory tracking for a 1D dynamic vehicle using unconstrained model predictive control (MPC).
66+
67+
### Vehicle Navigation Examples
68+
69+
Filename | Description
70+
-------- | -----------
71+
[diffdrive_GPS_EKF.py](diffdrive_GPS_EKF.py) | Simple EKF implementation for a differential drive vehicle with wheel encoders, an angular rate gyro, and GPS.
6672

6773
## Cite this Work
6874

‎diffdrive_GPS_EKF.py‎

Lines changed: 366 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,366 @@
1+
"""
2+
Example diffdrive_GPS_EKF.py
3+
Author: Joshua A. Marshall <[email protected]>
4+
GitHub: https://github.com/botprof/agv-examples
5+
"""
6+
7+
# %%
8+
# SIMULATION SETUP
9+
10+
import numpy as np
11+
import matplotlib.pyplot as plt
12+
from mobotpy.models import DiffDrive
13+
from mobotpy.integration import rk_four
14+
from scipy import signal
15+
from scipy.stats import chi2
16+
from numpy.linalg import matrix_power
17+
from numpy.linalg import inv
18+
19+
# Set the simulation time [s] and the sample period [s]
20+
SIM_TIME = 30.0
21+
T = 0.01
22+
23+
# Create an array of time values [s]
24+
t = np.arange(0.0, SIM_TIME, T)
25+
N = np.size(t)
26+
27+
# %%
28+
# VEHICLE SETUP
29+
30+
# Set the track length of the vehicle [m]
31+
ELL = 1.0
32+
33+
# Create a vehicle object of type DiffDrive
34+
vehicle = DiffDrive(ELL)
35+
36+
# %%
37+
# FUNCTION DEFINITIONS
38+
39+
40+
def unicycle_gps_ekf(u_m, y, Q, R, x, P, T):
41+
42+
# Define some matrices for the a priori step
43+
G = np.array([[np.cos(x[2]), 0], [np.sin(x[2]), 0], [0, 1]])
44+
F = np.identity(3) + T * np.array(
45+
[[0, 0, -u_m[0] * np.sin(x[2])], [0, 0, u_m[0] * np.cos(x[2])], [0, 0, 0]]
46+
)
47+
L = T * G
48+
49+
# Compute a priori estimate
50+
x_new = x + T * G @ u_m
51+
P_new = F @ P @ np.transpose(F) + L @ Q @ np.transpose(L)
52+
53+
# Numerically help the covariance matrix stay symmetric
54+
P_new = (P_new + np.transpose(P_new)) / 2
55+
56+
# Define some matrices for the a posteriori step
57+
C = np.array([[1, 0, 0], [0, 1, 0]])
58+
H = C
59+
M = np.identity(2)
60+
61+
# Compute the a posteriori estimate
62+
K = (
63+
P_new
64+
@ np.transpose(H)
65+
@ np.linalg.inv(H @ P_new @ np.transpose(H) + M @ R @ np.transpose(M))
66+
)
67+
x_new = x_new + K @ (y - C @ x_new)
68+
P_new = (np.identity(3) - K @ H) @ P_new
69+
70+
# Numerically help the covariance matrix stay symmetric
71+
P_new = (P_new + np.transpose(P_new)) / 2
72+
73+
# Define the function output
74+
return x_new, P_new
75+
76+
77+
# %%
78+
# SET UP INITIAL CONDITIONS AND ESTIMATOR PARAMETERS
79+
80+
# Initial conditions
81+
x_init = np.zeros(3)
82+
x_init[0] = 0.0
83+
x_init[1] = 0.0
84+
x_init[2] = 0.0
85+
86+
# Setup some arrays for the actual vehicle
87+
x = np.zeros((3, N))
88+
u = np.zeros((2, N))
89+
x[:, 0] = x_init
90+
91+
# Set the initial guess for the estimator
92+
x_guess = x_init + np.array([5.0, -5.0, 0.1])
93+
94+
# Set the initial pose covariance estimate as a diagonal matrix
95+
P_guess = np.diag(np.square([5.0, -5.0, 0.1]))
96+
97+
# Set the true process and measurement noise covariances
98+
Q = np.diag(
99+
[
100+
1.0 / (2.0 * np.power(T, 2)) * np.power(0.2 * np.pi / ((2 ** 12) * 3), 2),
101+
np.power(0.001, 2),
102+
]
103+
)
104+
R = np.power(5.0, 2) * np.identity(2)
105+
106+
# Initialized estimator arrays
107+
x_hat = np.zeros((3, N))
108+
x_hat[:, 0] = x_guess
109+
110+
# Measured odometry (speed and angular rate) and GPS (x, y) signals
111+
u_m = np.zeros((2, N))
112+
y = np.zeros((2, N))
113+
114+
# Covariance of the estimate
115+
P_hat = np.zeros((3, 3, N))
116+
P_hat[:, :, 0] = P_guess
117+
118+
# %%
119+
# SIMULATE AND PLOT WITHOUT GPS
120+
121+
# Set the process and measurement noise covariances to ignore GPS
122+
Q_hat = Q
123+
R_hat = 1e10 * R
124+
125+
for k in range(1, N):
126+
127+
# Compute some inputs to steer the unicycle around
128+
u_unicycle = np.array([2.0, np.sin(0.1 * T * k)])
129+
130+
# Simulate the differential drive vehicle's motion
131+
u[:, k] = vehicle.uni2diff(u_unicycle)
132+
x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
133+
134+
# Simulate the vehicle speed estimate
135+
u_m[0, k] = u_unicycle[0] + np.power(Q[0, 0], 0.5) * np.random.randn(1)
136+
137+
# Simulate the angular rate gyroscope measurement
138+
u_m[1, k] = u_unicycle[1] + np.power(Q[1, 1], 0.5) * np.random.randn(1)
139+
140+
# Simulate the GPS measurement
141+
y[0, k] = x[0, k] + np.power(R[0, 0], 0.5) * np.random.randn(1)
142+
y[1, k] = x[1, k] + np.power(R[1, 1], 0.5) * np.random.randn(1)
143+
144+
# Run the EKF estimator
145+
x_hat[:, k], P_hat[:, :, k] = unicycle_gps_ekf(
146+
u_m[:, k], y[:, k], Q_hat, R_hat, x_hat[:, k - 1], P_hat[:, :, k - 1], T
147+
)
148+
149+
# Change some plot settings (optional)
150+
plt.rc("text", usetex=True)
151+
plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}")
152+
plt.rc("savefig", format="pdf")
153+
plt.rc("savefig", bbox="tight")
154+
155+
# Plot results without GPS
156+
fig1 = plt.figure(1)
157+
ax1 = plt.subplot(311)
158+
plt.setp(ax1, xticklabels=[])
159+
plt.plot(t, x[0, :], "C0", label="Actual")
160+
plt.plot(t, x_hat[0, :], "C1--", label="Estimated")
161+
plt.ylabel(r"$x_1$ [m]")
162+
plt.grid(color="0.95")
163+
plt.legend()
164+
ax2 = plt.subplot(312)
165+
plt.setp(ax2, xticklabels=[])
166+
plt.plot(t, x[1, :], "C0")
167+
plt.plot(t, x_hat[1, :], "C1--")
168+
plt.ylabel(r"$x_2$ [m]")
169+
plt.grid(color="0.95")
170+
ax3 = plt.subplot(313)
171+
plt.plot(t, x[2, :], "C0")
172+
plt.plot(t, x_hat[2, :], "C1--")
173+
plt.xlabel(r"$t$ [s]")
174+
plt.ylabel(r"$x_3$ [rad]")
175+
plt.grid(color="0.95")
176+
177+
# Save the plot
178+
plt.savefig("../agv-book/figs/ch5/diffdrive_GPS_EKF_fig1.pdf")
179+
180+
# Find the scaling factor for plotting 99% covariance bounds
181+
alpha = 0.01
182+
s1 = chi2.isf(alpha, 1)
183+
s2 = chi2.isf(alpha, 2)
184+
185+
# Plot the estimation error with covariance bounds
186+
sigma = np.zeros((3, N))
187+
fig2 = plt.figure(2)
188+
ax1 = plt.subplot(311)
189+
sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :])
190+
plt.fill_between(
191+
t,
192+
-sigma[0, :],
193+
sigma[0, :],
194+
color="C1",
195+
alpha=0.2,
196+
label=str(100 * (1 - alpha)) + " \% Confidence",
197+
)
198+
plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Error")
199+
plt.ylabel(r"$e_1$ [m]")
200+
plt.setp(ax1, xticklabels=[])
201+
plt.grid(color="0.95")
202+
plt.legend()
203+
ax2 = plt.subplot(312)
204+
sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :])
205+
plt.fill_between(
206+
t,
207+
-sigma[1, :],
208+
sigma[1, :],
209+
color="C1",
210+
alpha=0.2,
211+
label=str(100 * (1 - alpha)) + " \% Confidence",
212+
)
213+
plt.plot(t, x[1, :] - x_hat[1, :], "C0", label="Error")
214+
plt.ylabel(r"$e_2$ [m]")
215+
plt.setp(ax2, xticklabels=[])
216+
plt.grid(color="0.95")
217+
ax3 = plt.subplot(313)
218+
sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :])
219+
plt.fill_between(
220+
t,
221+
-sigma[2, :],
222+
sigma[2, :],
223+
color="C1",
224+
alpha=0.2,
225+
label=str(100 * (1 - alpha)) + " \% Confidence",
226+
)
227+
plt.plot(t, x[2, :] - x_hat[2, :], "C0", label="Error")
228+
plt.ylabel(r"$e_3$ [rad]")
229+
plt.xlabel(r"$t$ [s]")
230+
plt.grid(color="0.95")
231+
232+
# Save the plot
233+
plt.savefig("../agv-book/figs/ch5/diffdrive_GPS_EKF_fig2.pdf")
234+
235+
# Show the plots to the screen
236+
plt.show()
237+
238+
# %%
239+
# PLOT THE NOISY GPS DATA
240+
241+
fig3 = plt.figure(3)
242+
ax1 = plt.subplot(211)
243+
plt.plot(t, y[0, :], "C1", label="GPS")
244+
plt.plot(t, x[0, :], "C0", label="Actual")
245+
plt.ylabel(r"$x_1$ [m]")
246+
plt.grid(color="0.95")
247+
plt.setp(ax1, xticklabels=[])
248+
plt.legend()
249+
ax2 = plt.subplot(212)
250+
plt.plot(t, y[1, :], "C1", label="GPS")
251+
plt.plot(t, x[1, :], "C0", label="Actual")
252+
plt.ylabel(r"$x_2$ [m]")
253+
plt.xlabel(r"$t$ [s]")
254+
plt.grid(color="0.95")
255+
256+
# Save the plot
257+
plt.savefig("../agv-book/figs/ch5/diffdrive_GPS_EKF_fig3.pdf")
258+
259+
# Show the plots to the screen
260+
plt.show()
261+
262+
# %%
263+
# SIMULATE AND PLOT WITH GPS + ODOMETRY FUSION
264+
265+
# Find the scaling factor for plotting covariance bounds
266+
alpha = 0.01
267+
s1 = chi2.isf(alpha, 1)
268+
s2 = chi2.isf(alpha, 2)
269+
270+
# Estimate the process and measurement noise covariances
271+
Q_hat = Q
272+
R_hat = R
273+
274+
for k in range(1, N):
275+
276+
# Compute some inputs to steer the unicycle around
277+
u_unicycle = np.array([2.0, np.sin(0.1 * T * k)])
278+
279+
# Simulate the differential drive vehicle's motion
280+
u[:, k] = vehicle.uni2diff(u_unicycle)
281+
x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
282+
283+
# Simulate the vehicle speed estimate
284+
u_m[0, k] = u_unicycle[0] + np.power(Q[0, 0], 0.5) * np.random.randn(1)
285+
286+
# Simulate the angular rate gyroscope measurement
287+
u_m[1, k] = u_unicycle[1] + np.power(Q[1, 1], 0.5) * np.random.randn(1)
288+
289+
# Simulate the GPS measurement
290+
y[0, k] = x[0, k] + np.power(R[0, 0], 0.5) * np.random.randn(1)
291+
y[1, k] = x[1, k] + np.power(R[1, 1], 0.5) * np.random.randn(1)
292+
293+
# Run the EKF estimator
294+
x_hat[:, k], P_hat[:, :, k] = unicycle_gps_ekf(
295+
u_m[:, k], y[:, k], Q_hat, R_hat, x_hat[:, k - 1], P_hat[:, :, k - 1], T
296+
)
297+
298+
# Set the ranges for error uncertainty axes
299+
x_range = 2.0
300+
y_range = 2.0
301+
theta_range = 0.2
302+
303+
# Plot the estimation error with covariance bounds
304+
sigma = np.zeros((3, N))
305+
fig4 = plt.figure(4)
306+
ax1 = plt.subplot(311)
307+
sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :])
308+
plt.fill_between(
309+
t,
310+
-sigma[0, :],
311+
sigma[0, :],
312+
color="C1",
313+
alpha=0.2,
314+
label=str(100 * (1 - alpha)) + " \% Confidence",
315+
)
316+
plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Error")
317+
plt.ylabel(r"$e_1$ [m]")
318+
plt.setp(ax1, xticklabels=[])
319+
ax1.set_ylim([-x_range, x_range])
320+
plt.grid(color="0.95")
321+
plt.legend()
322+
ax2 = plt.subplot(312)
323+
sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :])
324+
plt.fill_between(t, -sigma[1, :], sigma[1, :], color="C1", alpha=0.2)
325+
plt.plot(t, x[1, :] - x_hat[1, :], "C0")
326+
plt.ylabel(r"$e_2$ [m]")
327+
plt.setp(ax2, xticklabels=[])
328+
ax2.set_ylim([-y_range, y_range])
329+
plt.grid(color="0.95")
330+
ax3 = plt.subplot(313)
331+
sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :])
332+
plt.fill_between(
333+
t,
334+
-sigma[2, :],
335+
sigma[2, :],
336+
color="C1",
337+
alpha=0.2,
338+
label=str(100 * (1 - alpha)) + " \% Confidence",
339+
)
340+
plt.plot(t, x[2, :] - x_hat[2, :], "C0", label="Error")
341+
ax3.set_ylim([-theta_range, theta_range])
342+
plt.ylabel(r"$e_3$ [rad]")
343+
plt.xlabel(r"$t$ [s]")
344+
plt.grid(color="0.95")
345+
346+
# Save the plot
347+
plt.savefig("../agv-book/figs/ch5/diffdrive_GPS_EKF_fig4.pdf")
348+
349+
# Show the plots to the screen
350+
plt.show()
351+
352+
# %%
353+
# MAKE AN ANIMATION
354+
355+
# Create and save the animation
356+
ani = vehicle.animate_estimation(
357+
x, x_hat, P_hat, alpha, T, True, "../agv-book/gifs/ch5/diffdrive_GPS_EKF.gif"
358+
)
359+
360+
# Show the movie to the screen
361+
plt.show()
362+
363+
# # Show animation in HTML output if you are using IPython or Jupyter notebooks
364+
# plt.rc('animation', html='jshtml')
365+
# display(ani)
366+
# plt.close()

0 commit comments

Comments
 (0)