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