github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

update docs

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-17 09:06:55 +09:00
parent 3d0d06ede6
commit 2adbb3be5d
4 changed files with 115 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
docs/modules/Kalmanfilter_basics_2.rst
View File

@@ -2,7 +2,8 @@
KF Basics - Part 2
------------------
### Probabilistic Generative Laws
Probabilistic Generative Laws
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1st Law:
^^^^^^^^

214
docs/modules/Model_predictive_speed_and_steering_control.rst
View File

@@ -94,61 +94,57 @@ ODE is
where
.. math::
:raw-latex:`\begin{equation*}
A' =
\begin{bmatrix}
\frac{\partial }{\partial x}vcos(\phi) &
\frac{\partial }{\partial y}vcos(\phi) &
\frac{\partial }{\partial v}vcos(\phi) &
\frac{\partial }{\partial \phi}vcos(\phi)\\
\frac{\partial }{\partial x}vsin(\phi) &
\frac{\partial }{\partial y}vsin(\phi) &
\frac{\partial }{\partial v}vsin(\phi) &
\frac{\partial }{\partial \phi}vsin(\phi)\\
\frac{\partial }{\partial x}a&
\frac{\partial }{\partial y}a&
\frac{\partial }{\partial v}a&
\frac{\partial }{\partial \phi}a\\
\frac{\partial }{\partial x}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial y}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial v}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\
\end{bmatrix}
\\
　=
\begin{bmatrix}
0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\
0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\
0 & 0 & 0 & 0 \\
0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\
\end{bmatrix}
\end{equation*}`
\begin{equation*}
A' =
\begin{bmatrix}
\frac{\partial }{\partial x}vcos(\phi) &
\frac{\partial }{\partial y}vcos(\phi) &
\frac{\partial }{\partial v}vcos(\phi) &
\frac{\partial }{\partial \phi}vcos(\phi)\\
\frac{\partial }{\partial x}vsin(\phi) &
\frac{\partial }{\partial y}vsin(\phi) &
\frac{\partial }{\partial v}vsin(\phi) &
\frac{\partial }{\partial \phi}vsin(\phi)\\
\frac{\partial }{\partial x}a&
\frac{\partial }{\partial y}a&
\frac{\partial }{\partial v}a&
\frac{\partial }{\partial \phi}a\\
\frac{\partial }{\partial x}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial y}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial v}\frac{vtan(\delta)}{L}&
\frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\
\end{bmatrix}
\\
　=
\begin{bmatrix}
0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\
0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\
0 & 0 & 0 & 0 \\
0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\
\end{bmatrix}
\end{equation*}
.. math::
\begin{equation*}
B' =
\begin{bmatrix}
\frac{\partial }{\partial a}vcos(\phi) &
\frac{\partial }{\partial \delta}vcos(\phi)\\
\frac{\partial }{\partial a}vsin(\phi) &
\frac{\partial }{\partial \delta}vsin(\phi)\\
\frac{\partial }{\partial a}a &
\frac{\partial }{\partial \delta}a\\
\frac{\partial }{\partial a}\frac{vtan(\delta)}{L} &
\frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\
\end{bmatrix}
\\
　=
\begin{bmatrix}
0 & 0 \\
0 & 0 \\
1 & 0 \\
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\
\end{bmatrix}
\end{equation*}
:raw-latex:`\begin{equation*}
B' =
\begin{bmatrix}
\frac{\partial }{\partial a}vcos(\phi) &
\frac{\partial }{\partial \delta}vcos(\phi)\\
\frac{\partial }{\partial a}vsin(\phi) &
\frac{\partial }{\partial \delta}vsin(\phi)\\
\frac{\partial }{\partial a}a &
\frac{\partial }{\partial \delta}a\\
\frac{\partial }{\partial a}\frac{vtan(\delta)}{L} &
\frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\
\end{bmatrix}
\\
　=
\begin{bmatrix}
0 & 0 \\
0 & 0 \\
1 & 0 \\
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\
\end{bmatrix}
\end{equation*}`
You can get a discrete-time mode with Forward Euler Discretization with
sampling time dt.
@@ -167,66 +163,60 @@ So,
where,
.. math::
:raw-latex:`\begin{equation*}
A = (I + dtA')\\
=
\begin{bmatrix}
1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\
0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\
0 & 0 & 1 & 0 \\
0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\
\end{bmatrix}
\end{equation*}`
\begin{equation*}
A = (I + dtA')\\
=
\begin{bmatrix}
1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\
0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\
0 & 0 & 1 & 0 \\
0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\
\end{bmatrix}
\end{equation*}
:raw-latex:`\begin{equation*}
B = dtB'\\
=
\begin{bmatrix}
0 & 0 \\
0 & 0 \\
dt & 0 \\
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\
\end{bmatrix}
\end{equation*}`
.. math::
\begin{equation*}
B = dtB'\\
=
\begin{bmatrix}
0 & 0 \\
0 & 0 \\
dt & 0 \\
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\
\end{bmatrix}
\end{equation*}
.. math::
\begin{equation*}
C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\
= dt(
\begin{bmatrix}
\bar{v}cos(\bar{\phi})\\
\bar{v}sin(\bar{\phi}) \\
\bar{a}\\
\frac{\bar{v}tan(\bar{\delta})}{L}\\
\end{bmatrix}
-
\begin{bmatrix}
\bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\
\bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\
0\\
\frac{\bar{v}tan(\bar{\delta})}{L}\\
\end{bmatrix}
-
\begin{bmatrix}
0\\
0 \\
\bar{a}\\
\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\
\end{bmatrix}
)\\
=
\begin{bmatrix}
\bar{v}sin(\bar{\phi})\bar{\phi}dt\\
-\bar{v}cos(\bar{\phi})\bar{\phi}dt\\
0\\
-\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\
\end{bmatrix}
\end{equation*}
:raw-latex:`\begin{equation*}
C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\
= dt(
\begin{bmatrix}
\bar{v}cos(\bar{\phi})\\
\bar{v}sin(\bar{\phi}) \\
\bar{a}\\
\frac{\bar{v}tan(\bar{\delta})}{L}\\
\end{bmatrix}
-
\begin{bmatrix}
\bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\
\bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\
0\\
\frac{\bar{v}tan(\bar{\delta})}{L}\\
\end{bmatrix}
-
\begin{bmatrix}
0\\
0 \\
\bar{a}\\
\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\
\end{bmatrix}
)\\
=
\begin{bmatrix}
\bar{v}sin(\bar{\phi})\bar{\phi}dt\\
-\bar{v}cos(\bar{\phi})\bar{\phi}dt\\
0\\
-\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\
\end{bmatrix}
\end{equation*}`
This equation is implemented at