Skip to content

Commit 2adbb3b

Browse files
AtsushiSakaiAtsushiSakai
committed
update docs
1 parent 3d0d06e commit 2adbb3b

File tree

4 files changed

+118
-127
lines changed

4 files changed

+118
-127
lines changed

‎Localization/Kalmanfilter_basics_2.ipynb‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
"cell_type": "markdown",
1212
"metadata": {},
1313
"source": [
14-
" ### Probabilistic Generative Laws\n",
14+
"### Probabilistic Generative Laws\n",
1515
" \n",
1616
"#### 1st Law:\n",
1717
"The belief representing the state $x_{t}$, is conditioned on all past states, measurements and controls. This can be shown mathematically by the conditional probability shown below:\n",

‎PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@
128128
"source": [
129129
"where\n",
130130
"\n",
131-
"$$\\begin{equation*}\n",
131+
"\\begin{equation*}\n",
132132
"A' =\n",
133133
"\\begin{bmatrix}\n",
134134
"\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
@@ -156,15 +156,15 @@
156156
"0 & 0 & 0 & 0 \\\\\n",
157157
"0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
158158
"\\end{bmatrix}\n",
159-
"\\end{equation*}$$\n",
159+
"\\end{equation*}\n",
160160
"\n"
161161
]
162162
},
163163
{
164164
"cell_type": "markdown",
165165
"metadata": {},
166166
"source": [
167-
"$$\\begin{equation*}\n",
167+
"\\begin{equation*}\n",
168168
"B' =\n",
169169
"\\begin{bmatrix}\n",
170170
"\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
@@ -184,7 +184,7 @@
184184
"1 & 0 \\\\\n",
185185
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
186186
"\\end{bmatrix}\n",
187-
"\\end{equation*}$$\n",
187+
"\\end{equation*}\n",
188188
"\n"
189189
]
190190
},
@@ -212,7 +212,7 @@
212212
"\n",
213213
"where,\n",
214214
"\n",
215-
"$$\\begin{equation*}\n",
215+
"\\begin{equation*}\n",
216216
"A = (I + dtA')\\\\\n",
217217
"=\n",
218218
"\\begin{bmatrix} \n",
@@ -221,14 +221,14 @@
221221
"0 & 0 & 1 & 0 \\\\\n",
222222
"0 & 0 &\\frac{tan(\\bar{\\delta})}{L}dt & 1 \\\\\n",
223223
"\\end{bmatrix}\n",
224-
"\\end{equation*}$$"
224+
"\\end{equation*}"
225225
]
226226
},
227227
{
228228
"cell_type": "markdown",
229229
"metadata": {},
230230
"source": [
231-
"$$\\begin{equation*}\n",
231+
"\\begin{equation*}\n",
232232
"B = dtB'\\\\\n",
233233
"=\n",
234234
"\\begin{bmatrix} \n",
@@ -237,14 +237,14 @@
237237
"dt & 0 \\\\\n",
238238
"0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
239239
"\\end{bmatrix}\n",
240-
"\\end{equation*}$$"
240+
"\\end{equation*}"
241241
]
242242
},
243243
{
244244
"cell_type": "markdown",
245245
"metadata": {},
246246
"source": [
247-
"$$\\begin{equation*}\n",
247+
"\\begin{equation*}\n",
248248
"C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
249249
"= dt(\n",
250250
"\\begin{bmatrix} \n",
@@ -275,7 +275,7 @@
275275
"0\\\\\n",
276276
"-\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
277277
"\\end{bmatrix}\n",
278-
"\\end{equation*}$$"
278+
"\\end{equation*}"
279279
]
280280
},
281281
{

‎docs/modules/Kalmanfilter_basics_2.rst‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@
22
KF Basics - Part 2
33
------------------
44

5-
### Probabilistic Generative Laws
5+
Probabilistic Generative Laws
6+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
67

78
1st Law:
89
^^^^^^^^

‎docs/modules/Model_predictive_speed_and_steering_control.rst‎

Lines changed: 105 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -94,61 +94,57 @@ ODE is
9494

9595
where
9696

97-
.. math::
98-
99-
\begin{equation*}
100-
A' =
101-
\begin{bmatrix}
102-
\frac{\partial }{\partial x}vcos(\phi) &
103-
\frac{\partial }{\partial y}vcos(\phi) &
104-
\frac{\partial }{\partial v}vcos(\phi) &
105-
\frac{\partial }{\partial \phi}vcos(\phi)\\
106-
\frac{\partial }{\partial x}vsin(\phi) &
107-
\frac{\partial }{\partial y}vsin(\phi) &
108-
\frac{\partial }{\partial v}vsin(\phi) &
109-
\frac{\partial }{\partial \phi}vsin(\phi)\\
110-
\frac{\partial }{\partial x}a&
111-
\frac{\partial }{\partial y}a&
112-
\frac{\partial }{\partial v}a&
113-
\frac{\partial }{\partial \phi}a\\
114-
\frac{\partial }{\partial x}\frac{vtan(\delta)}{L}&
115-
\frac{\partial }{\partial y}\frac{vtan(\delta)}{L}&
116-
\frac{\partial }{\partial v}\frac{vtan(\delta)}{L}&
117-
\frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\
118-
\end{bmatrix}
119-
\\
120-
　=
121-
\begin{bmatrix}
122-
0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\
123-
0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\
124-
0 & 0 & 0 & 0 \\
125-
0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\
126-
\end{bmatrix}
127-
\end{equation*}
128-
129-
.. math::
130-
131-
\begin{equation*}
132-
B' =
133-
\begin{bmatrix}
134-
\frac{\partial }{\partial a}vcos(\phi) &
135-
\frac{\partial }{\partial \delta}vcos(\phi)\\
136-
\frac{\partial }{\partial a}vsin(\phi) &
137-
\frac{\partial }{\partial \delta}vsin(\phi)\\
138-
\frac{\partial }{\partial a}a &
139-
\frac{\partial }{\partial \delta}a\\
140-
\frac{\partial }{\partial a}\frac{vtan(\delta)}{L} &
141-
\frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\
142-
\end{bmatrix}
143-
\\
144-
　=
145-
\begin{bmatrix}
146-
0 & 0 \\
147-
0 & 0 \\
148-
1 & 0 \\
149-
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\
150-
\end{bmatrix}
151-
\end{equation*}
97+
:raw-latex:`\begin{equation*}
98+
A' =
99+
\begin{bmatrix}
100+
\frac{\partial }{\partial x}vcos(\phi) &
101+
\frac{\partial }{\partial y}vcos(\phi) &
102+
\frac{\partial }{\partial v}vcos(\phi) &
103+
\frac{\partial }{\partial \phi}vcos(\phi)\\
104+
\frac{\partial }{\partial x}vsin(\phi) &
105+
\frac{\partial }{\partial y}vsin(\phi) &
106+
\frac{\partial }{\partial v}vsin(\phi) &
107+
\frac{\partial }{\partial \phi}vsin(\phi)\\
108+
\frac{\partial }{\partial x}a&
109+
\frac{\partial }{\partial y}a&
110+
\frac{\partial }{\partial v}a&
111+
\frac{\partial }{\partial \phi}a\\
112+
\frac{\partial }{\partial x}\frac{vtan(\delta)}{L}&
113+
\frac{\partial }{\partial y}\frac{vtan(\delta)}{L}&
114+
\frac{\partial }{\partial v}\frac{vtan(\delta)}{L}&
115+
\frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\
116+
\end{bmatrix}
117+
\\
118+
　=
119+
\begin{bmatrix}
120+
0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\
121+
0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\
122+
0 & 0 & 0 & 0 \\
123+
0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\
124+
\end{bmatrix}
125+
\end{equation*}`
126+
127+
:raw-latex:`\begin{equation*}
128+
B' =
129+
\begin{bmatrix}
130+
\frac{\partial }{\partial a}vcos(\phi) &
131+
\frac{\partial }{\partial \delta}vcos(\phi)\\
132+
\frac{\partial }{\partial a}vsin(\phi) &
133+
\frac{\partial }{\partial \delta}vsin(\phi)\\
134+
\frac{\partial }{\partial a}a &
135+
\frac{\partial }{\partial \delta}a\\
136+
\frac{\partial }{\partial a}\frac{vtan(\delta)}{L} &
137+
\frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\
138+
\end{bmatrix}
139+
\\
140+
　=
141+
\begin{bmatrix}
142+
0 & 0 \\
143+
0 & 0 \\
144+
1 & 0 \\
145+
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\
146+
\end{bmatrix}
147+
\end{equation*}`
152148

153149
You can get a discrete-time mode with Forward Euler Discretization with
154150
sampling time dt.
@@ -167,66 +163,60 @@ So,
167163

168164
where,
169165

170-
.. math::
171-
172-
\begin{equation*}
173-
A = (I + dtA')\\
174-
=
175-
\begin{bmatrix}
176-
1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\
177-
0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\
178-
0 & 0 & 1 & 0 \\
179-
0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\
180-
\end{bmatrix}
181-
\end{equation*}
182-
183-
.. math::
184-
185-
\begin{equation*}
186-
B = dtB'\\
187-
=
188-
\begin{bmatrix}
189-
0 & 0 \\
190-
0 & 0 \\
191-
dt & 0 \\
192-
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\
193-
\end{bmatrix}
194-
\end{equation*}
195-
196-
.. math::
197-
198-
\begin{equation*}
199-
C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\
200-
= dt(
201-
\begin{bmatrix}
202-
\bar{v}cos(\bar{\phi})\\
203-
\bar{v}sin(\bar{\phi}) \\
204-
\bar{a}\\
205-
\frac{\bar{v}tan(\bar{\delta})}{L}\\
206-
\end{bmatrix}
207-
-
208-
\begin{bmatrix}
209-
\bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\
210-
\bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\
211-
0\\
212-
\frac{\bar{v}tan(\bar{\delta})}{L}\\
213-
\end{bmatrix}
214-
-
215-
\begin{bmatrix}
216-
0\\
217-
0 \\
218-
\bar{a}\\
219-
\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\
220-
\end{bmatrix}
221-
)\\
222-
=
223-
\begin{bmatrix}
224-
\bar{v}sin(\bar{\phi})\bar{\phi}dt\\
225-
-\bar{v}cos(\bar{\phi})\bar{\phi}dt\\
226-
0\\
227-
-\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\
228-
\end{bmatrix}
229-
\end{equation*}
166+
:raw-latex:`\begin{equation*}
167+
A = (I + dtA')\\
168+
=
169+
\begin{bmatrix}
170+
1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\
171+
0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\
172+
0 & 0 & 1 & 0 \\
173+
0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\
174+
\end{bmatrix}
175+
\end{equation*}`
176+
177+
:raw-latex:`\begin{equation*}
178+
B = dtB'\\
179+
=
180+
\begin{bmatrix}
181+
0 & 0 \\
182+
0 & 0 \\
183+
dt & 0 \\
184+
0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\
185+
\end{bmatrix}
186+
\end{equation*}`
187+
188+
:raw-latex:`\begin{equation*}
189+
C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\
190+
= dt(
191+
\begin{bmatrix}
192+
\bar{v}cos(\bar{\phi})\\
193+
\bar{v}sin(\bar{\phi}) \\
194+
\bar{a}\\
195+
\frac{\bar{v}tan(\bar{\delta})}{L}\\
196+
\end{bmatrix}
197+
-
198+
\begin{bmatrix}
199+
\bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\
200+
\bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\
201+
0\\
202+
\frac{\bar{v}tan(\bar{\delta})}{L}\\
203+
\end{bmatrix}
204+
-
205+
\begin{bmatrix}
206+
0\\
207+
0 \\
208+
\bar{a}\\
209+
\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\
210+
\end{bmatrix}
211+
)\\
212+
=
213+
\begin{bmatrix}
214+
\bar{v}sin(\bar{\phi})\bar{\phi}dt\\
215+
-\bar{v}cos(\bar{\phi})\bar{\phi}dt\\
216+
0\\
217+
-\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\
218+
\end{bmatrix}
219+
\end{equation*}`
230220

231221
This equation is implemented at
232222

0 commit comments

Comments
 (0)