diff --git a/Localization/extended_kalman_filter/ekf_with_velocity_correction.py b/Localization/extended_kalman_filter/ekf_with_velocity_correction.py new file mode 100644 index 00000000..5dd97830 --- /dev/null +++ b/Localization/extended_kalman_filter/ekf_with_velocity_correction.py @@ -0,0 +1,198 @@ +""" + +Extended kalman filter (EKF) localization with velocity correction sample + +author: Atsushi Sakai (@Atsushi_twi) +modified by: Ryohei Sasaki (@rsasaki0109) + +""" +import sys +import pathlib + +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) + +import math +import matplotlib.pyplot as plt +import numpy as np + +from utils.plot import plot_covariance_ellipse + +# Covariance for EKF simulation +Q = np.diag([ + 0.1, # variance of location on x-axis + 0.1, # variance of location on y-axis + np.deg2rad(1.0), # variance of yaw angle + 0.4, # variance of velocity + 0.1 # variance of scale factor +]) ** 2 # predict state covariance +R = np.diag([0.1, 0.1]) ** 2 # Observation x,y position covariance + +# Simulation parameter +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] + +show_animation = True + + +def calc_input(): + v = 1.0 # [m/s] + yawrate = 0.1 # [rad/s] + u = np.array([[v], [yawrate]]) + return u + + +def observation(xTrue, xd, u): + xTrue = motion_model(xTrue, u) + + # add noise to gps x-y + z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1) + + # add noise to input + ud = u + INPUT_NOISE @ np.random.randn(2, 1) + + xd = motion_model(xd, ud) + + return xTrue, z, xd, ud + + +def motion_model(x, u): + 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], + [0.0, 0.0]]) + + x = F @ x + B @ u + + return x + + +def observation_model(x): + H = np.array([ + [1, 0, 0, 0, 0], + [0, 1, 0, 0, 0] + ]) + z = H @ x + + return z + + +def jacob_f(x, u): + """ + Jacobian of Motion Model + + motion model + 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} + s_{t+1} = s{t} + so + dx/dyaw = -v*s*dt*sin(yaw) + dx/dv = dt*s*cos(yaw) + dx/ds = dt*v*cos(yaw) + dy/dyaw = v*s*dt*cos(yaw) + dy/dv = dt*s*sin(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 * 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(): + jH = np.array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0]]) + return jH + + +def ekf_estimation(xEst, PEst, z, u): + # Predict + xPred = motion_model(xEst, u) + jF = jacob_f(xEst, u) + PPred = jF @ PEst @ jF.T + Q + + # Update + jH = jacob_h() + zPred = observation_model(xPred) + y = z - zPred + S = jH @ PPred @ jH.T + R + K = PPred @ jH.T @ np.linalg.inv(S) + xEst = xPred + K @ y + PEst = (np.eye(len(xEst)) - K @ jH) @ PPred + return xEst, PEst + + +def main(): + print(__file__ + " start!!") + + time = 0.0 + + # 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((5, 1)) # Dead reckoning + + # history + hxEst = xEst + hxTrue = xTrue + hxDR = xTrue + hz = np.zeros((2, 1)) + + while SIM_TIME >= time: + time += DT + u = calc_input() + + xTrue, z, xDR, ud = observation(xTrue, xDR, u) + + xEst, PEst = ekf_estimation(xEst, PEst, z, ud) + + # store data history + hxEst = np.hstack((hxEst, xEst)) + 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. + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.plot(hz[0, :], hz[1, :], ".g") + plt.plot(hxTrue[0, :].flatten(), + hxTrue[1, :].flatten(), "-b") + plt.plot(hxDR[0, :].flatten(), + 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) + plt.pause(0.001) + + +if __name__ == '__main__': + main() diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png b/docs/modules/localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png new file mode 100644 index 00000000..595b651b Binary files /dev/null and b/docs/modules/localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png differ diff --git a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst b/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst index 0ec920c9..da214b6d 100644 --- a/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst +++ b/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst @@ -2,6 +2,9 @@ Extended Kalman Filter Localization ----------------------------------- +Position Estimation Kalman Filter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + .. image:: extended_kalman_filter_localization_1_0.png :width: 600px @@ -9,6 +12,7 @@ Extended Kalman Filter Localization .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif + This is a sensor fusion localization with Extended Kalman Filter(EKF). The blue line is true trajectory, the black line is dead reckoning @@ -22,9 +26,26 @@ The red ellipse is estimated covariance ellipse with EKF. Code: `PythonRobotics/extended_kalman_filter.py at master · AtsushiSakai/PythonRobotics `__ +Kalman Filter with Speed Scale Factor Correction +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +.. image:: ekf_with_velocity_correction_1_0.png + :width: 600px + +This is a Extended kalman filter (EKF) localization with velocity correction. + +This is for correcting the vehicle speed measured with scale factor errors due to factors such as wheel wear. + +Code: `PythonRobotics/extended_kalman_ekf_with_velocity_correctionfilter.py +AtsushiSakai/PythonRobotics `__ + Filter design ~~~~~~~~~~~~~ +Position Estimation Kalman Filter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + In this simulation, the robot has a state vector includes 4 states at time :math:`t`. @@ -61,9 +82,28 @@ In the code, “observation” function generates the input and observation vector with noise `code `__ +Kalman Filter with Speed Scale Factor Correction +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +In this simulation, the robot has a state vector includes 5 states at +time :math:`t`. + +.. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t, s_t] + +x, y are a 2D x-y position, :math:`\phi` is orientation, v is +velocity, and s is a scale factor of velocity. + +In the code, “xEst” means the state vector. +`code `__ + +The rest is the same as the Position Estimation Kalman Filter. + Motion Model ~~~~~~~~~~~~ +Position Estimation Kalman Filter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + The robot model is .. math:: \dot{x} = v \cos(\phi) @@ -97,9 +137,50 @@ Its Jacobian matrix is :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v \sin(\phi) \Delta t & \cos(\phi) \Delta t\\ 0 & 1 & v \cos(\phi) \Delta t & \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{equation*}` + +Kalman Filter with Speed Scale Factor Correction +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The robot model is + +.. math:: \dot{x} = v s \cos(\phi) + +.. math:: \dot{y} = v s \sin(\phi) + +.. math:: \dot{\phi} = \omega + +So, the motion model is + +.. math:: \textbf{x}_{t+1} = f(\textbf{x}_t, \textbf{u}_t) = F\textbf{x}_t+B\textbf{u}_t + +where + +:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} B= \begin{bmatrix} cos(\phi) \Delta t s & 0\\ sin(\phi) \Delta t s & 0\\ 0 & \Delta t\\ 1 & 0\\ 0 & 0\\ \end{bmatrix} \end{equation*}` + +:math:`\Delta t` is a time interval. + +This is implemented at +`code `__ + +The motion function is that + +:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \\ w' \\ v' \end{bmatrix} = f(\textbf{x}, \textbf{u}) = \begin{bmatrix} x + v s \cos(\phi)\Delta t \\ y + v s \sin(\phi)\Delta t \\ \phi + \omega \Delta t \\ v \end{bmatrix} \end{equation*}` + +Its Jacobian matrix is + +:math:`\begin{equation*} J_f = \begin{bmatrix} \frac{\partial x'}{\partial x}& \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{\partial v} & \frac{\partial y'}{\partial s}\\ \frac{\partial \phi'}{\partial x}& \frac{\partial \phi'}{\partial y} & \frac{\partial \phi'}{\partial \phi} & \frac{\partial \phi'}{\partial v} & \frac{\partial \phi'}{\partial s}\\ \frac{\partial v'}{\partial x}& \frac{\partial v'}{\partial y} & \frac{\partial v'}{\partial \phi} & \frac{\partial v'}{\partial v} & \frac{\partial v'}{\partial s} \\ \frac{\partial s'}{\partial x}& \frac{\partial s'}{\partial y} & \frac{\partial s'}{\partial \phi} & \frac{\partial s'}{\partial v} & \frac{\partial s'}{\partial s} \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v s \sin(\phi) \Delta t & s \cos(\phi) \Delta t & \cos(\phi) v \Delta t\\ 0 & 1 & v s \cos(\phi) \Delta t & s \sin(\phi) \Delta t & v \sin(\phi) \Delta t\\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \end{bmatrix} \end{equation*}` + + Observation Model ~~~~~~~~~~~~~~~~~ +Position Estimation Kalman Filter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + The robot can get x-y position infomation from GPS. So GPS Observation model is @@ -120,6 +201,30 @@ Its Jacobian matrix is :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}` +Kalman Filter with Speed Scale Factor Correction +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The robot can get x-y position infomation from GPS. + +So GPS Observation model is + +.. math:: \textbf{z}_{t} = g(\textbf{x}_t) = H \textbf{x}_t + +where + +:math:`\begin{equation*} H = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}` + +The observation function states that + +:math:`\begin{equation*} \begin{bmatrix} x' \\ y' \end{bmatrix} = g(\textbf{x}) = \begin{bmatrix} x \\ y \end{bmatrix} \end{equation*}` + +Its Jacobian matrix is + +:math:`\begin{equation*} J_g = \begin{bmatrix} \frac{\partial x'}{\partial x} & \frac{\partial x'}{\partial y} & \frac{\partial x'}{\partial \phi} & \frac{\partial x'}{\partial v} & \frac{\partial x'}{\partial s}\\ \frac{\partial y'}{\partial x}& \frac{\partial y'}{\partial y} & \frac{\partial y'}{\partial \phi} & \frac{\partial y'}{ \partial v} & \frac{\partial y'}{ \partial s}\\ \end{bmatrix} \end{equation*}` + +:math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0\\ \end{bmatrix} \end{equation*}` + + Extended Kalman Filter ~~~~~~~~~~~~~~~~~~~~~~