Cognitive Robotics – Formula Sheet

1. Probability Theory

Conditional Probability

$$p(x \mid y) = \frac{p(x,y)}{p(y)}$$ $$p(x,y) = p(x \mid y)p(y)$$

Law of Total Probability

Discrete:

$$p(x) = \sum_y p(x \mid y)p(y)$$

Continuous:

$$p(x) = \int p(x \mid y)p(y),dy$$

Bayes' Rule

$$p(x \mid y) = \frac{p(y \mid x)p(x)}{p(y)}$$ $$p(x \mid y) = \eta \cdot p(y \mid x)p(x)$$

Complement Rule

$$p(\neg A) = 1 - p(A)$$

Entropy

How uncertain are we about a random variable X?

(The entries of X can be a discrete set of states/ a distribution over states. A distribution (kitchen, bedrrom, hallway)=(1, 0, 0) has low entropy, while a distribution (0.33, 0.33, 0.34) has high entropy)

$$H(X) = -\sum_x p(x)\log p(x)$$

2. Gaussian Distributions

Univariate Gaussian

$$\mathcal{N}(x;\mu,\sigma^2) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left( -\frac{(x-\mu)^2}{2\sigma^2} \right)$$

Multivariate Gaussian

$$\mathcal{N}(x;\mu,\Sigma) = \frac{1}{\sqrt{(2\pi)^n |\Sigma|}} \exp\left( -\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu) \right)$$

Linear Transformation

$$x' = A x + b$$ $$\mu' = A\mu + b$$ $$\Sigma' = A\Sigma A^T$$

3. Bayes Filter

Belief Definition

$$Bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t})$$

Markov Assumptions

$$p(x_t \mid x_{0:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t)$$ $$p(z_t \mid x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t)$$

Recursive Bayes Filter

$$Bel(x_t) = \eta p(z_t \mid x_t) \int p(x_t \mid u_t, x_{t-1}) Bel(x_{t-1}) dx_{t-1}$$ $$\eta = \frac{1}{p(z_t \mid z_{1:t-1}, u_{1:t})}$$

Prediction

$$\overline{Bel}(x_t) = \int p(x_t \mid u_t, x_{t-1}) Bel(x_{t-1}) dx_{t-1}$$

Correction

$$Bel(x_t) = \eta p(z_t \mid x_t)\overline{Bel}(x_t)$$

4. Kalman Filter

Linear System Model

$$x_t = A x_{t-1} + B u_t + \epsilon_t$$ $$\epsilon_t \sim \mathcal{N}(0,R)$$

R: motion noise covariance

Measurement Model

$$z_t = C x_t + \delta_t$$ $$\delta_t \sim \mathcal{N}(0,Q)$$

Q: measurement noise covariance

Prediction

$$\bar{x}_t = A_t x_{t-1} + B_t u_t$$ $$\bar{\Sigma}_t = A_t \Sigma_{t-1} A_t^T + R$$

Kalman Gain

$$K_t = \bar{\Sigma}_t C_t^T \left( C_t \bar{\Sigma}_t C_t^T + Q \right)^{-1}$$

Correction

$$x_t = \bar{x}_t + K_t (z_t - C_t\bar{x}_t)$$ $$\Sigma_t = (I - K_t C_t)\bar{\Sigma}_t$$

1D Kalman Filter

Prediction: The mean is transformed linearly and the variances add up.

$$\mu_1 = \mu_0 + u$$ $$\sigma_1^2 = \sigma_0^2 + \sigma_u^2 \quad \quad \quad \sigma_1 = \sqrt{\sigma_0^2 + \sigma_u^2}$$

Correction: The Kalman gain is the ratio of the variance of the prediction to the total variance (prediction + measurement). The innovation $z-\mu_1$ is added to the prediction mean, weighted by the Kalman gain. The variance is reduced by a factor of (1-K).

$$K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_z^2}$$ $$\mu = \mu_1 + K (z-\mu_1)$$ $$\sigma^2 = (1-K)\sigma_1^2$$

5. Extended Kalman Filter (EKF)

Nonlinear Models

$$x_t = g(u_t, x_{t-1}) + \epsilon_t$$ $$z_t = h(x_t) + \delta_t$$

Jacobians

$$G_t = \frac{\partial g(u_t, x)}{\partial x}\Big|_{x=\mu_{t-1}}$$ $$H_t = \frac{\partial h(x)}{\partial x}\Big|_{x=\bar{\mu}_t}$$

EKF Prediction

$$\bar{\mu}_t = g(u_t, \mu_{t-1})$$ $$\bar{\Sigma}_t = G_t \Sigma_{t-1} G_t^T + R$$

EKF Correction

$$K_t = \bar{\Sigma}_t H_t^T (H_t \bar{\Sigma}_t H_t^T + Q)^{-1}$$ $$\mu_t = \bar{\mu}_t + K_t (z_t - h(\bar{\mu}_t))$$ $$\Sigma_t = (I - K_t H_t)\bar{\Sigma}_t$$

6. Unscented Kalman Filter (UKF)

TODO weights

Sigma Points

$$\chi_0 = \mu$$ $$\chi_i = \mu + (\sqrt{(n+\lambda)\Sigma})_i$$ $$\chi_{i+n} = \mu - (\sqrt{(n+\lambda)\Sigma})_i$$ $$\lambda = \alpha^2(n+\kappa) - n$$

Mean

$$\mu = \sum_i w_i^{(m)} \chi_i$$

Covariance

