Addition of velocity scale factor estimation to EKF (AtsushiSakai#937)

* Addition of velocity scale factor estimation to EKF

* Format

* Add a scale factor motion model in the Jacobian function description

* Fix Jacobian function description

* New script: 'ekf_with_velocity_correction.py'

* Add doc

* Fix doc

* Correct the parts where the first letter of the sentence is lowercase

* Fix doc title

* Fix script title

* Do grouping

* Fix wrong motion function in ekf doc

* Update docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst

---------

Co-authored-by: Atsushi Sakai <[email protected]>

Author: rsasaki0109

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()

docs/modules/localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png

@@ -2,13 +2,17 @@
 Extended Kalman Filter Localization
 -----------------------------------
 
+Position Estimation Kalman Filter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
 .. image:: extended_kalman_filter_localization_1_0.png
    :width: 600px
 
 
 
 .. 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 <https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py>`__
 
+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 <https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_ekf_with_velocity_correctionfilter.py>`__
+
 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 <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50>`__
 
+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 <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_ekf_with_velocity_correctionfilter.py#L163>`__
+
+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 <https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L61-L76>`__
+
+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
 ~~~~~~~~~~~~~~~~~~~~~~