Tyler Barkin Tyler Barkin

Sensor Fusion

General Idea : Use multiple data sources to improve measurement quality, reliability and coverage, as well as estimate states that aren’t measured directly.

Resources :

Notes :

Sensor Fusion : Combining two or more data sources in a way that generates a better understanding of the system.

  • Increase the quality of the data

    • More Consistent

    • More Accurate

    • More Dependable

  • Increase Reliability

    • Redundant computation paths in case we lose a sensor?

      • Estimation quality will drop but we wont lose the measurement completely.

      • This makes more sense with sensor duplication?

  • Estimation of unmeasured states

  • Increase coverage area

  • Sense

    • Gather Data

  • Perceive

    • Localization and Positioning

    • Detection and Tracking

  • Plan

  • Act

    • Control System

Read More
Tyler Barkin Tyler Barkin

Graph Slam

Resources :

Notes :

  • n-D Measurement Constraint : $$(z_t - h(x_t, m_j))^T Q_t^{-1}(z_t - h(x_t, m_j))$$

  • n-D Motion Constraint : $$(x_t - g(u_t, x_{t-1}))^T R_t^{-1}(x_t - g(u_t, x_{t-1}))$$

    • h() = measurement function

    • g() = motion function

    • Qt = measurement covariance

    • Rt = motion covariance

  • Multidimensional Formula for the sum of all constraints : $$J_{GraphSLAM} = x_0^T \Omega x_0 + \Sigma_t(x_t - g(u_t, x_{t-1}))^T R_t^{-1}(x_t - g(u_t, x_{t-1})) + \Sigma_t (z_t - h(x_t, m_j))^T Q_t^{-1}(z_t - h(x_t, m_j))$$

    • information matrix : $$\Omega = \Sigma^{-1}$$

    • information vector : $$\xi$$

Solving The System of Equations :

Once the information matrix and vector have been populated, the path and map are recovered by : $$\mu = \Omega^{-1}\xi$$

Variable Elimination

  • Applied iteratively to remove all cyclical constraints before solving

Nonlinear Constraints

  • Motion and Measurement constraints must be linearized before they can be added to the information matrix and vector. Otherwise it would be impractical, if not impossible, to solve the system of equations analytically… (When Do NN come into play to fix this one?)

  • Taylor Series approximates a function using the sum of an infinite number of terms. A linear approximation can be computed by using only the first two terms and ignoring all higher-order terms.

  • In multi-dimensional models, the first derivative is replaced by a Jacobian — a matrix of partial derivatives.

Linearizing Constraints

  • $$g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1} + G_t(x_{t-1} - \mu_{t-1})$$

  • $$h(y_t) \approx h(\mu_t) + H_t^i(y_t - \mu_t)$$

What do we set mu to?

  • Apply only the motion constraints to create a pose estimate

    • $$[x_0 … x_t]^T$$ as a primitive estimate in place of $$\mu$$

    • repeat the linearization process several times, each time using a better estimate to linearize the constraints

Read More