mirror of
https://github.com/janishutz/eth-summaries.git
synced 2026-09-10 19:15:25 +02:00
[AMR] updated first 4.5 sections
This commit is contained in:
Binary file not shown.
@@ -5,7 +5,7 @@
|
||||
|
||||
\shorttheorem[Bayes] $\displaystyle \P(Y_i | X) = \frac{\P(X | Y_i) \P(Y_i)}{\sum_{j = 1}^n \P(X | Y_j) \P(Y_j)}$
|
||||
|
||||
\shortdefinition[Cont. Var] Sums become integrals\\
|
||||
\shortdefinition[Cont. Var] Sums become integrals (and vice-versa)\\
|
||||
e.g. $\sum_{X} \P(X) = 1$ becomes $\int \P(x) \dx = 1$
|
||||
|
||||
\shortdefinition[Indep.] $x, y$ indep. iff $\P(\cX \cap \cY) = \P(\cX) \P(\cY)$
|
||||
|
||||
@@ -5,6 +5,8 @@
|
||||
\shortdefinition[Orthogonal vec] $v^\top w = 0$;
|
||||
\shortdefinition[Vec Line Eq] $\vec{p} + \lambda \vec{d}$
|
||||
|
||||
\shortdefinition[Determinant] $ad - bc$ for mat $[a, b; c, d]$
|
||||
\shortdefinition[Determinant] $ad - bc$ for mat $[a, b; c, d]$.
|
||||
|
||||
\shortdefinition[Eigenvalues] Solve $\det(M - \lambda I) = 0$
|
||||
\shortdefinition[Eigenvalues] Solve $\det(M - \lambda I) = 0$.
|
||||
|
||||
\shortremark[Matrix Inverse] For diagonal, inverse of diag els
|
||||
|
||||
@@ -41,17 +41,17 @@ $\begin{smallmatrix}
|
||||
\varphi = \arcsin\left( R_{31} \div \sqrt{1 - R_{31}^2} \right)\\
|
||||
\end{smallmatrix}$
|
||||
$[\vec{n}]^\times = \begin{bmatrix}
|
||||
0 & -a_3 & a_2 \\
|
||||
a_3 & 0 & -a_1 \\
|
||||
-a_2 & a_1 & 0
|
||||
0 & -n_3 & n_2 \\
|
||||
n_3 & 0 & -n_1 \\
|
||||
-n_2 & n_1 & 0
|
||||
\end{bmatrix}$
|
||||
|
||||
For pitch axis $= \pm 90\deg$, Gimbal Lock, Jacobian singular.
|
||||
For pitch axis $\theta = \pm 90\deg$, \bi{Gimbal Lock}, Jacobian \bi{singular}.
|
||||
|
||||
\shortdefinition[Angle-Axis]
|
||||
$\vec{\alpha} = \alpha \vec{n}$ ($\vec{n}$ normal); Convert to rot. mat\\
|
||||
$\vec{\alpha} = \alpha \vec{n}$ ($\vec{n}$ normal); To \bi{rotation matrix}:\\
|
||||
$\mat{R}(\alpha, \vec{n}) = \mat{I}_3 + \sin(\alpha)[\vec{n}]^\times + (1 - \cos(\alpha))([\vec{n}]^\times)^2$\\
|
||||
To quat: $\vec{q} = [\vec{n}, \alpha]$
|
||||
To \bi{quaternion}: $\vec{q} = [\vec{n}, \alpha]$
|
||||
|
||||
|
||||
\shortdefinition[Quaternions] $q = q_w + q_x i + q_y j + q_z k$ with\\
|
||||
@@ -68,14 +68,14 @@ $\vec{q} = \begin{bmatrix}
|
||||
\end{bmatrix}$
|
||||
}
|
||||
|
||||
\bi{To Rot Mat} {\scriptsize
|
||||
\bi{To Rot Mat} {
|
||||
$R_{AB} = \mat{I}_3 + 2a(\vec{q}_{AB}) [\vec{v}(\vec{q}_{AB})]^\times + 2([\vec{v}(\vec{q}_{AB})]^\times)^2$
|
||||
}
|
||||
|
||||
|
||||
\shortdefinition[Transf. M]
|
||||
{\scriptsize
|
||||
$\mat{T}_{AC} = \mat{T}_{AB} \mat{T}_{BC}$ R.-Handed typ; Aero: Left-H\\
|
||||
$\mat{T}_{AC} = \mat{T}_{AB} \mat{T}_{BC}$ Right-Handed typically; Aero: Left-H\\
|
||||
$\mat{T}_{AB} = \begin{bmatrix}
|
||||
\mat{R}_{AB} & {_A}\vec{t}_B \\
|
||||
\mat{0}_{1\times 3} & 1
|
||||
|
||||
@@ -11,7 +11,8 @@ $ \begin{bmatrix}
|
||||
Workspace $W$: $\theta_1, \theta_2 \in [-\pi, \pi]$.
|
||||
\bi{Computation} Similar for $n$R sys (more angles, more lengths).
|
||||
Last dim is typically sum of angles (or equiv).
|
||||
\bi{For velocity kinematics}: Comp. Jacobian of this $nD$ position ($nD$ pos is vec). ${_W}\vec{t}_{WE} \mat{J}(\vec{\theta}) \cdot \vec{\dot{\theta}}$
|
||||
\bi{Jacobian}: see \ref{sec:ms-lin}.
|
||||
\bi{For velocity kinematics}: See \ref{sec:manipulator-velocity-kinematics}.
|
||||
% Comp. Jacobian of this $nD$ position ($nD$ pos is vec). ${_W}\vec{t}_{WE} \mat{J}(\vec{\theta}) \cdot \vec{\dot{\theta}}$
|
||||
% \bi{Jacobian}: see \ref{sec:ms-lin}.
|
||||
|
||||
\shortdefinition[Singularity] Loss of deg of Freedom $\det(\mat{J}(\vec{\theta})) = 0$
|
||||
|
||||
@@ -13,3 +13,13 @@ $\theta_1 = \varphi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$
|
||||
3. Solve for wrist joints (no infl. on pos)
|
||||
|
||||
\bi{For Inv. velocity}: $\vec{\dot{\theta}} = J_S^{-1}(\vec{\theta}) {_W} \vec{t}_{WE}$
|
||||
|
||||
% \shortdefinition[Denavit-Hartengerb (DH)] Serial chains
|
||||
% \[
|
||||
% \mat{T}_{L_j L_{j + 1}} = \mat{T}^{R_z}(\theta_j) \mat{T}^{t_z}(d_j) \mat{T}^{t_x}(a_j) \mat{T}^{R_x}(\alpha_j)
|
||||
% \]
|
||||
% with $
|
||||
|
||||
\shortdefinition[Underactuated] workspace $<6$D; IK solvable
|
||||
|
||||
\shortdefinition[Overactuated] $\8$ IK sol; Reduced singularities; Num. sol.
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \dot{\alpha} \cdot {_I}\vec{n}$
|
||||
|
||||
\bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$
|
||||
\bi{Vel. point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$
|
||||
|
||||
\bi{Rotation Matrices}
|
||||
\begin{itemize}
|
||||
@@ -14,8 +14,8 @@
|
||||
$\mat{\dot{R}}_{IB} = [{_I} \omega_{IB}]^\times \mat{R}_{IB}$
|
||||
\item For right pertubing
|
||||
$\mat{\dot{R}}_{IB} = \mat{R}_{IB} [{_I} \omega_{IB}]^\times$
|
||||
\item Constant angular velocity ($\exp{[\Delta \vec{\alpha}]^\times} = \delta \mat{R}(\Delta \vec{\alpha})$)\\
|
||||
$\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \vec{\alpha}]^\times} \mat{R}_{IB}(t) \quad \vec{\alpha} = {_I}\vec{\omega}_{IB} \delta t$
|
||||
\item Constant angular velocity ($\exp{[\Delta \vec{\alpha}]^\times} = \delta \mat{R}(\Delta \vec{\alpha}) \quad \vec{\alpha} = {_I}\vec{\omega}_{IB} \delta t$)\\
|
||||
$\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \vec{\alpha}]^\times} \mat{R}_{IB}(t)$
|
||||
\end{itemize}
|
||||
|
||||
\bi{Quaternions}
|
||||
@@ -56,4 +56,4 @@ where {\color{gray} gray parts} only IRL (in theor. models, leave out), with $\v
|
||||
|
||||
\bi{IMU Sensor Model}: $\vec{\tilde{z}} = \vec{b}_C + s\mat{M}\vec{z} + \vec{b} + \vec{n} + \vec{o}$
|
||||
where bias $\vec{b}$ and scale $s$ often modelled time-varying $\dot{b}(t) = \sigma_C n(t)$.
|
||||
$\vec{b}_C$ const. calib; $\mat{M}$ Misalignment; $\vec{n}$ noise; $\vec{o}$ other infl.
|
||||
$\vec{b}_C$ const. calibration / bias; $\mat{M}$ Misalignment; $\vec{n}$ noise; $\vec{o}$ other infl.
|
||||
|
||||
@@ -2,14 +2,12 @@
|
||||
\label{sec:rigid-body-dynamics}
|
||||
\shortdefinition[Newton II] For fin. body w/ mass $m$ and inertia mat. $I$, with force $\vec{F}$ and torque $\vec{T}$ on \bi{Centre of Mass} (CoM), in body frame:
|
||||
\begin{align*}
|
||||
{_B}\vec{F} & = \sum {_B}\vec{F}_i = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\
|
||||
{_B}\vec{T} & = \sum {_B}\vec{T}_i = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega}
|
||||
{_B}\vec{F} & = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\
|
||||
{_B}\vec{T} & = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega}
|
||||
\end{align*}
|
||||
|
||||
${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. inert. frame
|
||||
|
||||
{\scriptsize Legged robots don't need to maintain stability!}
|
||||
|
||||
To determine dynamics:
|
||||
\bi{(1)} Define control inputs (e.g. wheel speeds),
|
||||
\bi{(2)} Assumptions / Simplifications about state \& inputs
|
||||
@@ -17,3 +15,18 @@ To determine dynamics:
|
||||
\bi{(4)} Express as func of states / inp
|
||||
\bi{(5)} Reduce to the CoM
|
||||
\bi{(6)} Plug into Newton-Euler eq combined with rigid body kinematics
|
||||
|
||||
\subsection{Manipulator Velocity Kinematics}
|
||||
\label{sec:manipulator-velocity-kinematics}
|
||||
Differentiate FK $x = f(\vec{\theta})$, with $\dot{\vec{x}} = [{_W}\vec{v}, {_W} \vec{\omega}]^\top = \mat{J}(\vec{\theta})\dot{\vec{\theta}}$,
|
||||
with $\mat{J} = \frac{\partial f}{\partial \vec{\theta}}$ manipulator Jacobian. Then $\dot{\vec{\theta}} = J^{-1} \vec{\dot{x}}$ or $\vec{\dot{\theta}} = J^+ \vec{\dot{x}}$,
|
||||
if over-actuated, with $J^+ = J^\top(J J^\top)^{-1}$ (Moore Penrose-Inverse)
|
||||
|
||||
|
||||
\newpage
|
||||
\subsection{Legged robots}
|
||||
{\scriptsize Legged robots don't need to maintain stability!}
|
||||
|
||||
\shortdefinition[Static Walking] 3 or more legs on ground, stable if frozen, safe, slow, inefficient.
|
||||
|
||||
\shortdefinition[Dynamic Walking] $<$ 3 legs on ground, falls when not moving, fast, efficient, hard.
|
||||
|
||||
@@ -1,11 +1,6 @@
|
||||
\newpage
|
||||
\subsection{Wheeled robot Kinematics}
|
||||
\begin{wrapfigure}[7]{r}{0.2\columnwidth}
|
||||
\includegraphics[width=0.2\columnwidth]{assets/wheel-constraints.png}
|
||||
\end{wrapfigure}
|
||||
\bi{Non-holonomic} systems \textbf{not integrable}, no inst. move in every direct.
|
||||
|
||||
\bi{Wheel constraints} $v_i = \omega_i r_i$ ($r_i$ constraints)
|
||||
\bi{Wheel constraints} $v_i = \omega_i r_i$ ($r_i$ constraints, (wheel radii))
|
||||
\begin{itemize}
|
||||
\item \textit{Driving straight} all $\vec{v}$ equal (ICR: R.Cent.)
|
||||
\item \textit{Turning} Wheel axis must intersect the \bi{Instant Centre of Rotation} (ICR) of vehicle,
|
||||
@@ -40,16 +35,20 @@ $c_i = [\sin(\alpha + \beta) \div r, -\cos(\alpha + \beta) \div r, -\cos(\beta)
|
||||
\end{tabular}
|
||||
\end{scriptsize}
|
||||
|
||||
\shortdefinition[Differential Drive Kinematics]
|
||||
\subsubsection{Differential Drive Kinematics}
|
||||
\label{sec:diff-drive-kin}
|
||||
|
||||
\bi{State vec} $\vec{x} = [x_1, x_2, \theta]^\top$,
|
||||
\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, radius of right (left) wheel $r_r$ ($r_l$), $w$ width of robot
|
||||
\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, wheel radii $r_l, r_r$, $w$ width of robot
|
||||
|
||||
\bi{Gen. eq. of Motion} $\dot{x}_1 = v\cos(\theta)$, $\dot{x}_2 = v\sin(\theta)$, $\dot{\theta} = \Omega$,
|
||||
with $v = 0.5\cdot(\omega_l r_l + \omega_r + r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$
|
||||
with $v = 0.5\cdot(\omega_l r_l + \omega_r r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$
|
||||
|
||||
% TODO: Consider adding wheel constraints (planar) here as well (from W05 slides)
|
||||
\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\
|
||||
\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.
|
||||
|
||||
\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$
|
||||
|
||||
\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} + b_i$ with $i \in \{s, t\}$, respectively, with
|
||||
$\vec{b}_s = {\scriptsize \begin{bmatrix}
|
||||
D \cos(\theta) \\
|
||||
D \sin(\theta) \\
|
||||
@@ -59,11 +58,7 @@ $\vec{b}_s = {\scriptsize \begin{bmatrix}
|
||||
R(\sin(\Delta \theta + \theta) - \sin(\theta)) \\
|
||||
-R(\cos(\Delta \theta + \theta) - \cos(\theta)) \\
|
||||
\Delta \theta
|
||||
\end{bmatrix}}$
|
||||
|
||||
\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$
|
||||
|
||||
\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} + b_i$ with $i \in \{s, t\}$, respectively
|
||||
\end{bmatrix}}$; $R$ dist ICR
|
||||
|
||||
\textbf{Swedish Wheels}: Have 3 DoF, typ. (3 pcs) equally spaced on circle.
|
||||
Typ. Param. each: $\alpha, \beta, \gamma$ ($z$, $x$, $y$ axes, oriented along $x$, $z$ up)
|
||||
|
||||
@@ -7,3 +7,7 @@ Probabilistic M.M: use random variable in definition, and s. \ref{sec:error-prop
|
||||
\bi{Motor encoders} Typ. 64-2048 increments per rev; Estimate rot
|
||||
|
||||
\bi{Rolling-Shutter} Most CMOS sensors don't take full image at once, need time stamp for each row
|
||||
|
||||
\shortdefinition[Proprioceptive] Robot-internal states (e.g. IMU, encoders)
|
||||
|
||||
\shortdefinition[Exteroceptive] Environment (e.g. LIDAR, Cameras)
|
||||
|
||||
@@ -35,12 +35,13 @@ $\vec{u} =
|
||||
If p. in diff frame, e.g. $W$-frame then $\vec{u} = \mat{K}[\mat{R}_{CW}\; {_C}\vec{t}_{CW}] {_W}\vec{t}_P$
|
||||
|
||||
\subsubsection{Pinhole Camera Projection with distortion}
|
||||
\shortdefinition Model: $\vec{u} = \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P)))$, with ($\vec{c}$ as above):
|
||||
\shortdefinition Model: $\vec{u} = \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P)))$, with $\vec{c}$ as above, $k_i$ radial distortion params, optional for $i > 2$,
|
||||
$p_i$ tangential distortion parameter, $f_u, f_v$ $x$/$y$ focal lengths in pixels. These should be given by task
|
||||
|
||||
(Projection) $\vec{x'} = \vec{p}({_C}\vec{t}_P) = t_z^{-1} \cdot [t_x, t_y]^\top$
|
||||
|
||||
(Distortion model) $r^2 = x'^2 + y'^2$, $\vec{x''} = \vec{d}(\vec{x'})$\\
|
||||
$\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}; \vec{x'}
|
||||
$\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} \cdot \vec{x'}
|
||||
+
|
||||
{\scriptsize
|
||||
\begin{bmatrix}
|
||||
@@ -51,9 +52,6 @@ $\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k
|
||||
|
||||
(Scale and Centre) $\vec{u} = \vec{k}(\vec{x''}) = \text{diag}([f_u, f_v]) \cdot \vec{x''} + \vec{c}$
|
||||
|
||||
All with $k_i$ radial distortion params, optional for $i > 2$, $p_i$ tang. dist. param, $f_u, f_v$ focal length in pixels
|
||||
|
||||
\shade{green}{Inverse} ${_C}\vec{r} = [\vec{d}^{-1}(\vec{k^{-1}(\vec{u})}), 1]^\top$\\
|
||||
(To unit plane)
|
||||
$\vec{x''} = \vec{k}^{-1}(\vec{u}) = [f_u^{-1}, f_v^{-1}]^\top (\vec{u} - \vec{c})$
|
||||
|
||||
@@ -65,8 +63,8 @@ ${_C}\vec{r} = [\vec{x'}, 1]^\top$
|
||||
|
||||
|
||||
\subsubsection{Undestorting a whole image}
|
||||
$\vec{u}_i = \vec{k}(\vec{d}(\vec{k_{\text{new}}}^{-1}(\vec{u}_{i, \text{new}})))$
|
||||
where $\vec{u}_{i, \text{new}}$ is the place of pixel in output, $\vec{u}_i$ is the input
|
||||
$\vec{u}_i = \vec{k}(\vec{d}(\vec{k_{\text{new}}}^{-1}(\vec{u}_{i, \text{new}})))$,
|
||||
where $\vec{u}_{i, \text{new}}$ is the place of pixel in output, $\vec{u}_i$ is the input, with $k_{\text{new}}$ the new focal lengths and image centre
|
||||
|
||||
\bi{Omnidir. Cam} undistortion model with $f(u, v) = \sum_{i = 0}^{N} a_i \rho^i$
|
||||
with $\rho = \sqrt{(u - c_u)^2 + (v - c_v)^2}$, $N = 4$ accurately describes it for most fisheye and catadioptric cameras
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
\label{sec:ms-lin}
|
||||
$\vec{f}(\vec{x}) \approx \vec{f}(\vec{\overline{x}}) + \mat{J}_{\vec{f}} \big|_{x = \overline{x}}(\vec{x} - \vec{\overline{x}})$, $f'$, no vec in 1D; $\vec{\overline{x}}$ lin. p.
|
||||
|
||||
\shortdefinition[Jac.] $\mat{J}_{\vec{f}}$ rows for eq of $\vec{f}$; cols for vars of each eq.
|
||||
\shortdefinition[Jacobian] $\mat{J}_{\vec{f}}$ rows for eq of $\vec{f}$; cols for vars of each eq.
|
||||
It may also be a single value (if just one var in the state)
|
||||
|
||||
\shortdefinition[Gradient] $\nabla \vec{f}$ is vec, each comp. for par diff of var
|
||||
|
||||
@@ -5,9 +5,11 @@ Alt: compute sum of squared errors $S$, then min. $S$, i.e. solve system $\pardi
|
||||
|
||||
\bi{Man. sol.}: comp. $M = A^\top A$, $b' = A^\top b$, then $Mx = b'$.
|
||||
|
||||
\bi{Numpy}: \texttt{numpy.linalg.lstsq}
|
||||
|
||||
\bi{Prob. sol.}: $\text{argmax} \; \P(\vec{x} \divider \vec{z})$ with Maximum \dots
|
||||
\begin{itemize}
|
||||
\item \bi{Likelihood} $\P(\vec{x} | \vec{z}) \propto \P(\vec{z} | \vec{x}) = \prod_{i = 1}^N \P(\vec{z}_i | \vec{x})$
|
||||
\item \bi{a Post} $\P(\vec{x} | \vec{z}) \propto \P(\vec{z} | \vec{x}) \P(\vec{x}) = \P(\vec{x}) \prod_{i = 1}^N \P(\vec{z}_i | \vec{x})$
|
||||
\end{itemize}
|
||||
{\scriptsize Gauss-Dist (non)-lineaer meas. processed in batches, MLE is weighted non-linear Least Squares}
|
||||
{\scriptsize Gauss-Dist (non)-linear meas. processed in batches, MLE is weighted non-linear Least Squares}
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
\newpage
|
||||
\subsection{Non-Linear Least Squares}
|
||||
Find $\vec{x}^* = \text{argmax}\; \P(\vec{x} | \vec{z}) = \argmin{}(-\log(\P(\vec{x}|\vec{z})))$
|
||||
|
||||
|
||||
@@ -1,7 +1,13 @@
|
||||
\subsection{Bayes Filter (in DAG)}
|
||||
$\vec{x}_k^R$ state at time k, $\vec{z}_k^p$ dist. meas., $\vec{u}^p_k$ wheel odometry (= meas.).
|
||||
Typically care about current state: alternate predict \& update.
|
||||
Prediction with Product/Sum rule for $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1:k - 1}^d]$,
|
||||
Update with Bayes' Theorem for $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1:k}^d]$
|
||||
|
||||
% TODO: Do we really need the below?
|
||||
% Init prev distr $\P[\vec{x}_0^R]$.
|
||||
% Pred: $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1 : k - 1}^d] = \int \P[\vec{x}_k^R \divider \vec{u}_{k}^p, \vec{x}_{k - 1}^d]
|
||||
% \P[\vec{x}_k^R \divider \vec{u}_{1:k - 1}^p, \vec{z}_{1 : k - 1}^d] \dx \vec{x}_{k - 1}^R$
|
||||
% Pred: {\scriptsize $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1 : k - 1}^d] = \int \P[\vec{x}_k^R \divider \vec{u}_{k}^p, \vec{x}_{k - 1}^d]
|
||||
% \P[\vec{x}_k^R \divider \vec{u}_{1:k - 1}^p, \vec{z}_{1 : k - 1}^d] \dx \vec{x}_{k - 1}^R$}
|
||||
%
|
||||
% Update: {\scriptsize $\P[\vec{x_k^R \divider \vec{u}_{1:k}}^p, \vec{z}_{1:k}^d] = \eta \P[\vec{x}_{1:k}^p] $}
|
||||
% with $\eta$ a normalizer
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
\subsection{Particle Filter}
|
||||
Is a bayes filter approximating the state distribution with a set of random samples. Dist. not necessarily unimodal.
|
||||
Update step:
|
||||
Is a Bayes filter approximating the state with $S$ weighted particles $\{ x_{k, s}, w_{k, s} \}$. Distribution not necessarily unimodal.
|
||||
|
||||
Predict: Push each particle through transition model; Update step:
|
||||
\begin{itemize}
|
||||
\item Apply Bayes rule $w'_{k, s} = \P[\vec{z}_i \divider \vec{x}_{k, s}] w_{k - 1, s}$
|
||||
\item Renormalize: $w_{k, s} = w'_{k, s} \div \sum_{s} w'_{k, s}$
|
||||
|
||||
@@ -17,3 +17,4 @@ $\vec{x}_k = \mat{F}\vec{x}_{k - 1} + \mat{G}\vec{u}_k + \mat{L}\vec{w}_k$ with
|
||||
\item \bi{Updated mean}: $\hat{\vec{x}}_{k | k} = \hat{\vec{x}}_{k | k - 1} + \mat{K}_k \vec{y}_k$
|
||||
\item \bi{Updated Cov.}: $\mat{P}_{k | k} = (\mat{I} - \mat{K}_k \mat{H}) \mat{P}_{k | k - 1}$
|
||||
\end{itemize}
|
||||
All with $G$ the input model, $L$ noise map, $\tilde{\vec{z}}_k$ actual measurement, $K$ trust measurement ($R_k$ small vs $P_{k | k - 1}$ small).
|
||||
|
||||
@@ -13,5 +13,13 @@ With $\mat{F}_k = \frac{\partial \vec{f}}{\partial \vec{x}}$ and $\mat{L}_k = \f
|
||||
Difference to KF: $\mat{H}$ becomes $\mat{H}_k$, and $\mat{H}^\top$ is $\mat{H}_k^\top$.
|
||||
They are linearizations of $\vec{h}$, see \ref{sec:temporal-models}
|
||||
|
||||
% \shortexample D-D Rob, measurements: $\Delta \varphi_l, \varphi_r$, GPS pos, yaw angle.
|
||||
% TODO: Finish this?
|
||||
\bi{Limitations} Linearization error (shifts bias or sharpens it), fixed by UKF (by passing charact. points around mean through nonlin. func instead of Jac) or Iterative EKF
|
||||
|
||||
\shortexample[Diff-Drive Lawn Mower] Known: Radii ($r_l$ and $r_r$) and track $w$. Measurements: wheel increments ($\Delta \varphi_l$ and $\Delta \varphi_r$).
|
||||
State $[x, y, \theta]^\top$. Can compute distance, heading change \& turn radius.
|
||||
|
||||
For \bi{State Transition}:
|
||||
$D = 0.5(r_l \Delta \varphi_l + r_r \Delta \varphi_r)$, rest same as in \ref{sec:diff-drive-kin}.
|
||||
$F_k$ is of form $[1, 0, \vec{d}_1; 0, 1, \vec{d}_2; 0, 0, 1]$, with $\vec{d} = b_s$ or $\vec{d} = b_t$ for straight and turning, respectively.
|
||||
|
||||
\bi{Updates} are linear (GPS: $[x, y]^\top$ and compass: $\theta$, matrix: $\mat{I}$)
|
||||
|
||||
@@ -1,13 +1,20 @@
|
||||
\subsection{Keypoints}
|
||||
\bi{Corner det.} $SSD(\Delta x, \Delta y) \approx [\Delta_x \; \Delta_y] \mat{M} [\Delta_x \; \Delta_y]^\top$
|
||||
with $\mat{M} = \mat{R}^\top \text{diag}(\lambda_1, \lambda_2) \mat{R}$; $\lambda_i$ E.V. of $M$; $\kappa$ const 0.04-0.15;\\
|
||||
$\lambda_i$ E.V. of $M$; $\kappa$ const 0.04-0.15;
|
||||
$\mat{R} = \det(M) - \kappa \cdot \text{TR}(M)^2 = \lambda_1\lambda_2 - \kappa(\lambda_1 + \lambda_2)^2$;\\
|
||||
$M = \sum_{x, y \in P}
|
||||
$\mat{M} = \sum_{x, y \in P}
|
||||
{\scriptsize
|
||||
\begin{bmatrix}
|
||||
I_x^2 & I_x I_y \\
|
||||
I_x I_y & I_y^2
|
||||
\end{bmatrix}
|
||||
=
|
||||
R^\top
|
||||
\begin{bmatrix}
|
||||
\lambda_1 & 0 \\
|
||||
0 & \lambda_2
|
||||
\end{bmatrix}
|
||||
R
|
||||
}$
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
\newpage
|
||||
\subsubsection{Rapidly-Exploring Random Tree (RRT)}
|
||||
\begin{algorithm}
|
||||
\small
|
||||
|
||||
Reference in New Issue
Block a user