From 4fc119e0745a7094e22ce3b3ffed3c2d0452fbd4 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sun, 19 Nov 2023 16:55:38 +0900 Subject: [PATCH] Addition of velocity scale factor estimation to EKF --- .../extended_kalman_filter.py | 76 ++++++++++--------- 1 file changed, 40 insertions(+), 36 deletions(-) diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index d9ece6c6f3..a39ac808b3 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -21,13 +21,14 @@ 0.1, # variance of location on x-axis 0.1, # variance of location on y-axis np.deg2rad(1.0), # variance of yaw angle - 1.0 # variance of velocity + 0.4, # variance of velocity + 0.1 # variance of scale factor ]) ** 2 # predict state covariance -R = np.diag([1.0, 1.0]) ** 2 # Observation x,y position covariance +R = np.diag([0.1, 0.1]) ** 2 # Observation x,y position covariance # Simulation parameter -INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2 -GPS_NOISE = np.diag([0.5, 0.5]) ** 2 +INPUT_NOISE = np.diag([0.1, np.deg2rad(5.0)]) ** 2 +GPS_NOISE = np.diag([0.05, 0.05]) ** 2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] @@ -57,15 +58,17 @@ def observation(xTrue, xd, u): def motion_model(x, u): - F = np.array([[1.0, 0, 0, 0], - [0, 1.0, 0, 0], - [0, 0, 1.0, 0], - [0, 0, 0, 0]]) - - B = np.array([[DT * math.cos(x[2, 0]), 0], - [DT * math.sin(x[2, 0]), 0], + F = np.array([[1.0, 0, 0, 0, 0], + [0, 1.0, 0, 0, 0], + [0, 0, 1.0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 1.0]]) + + B = np.array([[DT * math.cos(x[2, 0]) * x[4, 0], 0], + [DT * math.sin(x[2, 0]) * x[4, 0], 0], [0.0, DT], - [1.0, 0.0]]) + [1.0, 0.0], + [0.0, 0.0]]) x = F @ x + B @ u @@ -73,11 +76,8 @@ def motion_model(x, u): def observation_model(x): - H = np.array([ - [1, 0, 0, 0], - [0, 1, 0, 0] - ]) - + H = np.array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0]]) z = H @ x return z @@ -88,8 +88,8 @@ def jacob_f(x, u): Jacobian of Motion Model motion model - x_{t+1} = x_t+v*dt*cos(yaw) - y_{t+1} = y_t+v*dt*sin(yaw) + x_{t+1} = x_t+v*s*dt*cos(yaw) + y_{t+1} = y_t+v*s*dt*sin(yaw) yaw_{t+1} = yaw_t+omega*dt v_{t+1} = v{t} so @@ -97,25 +97,24 @@ def jacob_f(x, u): dx/dv = dt*cos(yaw) dy/dyaw = v*dt*cos(yaw) dy/dv = dt*sin(yaw) + dx/ds = dt*v*cos(yaw) + dy/ds = dt*v*sin(yaw) """ yaw = x[2, 0] v = u[0, 0] + s = x[4, 0] jF = np.array([ - [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)], - [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0]]) - + [1.0, 0.0, -DT * v * s * math.sin(yaw), DT * s * math.cos(yaw), DT * v * math.cos(yaw)], + [0.0, 1.0, DT * v * s * math.cos(yaw), DT * s * math.sin(yaw), DT * v * math.sin(yaw)], + [0.0, 0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 1.0]]) return jF def jacob_h(): - # Jacobian of Observation Model - jH = np.array([ - [1, 0, 0, 0], - [0, 1, 0, 0] - ]) - + jH = np.array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0]]) return jH @@ -141,12 +140,15 @@ def main(): time = 0.0 - # State Vector [x y yaw v]' - xEst = np.zeros((4, 1)) - xTrue = np.zeros((4, 1)) - PEst = np.eye(4) + # State Vector [x y yaw v s]' + xEst = np.zeros((5, 1)) + xEst[4, 0] = 1.0 # Initial scale factor + xTrue = np.zeros((5, 1)) + true_scale_factor = 0.9 # True scale factor + xTrue[4, 0] = true_scale_factor + PEst = np.eye(5) - xDR = np.zeros((4, 1)) # Dead reckoning + xDR = np.zeros((5, 1)) # Dead reckoning # history hxEst = xEst @@ -167,7 +169,7 @@ def main(): hxDR = np.hstack((hxDR, xDR)) hxTrue = np.hstack((hxTrue, xTrue)) hz = np.hstack((hz, z)) - + estimated_scale_factor = hxEst[4, -1] if show_animation: plt.cla() # for stopping simulation with the esc key. @@ -180,6 +182,8 @@ def main(): hxDR[1, :].flatten(), "-k") plt.plot(hxEst[0, :].flatten(), hxEst[1, :].flatten(), "-r") + plt.text(0.45, 0.85, f"True Velocity Scale Factor: {true_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes) + plt.text(0.45, 0.95, f"Estimated Velocity Scale Factor: {estimated_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes) plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst) plt.axis("equal") plt.grid(True)