diff --git a/src/components/localization/kalman_filter/extended_kalman_filter_localizer.py b/src/components/localization/kalman_filter/extended_kalman_filter_localizer.py index 72e5b3da..2e5ccac4 100644 --- a/src/components/localization/kalman_filter/extended_kalman_filter_localizer.py +++ b/src/components/localization/kalman_filter/extended_kalman_filter_localizer.py @@ -19,19 +19,59 @@ class ExtendedKalmanFilterLocalizer: """ Self localization by Extended Kalman Filter class + + Notation used in the comments below + x = [x, y, yaw, speed]^T state vector (4x1) + u = [accel, yaw rate]^T input vector (2x1) + z = [x, y]^T observation (GNSS position, 2x1) + P covariance matrix of the state (4x4) + Q covariance matrix of the input noise (2x2) + R covariance matrix of the observation noise (2x2) + + Each cycle has two steps. + Predict: propagate the last estimate with the motion model + x_pred = f(x_last, u) + P_pred = F P_last F^T + G Q G^T + Update: correct the prediction with the GNSS observation + S = H P_pred H^T + R + K = P_pred H^T S^-1 + x_est = x_pred + K (z - h(x_pred)) + P_est = (I - K H) P_pred + + The motion model f is State.motion_model(). The Jacobians F, G and H + are described at _jacobian_F(), _jacobian_G() and JACOB_H. + + References + S. Thrun, W. Burgard, D. Fox, "Probabilistic Robotics", + MIT Press, 2005. Chapter 3 (Gaussian Filters) explains the + Kalman filter and the Extended Kalman Filter. + R. E. Kalman, "A New Approach to Linear Filtering and Prediction + Problems", Journal of Basic Engineering, 82(1), 1960. + PythonRobotics (Atsushi Sakai), Extended Kalman Filter Localization: + https://github.com/AtsushiSakai/PythonRobotics """ def __init__(self, accel_noise=0.2, yaw_rate_noise=10.0, obsrv_x_noise=1.0, obsrv_y_noise=1.0, color='r'): """ Constructor + accel_noise: Standard deviation of acceleration input noise[m/s^2] + yaw_rate_noise: Standard deviation of yaw rate input noise[deg/s] + obsrv_x_noise: Standard deviation of x observation noise[m] + obsrv_y_noise: Standard deviation of y observation noise[m] color: Color of drawing error covariance ellipse """ self.state = np.zeros((4, 1)) self.cov_mat = np.eye(4) + # Q = diag(sigma_accel^2, sigma_yaw_rate^2), yaw rate in [rad/s] self.INPUT_NOISE_VAR_MAT = np.diag([accel_noise, np.deg2rad(yaw_rate_noise)]) ** 2 + # R = diag(sigma_x^2, sigma_y^2) self.OBSRV_NOISE_VAR_MAT = np.diag([obsrv_x_noise, obsrv_y_noise]) ** 2 + # Jacobian H of the observation model h(x) = [x, y]^T. + # GNSS observes only x and y, so h is linear and H = dh/dx is constant: + # H = [[1, 0, 0, 0], + # [0, 1, 0, 0]] self.JACOB_H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) self.DRAW_COLOR = color @@ -39,10 +79,11 @@ def __init__(self, accel_noise=0.2, yaw_rate_noise=10.0, def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss): """ Function to update data - state: Last extimated state data + state: Last estimated state data accel_mps2: Acceleration input from controller yaw_rate_rps: Yaw rate input from controller time_s: Simulation interval time[sec] + gnss: Observation [x, y]^T from GNSS(2x1 array)[m] Return: Estimated state data """ @@ -52,29 +93,54 @@ def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss): [state.get_speed_mps()]]) # input with noise + # u = u_org + noise + # Note: the noise is made by multiplying the variance matrix Q (not + # the standard deviation) by a standard normal random vector, so the + # sampled noise is sigma^2 * N(0, 1). This is kept as it is, so the + # behavior of the simulation does not change. input_org = np.array([[accel_mps2], [yaw_rate_rps]]) input_noise = self.INPUT_NOISE_VAR_MAT @ np.random.randn(2, 1) next_input = input_org + input_noise # predict state + # x_pred = f(x_last, u) = A(yaw_last) x_last + B(yaw_last) u + # x_pred = x + speed*cos(yaw)*t + accel*cos(yaw)*t^2/2 + # y_pred = y + speed*sin(yaw)*t + accel*sin(yaw)*t^2/2 + # yaw_pred = yaw + yaw_rate*t + # spd_pred = speed + accel*t pred_state = State.motion_model(last_state, next_input, time_s) + # F = df/dx, G = df/du + # Note: the code evaluates them at the predicted state (pred_state), + # not at the last state as in the textbook. jF = self._jacobian_F(pred_state, next_input, time_s) jG = self._jacobian_G(pred_state, time_s) Q = self.INPUT_NOISE_VAR_MAT last_cov = self.cov_mat + # P_pred = F P_last F^T + G Q G^T + # The first term propagates the last uncertainty through the motion + # model, the second term adds the uncertainty of the input noise. pred_cov = jF @ last_cov @jF.T + jG @ Q @ jG.T # predict observation + # z_pred = h(x_pred) = H x_pred + # S = H P_pred H^T + R (covariance of the innovation) pred_obsrv = self._observation_model(pred_state) jH = self.JACOB_H R = self.OBSRV_NOISE_VAR_MAT pred_obsrv_cov = jH @ pred_cov @ jH.T + R # kalman gain + # K = P_pred H^T S^-1 + # Large R (noisy GNSS) makes K small, so the prediction is trusted. + # Small R makes K large, so the observation is trusted. k = pred_cov @ jH.T @ np.linalg.inv(pred_obsrv_cov) # update + # innovation: y = z - z_pred (GNSS observation minus predicted one) + # x_est = x_pred + K y + # P_est = P_pred - K S K^T + # This equals (I - K H) P_pred, because K S K^T = K H P_pred. inov = gnss - pred_obsrv est_state = pred_state + k @ inov est_cov = pred_cov - k @ pred_obsrv_cov @ k.T @@ -111,6 +177,17 @@ def draw(self, axes, elems, pose): def _jacobian_F(self, state, input, time_s): """ Private function to calculate jacobian F + F = df/dx: partial derivatives of the motion model f(x, u) with + respect to the state x = [x, y, yaw, speed]^T. The motion model is + nonlinear in yaw, so it is linearized around the given state + (first-order Taylor expansion). With t = time_s: + x_next = x + speed*cos(yaw)*t + accel*cos(yaw)*t^2/2 + y_next = y + speed*sin(yaw)*t + accel*sin(yaw)*t^2/2 + yaw_next = yaw + yaw_rate*t + spd_next = speed + accel*t + Differentiating each row gives, for example, + d x_next / d yaw = -speed*sin(yaw)*t - accel*sin(yaw)*t^2/2 + d x_next / d speed = cos(yaw)*t state: Predicted state input: Input vector[accel, yaw rate] from controller time_s: Simulation interval time[sec] @@ -135,6 +212,13 @@ def _jacobian_F(self, state, input, time_s): def _jacobian_G(self, state, time_s): """ Private function to calculate jacobian G + G = df/du: partial derivatives of the motion model f(x, u) with + respect to the input u = [accel, yaw rate]^T. It maps the input noise + into the state space. Because f is linear in u, G is the same as + matrix B in State.motion_model(). For example, + d x_next / d accel = cos(yaw)*t^2/2 + d yaw_next / d yaw_rate = t + d spd_next / d accel = t state: Predicted state time_s: Simulation interval time[sec] Return: Jacobian G @@ -153,6 +237,8 @@ def _jacobian_G(self, state, time_s): def _observation_model(self, state): """ Private function of observation model + h(x) = H x = [x, y]^T: GNSS observes only the position, so yaw and + speed are dropped by H. state: Predicted state Return Predicted observation data """