$$\Sigma = \sum_i w_i^{(c)}(\chi_i - \mu)(\chi_i - \mu)^T$$

7. Particle Filter

Importance Weight (General)

$$w_t^{(i)} \propto \frac{ p(z_t \mid x_t^{(i)}) p(x_t^{(i)} \mid u_t, x_{t-1}^{(i)}) }{ q(x_t^{(i)} \mid x_{t-1}^{(i)},u_t,z_t) }$$

or recursively:

$$w_t^{(i)} = w_{t-1}^{(i)} \frac{ p(z_t \mid x_t^{(i)}) p(x_t^{(i)} \mid u_t, x_{t-1}^{(i)}) }{ q(x_t^{(i)} \mid x_{t-1}^{(i)},u_t,z_t) }$$

Bootstrap Filter

$$w_t^{(i)} \propto p(z_t \mid x_t^{(i)})$$

Normalization

$$w_t^{(i)} = \frac{w_t^{(i)}}{\sum_j w_t^{(j)}}$$

Effective Sample Size

$$N_{\text{eff}} = \frac{1}{\sum_i (w_t^{(i)})^2}$$

8. Differential Drive & Motion Models

Wheel Velocities

$$v = \frac{v_r + v_l}{2}$$ $$\omega = \frac{v_r - v_l}{L}$$

Instantaneous Turning Radius

$$R = \frac{v}{\omega}$$

Velocity Motion Model

$$x' = x - \frac{v}{\omega}\sin\theta + \frac{v}{\omega}\sin(\theta + \omega \Delta t)$$ $$y' = y + \frac{v}{\omega}\cos\theta - \frac{v}{\omega}\cos(\theta + \omega \Delta t)$$ $$\theta' = \theta + \omega \Delta t$$

Special Case: $\omega = 0$, straight line motion

$$x' = x + v \Delta t \cos\theta$$ $$y' = y + v \Delta t \sin\theta$$ $$\theta' = \theta$$

9. Occupancy Grid Mapping

Binary Bayes Update

Assumption: Cells are independent given measurements.

$$p(m_i \mid z_{1:t}, x_{1:t}) = \frac{ p(z_t \mid m_i, x_t) p(m_i \mid z_{1:t-1}, x_{1:t-1}) }{ p(z_t \mid z_{1:t-1}, x_{1:t}) }$$

Log-Odds Representation

$$l_t = \log \frac{p(m_i \mid z_{1:t}, x_{1:t})}{p(\neg m_i \mid z_{1:t}, x_{1:t})}$$

Log-Odds Update

$$l(m_i | z_{1:t}, x_{1:t}) = l(m_i | z_t, x_t) + l(m_i | z_{1:t-1}, x_{1:t-1}) - l(m_i)$$

10. EKF-SLAM

State Vector

$$\mu = \begin{pmatrix} x \\ m \end{pmatrix}$$ $$\dim(\mu) = 3 + 2N$$ $$\Sigma \in \mathbb{R}^{(3+2N)\times(3+2N)}$$ $$\Sigma = \begin{pmatrix} \Sigma_{xx} & \Sigma_{xm} \\ \Sigma_{mx} & \Sigma_{mm} \end{pmatrix}$$

11. FastSLAM (Rao-Blackwellization)

$$p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) = p(x_{1:t} \mid z_{1:t}, u_{1:t}) \prod_j p(m_j \mid x_{1:t}, z_{1:t})$$

12. ICP (Least Squares Alignment)

Minimize:

$$E(R,t) = \sum_i | y_i - (R x_i + t) |^2$$

Centroids:

$$\bar{x} = \frac{1}{N}\sum_i x_i$$ $$\bar{y} = \frac{1}{N}\sum_i y_i$$

Covariance:

$$H = \sum_i (x_i - \bar{x})(y_i - \bar{y})^T$$

SVD:

$$H = U \Sigma V^T$$ $$R = V U^T$$ $$t = \bar{y} - R \bar{x}$$

13. A*

TODO admissability and consistency

$$f(n) = g(n) + h(n)$$

Without heuristic:

$$f(n) = g(n)$$

14. Hough Transform (Line)

$$\rho = x\cos\theta + y\sin\theta$$

15. Precision / Recall / F1

$$\text{Precision} = \frac{TP}{TP + FP}$$ $$\text{Recall} = \frac{TP}{TP + FN}$$ $$F_1 = \frac{2 \cdot \text{Precision} \cdot \text{Recall}} {\text{Precision} + \text{Recall}}$$

16. Distance Point to Line

For line through points (A,B) and point (C):

$$d = \frac{| (B-A) \times (C-A) |} {|B-A|}$$

17. DWA Navigation Function

$$NF = \alpha \text{vel} + \beta nf + \gamma \Delta nf + \theta goal$$

18. Types of Robotics

• Classical Robotics: (e.g. industrial robots)
  • Exact models of the world
  • No sensing necessary
• Reactive Paradigm: (e.g. Didabot)
  • No world model at all
  • Relies heavily on sensing
• Hybrid Systems: (e.g. rovers)
  • model based at higher levels, e.g. for navigation
  • reactive at lower levels, e.g. for obstacle avoidance
• Probabilistic Robotics: (e.g. self-driving cars)
  • Integrates models and sensing with explicit representation of uncertainty (e.g. bayesian filters)
  • Models and sensors are inaccurate
• Cognitive Robotics: (e.g. humanoid robots)
  • Cognitive functions normally associated with people or animals: act autonomously to achieve goals and cope with unpredictable situations
  • Can interpret various kinds of sensor data