AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-10 05:28:07 -05:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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 <asakai.amsl+github@gmail.com>
This commit is contained in:
Ryohei Sasaki
2024-01-22 22:02:22 +09:00
committed by GitHub
parent 4fcab6aa57
commit cfe1e30357
3 changed files with 303 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
Localization/extended_kalman_filter/ekf_with_velocity_correction.py Normal file
View File

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

BIN
docs/modules/localization/extended_kalman_filter_localization_files/ekf_with_velocity_correction_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 39 KiB

105
docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_main.rst
View File

@@ -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 <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
~~~~~~~~~~~~~~~~~~~~~~