UKF vs EKF for a Practical Robot Localization Problem
import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import block_diag
#State Transition Model (Bicycle Model): This model represents the vehicle’s motion. It includes parameters like velocity, steering angle, and wheelbase to predict the next state of the vehicle.
def state_transition(x, u, dt, L):
theta = x[2]
v = u[0]
alpha = u[1]
x_next = np.zeros_like(x)
x_next[0] = x[0] + v * np.cos(theta) * dt
x_next[1] = x[1] + v * np.sin(theta) * dt
x_next[2] = x[2] + (v / L) * np.tan(alpha) * dt
return x_next
#Measurement Model: This model relates the observed measurements (range and bearing to landmarks) to the vehicle’s state. Given the non-linear nature of these measurements, both EKF and UKF need to handle this non-linearity effectively.
def measurement_model(x, landmark):
dx = landmark[0] - x[0]
dy = landmark[1] - x[1]
r = np.sqrt(dx**2 + dy**2)
phi = np.arctan2(dy, dx) - x[2]
return np.array([r, phi])
#Extended Kalman Filter (EKF)-Linearize the state transition and measurement models around the current estimate,Use the Jacobian matrices to propagate the mean and covariance
def ekf_predict(x, P, u, Q, dt, L):
F = np.array([[1, 0, -u[0] * np.sin(x[2]) * dt],
[0, 1, u[0] * np.cos(x[2]) * dt],
[0, 0, 1]])
x = state_transition(x, u, dt, L)
P = F @ P @ F.T + Q
return x, P
def ekf_update(x, P, z, R, landmark):
H = np.array([[-(landmark[0] - x[0]) / np.sqrt((landmark[0] - x[0])**2 + (landmark[1] - x[1])**2),
-(landmark[1] - x[1]) / np.sqrt((landmark[0] - x[0])**2 + (landmark[1] - x[1])**2), 0],
[(landmark[1] - x[1]) / ((landmark[0] - x[0])**2 + (landmark[1] - x[1])**2),
-(landmark[0] - x[0]) / ((landmark[0] - x[0])**2 + (landmark[1] - x[1])**2), -1]])
z_pred = measurement_model(x, landmark)
y = z - z_pred
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.inv(S)
x = x + K @ y
P = (np.eye(len(x)) - K @ H) @ P
return x, P
#Unscented Kalman Filter (UKF):Use a set of sigma points to capture the mean and covariance. Propagate these sigma points through the non-linear models to better capture the true distribution.
def unscented_transform(sigma_points, Wm, Wc, noise_cov):
mean = np.dot(Wm, sigma_points)
cov = np.zeros((mean.shape[0], mean.shape[0]))
for i in range(sigma_points.shape[0]):
diff = sigma_points[i] - mean
cov += Wc[i] * np.outer(diff, diff)
cov += noise_cov
return mean, cov
def generate_sigma_points(x, P, kappa):
n = len(x)
sigma_points = np.zeros((2 * n + 1, n))
sigma_points[0] = x
sqrt_P = np.linalg.cholesky((n + kappa) * P)
for i in range(n):
sigma_points[i + 1] = x + sqrt_P[i]
sigma_points[n + i + 1] = x - sqrt_P[i]
return sigma_points
def ukf_predict(x, P, u, Q, dt, L, kappa):
n = len(x)
sigma_points = generate_sigma_points(x, P, kappa)
Wm = np.full(2 * n + 1, 1 / (2 * (n + kappa)))
Wm[0] = kappa / (n + kappa)
Wc = Wm.copy()
Wc[0] += 1 - 1 / (2 * (n + kappa))
sigma_points_pred = np.array([state_transition(sp, u, dt, L) for sp in sigma_points])
x_pred, P_pred = unscented_transform(sigma_points_pred, Wm, Wc, Q)
return x_pred, P_pred
def ukf_update(x, P, z, R, landmark, kappa):
n = len(x)
sigma_points = generate_sigma_points(x, P, kappa)
Wm = np.full(2 * n + 1, 1 / (2 * (n + kappa)))
Wm[0] = kappa / (n + kappa)
Wc = Wm.copy()
Wc[0] += 1 - 1 / (2 * (n + kappa))
sigma_points_meas = np.array([measurement_model(sp, landmark) for sp in sigma_points])
z_pred, Pz = unscented_transform(sigma_points_meas, Wm, Wc, R)
Pxz = np.zeros((n, z_pred.shape[0]))
for i in range(2 * n + 1):
Pxz += Wc[i] * np.outer(sigma_points[i] - x, sigma_points_meas[i] - z_pred)
K = Pxz @ np.linalg.inv(Pz)
x = x + K @ (z - z_pred)
P = P - K @ Pz @ K.T
return x, P
# Simulation parameters
dt = 0.1
L = 2.0
x_true = np.array([0, 0, 0])
P_true = np.eye(3)
Q = np.diag([0.1, 0.1, np.deg2rad(1)])**2
R = np.diag([0.5, np.deg2rad(5)])**2
landmark = np.array([10, 10])
u = np.array([1, np.deg2rad(10)])
# Initial estimates
x_ekf = x_true.copy()
P_ekf = P_true.copy()
x_ukf = x_true.copy()
P_ukf = P_true.copy()
kappa = 3 - len(x_true)
# Storage for plotting
x_ekf_hist = []
x_ukf_hist = []
x_true_hist = []
# Run the simulation
for _ in range(100):
# True state update
x_true = state_transition(x_true, u, dt, L)
x_true_hist.append(x_true.copy())
# Simulate measurement
z = measurement_model(x_true, landmark) + np.random.multivariate_normal([0, 0], R)
# EKF update
x_ekf, P_ekf = ekf_predict(x_ekf, P_ekf, u, Q, dt, L)
x_ekf, P_ekf = ekf_update(x_ekf, P_ekf, z, R, landmark)
x_ekf_hist.append(x_ekf.copy())
# UKF update
x_ukf, P_ukf = ukf_predict(x_ukf, P_ukf, u, Q, dt, L, kappa)
x_ukf, P_ukf = ukf_update(x_ukf, P_ukf, z, R, landmark, kappa)
x_ukf_hist.append(x_ukf.copy())
# Convert histories to arrays for plotting
x_ekf_hist = np.array(x_ekf_hist)
x_ukf_hist = np.array(x_ukf_hist)
x_true_hist = np.array(x_true_hist)
# Plot the results:
#Set up the simulation parameters, initializes the state and covariance matrices, and runs the simulation for 100 iterations, updating the state using both EKF and UKF
plt.figure()
plt.plot(x_true_hist[:, 0], x_true_hist[:, 1], label='True Path')
plt.plot(x_ekf_hist[:, 0], x_ekf_hist[:, 1], label='EKF Path')
plt.plot(x_ukf_hist[:, 0], x_ukf_hist[:, 1], label='UKF Path')
plt.scatter(landmark[0], landmark[1], c='r', marker='x', label='Landmark')
plt.legend()
plt.xlabel('X position')
plt.ylabel('Y position')
plt.title('EKF vs UKF for Bicycle Model')
plt.show()
-------------- End of python code ----------------------------------
Reason Why UKF is better than EKF:
1. Accuracy in Nonlinear Systems
UKF Path (Orange Line): The UKF path closely follows the True Path (Blue Line), indicating that the UKF is accurately estimating the bicycle’s trajectory.
EKF Path (Green Line): The EKF path deviates more from the True Path, showing less accuracy in tracking the bicycle’s movement.
2. Handling Nonlinearity
UKF: Uses a set of sigma points to better capture the mean and covariance of the state distribution through nonlinear transformations. This allows the UKF to handle the nonlinearities in the bicycle model more effectively.
EKF: Linearizes the nonlinear models around the current estimate, which can lead to inaccuracies if the system’s nonlinearity is significant.
3. Landmark Association
The UKF’s estimates are closer to the landmarks (Red Crosses), suggesting better association and correction based on sensor measurements.
4. Overall Performance
UKF: Provides a more accurate and reliable estimate of the state, especially in highly nonlinear systems like the bicycle model.
EKF: While simpler and faster in some cases, it may not perform as well in systems with significant nonlinearity due to its reliance on linear approximations.