Week 7 Optional Exercises: Least Squares, MLE, MAP, and GraphSLAM

These exercises are optional and are not graded. They are based on the material from Lecture 7. Try each exercise on your own first, then click on Solution to reveal a worked answer.

Exercise 1 - Linear Least Squares

You are given four data points \((x_i, z_i)\): \((0, 1)\), \((1, 3)\), \((2, 4)\), \((3, 4)\). You believe they were generated by a line \(z = \theta_0 + \theta_1 x\) plus noise.

(a) Write down the normal equations \(\left[\sum_i \mathbf{H}_{x_i}^\T\mathbf{H}_{x_i}\right]\boldsymbol\theta = \sum_i \mathbf{H}_{x_i}^\T z_i\) with \(\mathbf{H}_{x_i} = \begin{bmatrix} 1 & x_i \end{bmatrix}\), and solve them for \(\boldsymbol\theta_{LS}\).

(b) Compute the residuals and the fitting error \(e(\boldsymbol\theta_{LS})\).

(c) Stack the rows \(\mathbf{H}_{x_i}\) into a matrix \(\mathbf{H} \in \R^{N \times 2}\) and the measurements into \(\mathbf{z} \in \R^N\). Write \(e(\boldsymbol\theta)\) and the least squares solution in matrix form. When is \(\mathbf{H}^\T\mathbf{H}\) not invertible? Give an example with two data points.

(d) How would you change \(\mathbf{H}_{x_i}\) to fit a quadratic \(z = \theta_0 + \theta_1 x + \theta_2 x^2\)? Why is this still called linear least squares?

(a) \[ \sum_i \begin{bmatrix} 1 & x_i \\ x_i & x_i^2 \end{bmatrix} = \begin{bmatrix} 4 & 6 \\ 6 & 14 \end{bmatrix}, \qquad \sum_i \begin{bmatrix} z_i \\ x_iz_i \end{bmatrix} = \begin{bmatrix} 12 \\ 23 \end{bmatrix}. \] The determinant is \(4 \cdot 14 - 6 \cdot 6 = 20\), so \[ \theta_0 = \frac{14 \cdot 12 - 6 \cdot 23}{20} = 1.5, \qquad \theta_1 = \frac{4 \cdot 23 - 6 \cdot 12}{20} = 1. \] The fitted line is \(z = 1.5 + x\).

(b) The predictions are \(1.5, 2.5, 3.5, 4.5\), so the residuals are \(-0.5, 0.5, 0.5, -0.5\) and \(e = 4 \times 0.25 = 1\).

(c) \(e(\boldsymbol\theta) = \|\mathbf{z} - \mathbf{H}\boldsymbol\theta\|^2\), and setting the gradient \(-2\mathbf{H}^\T(\mathbf{z} - \mathbf{H}\boldsymbol\theta)\) to zero gives \(\boldsymbol\theta_{LS} = (\mathbf{H}^\T\mathbf{H})^{-1}\mathbf{H}^\T\mathbf{z}\). \(\mathbf{H}^\T\mathbf{H}\) is invertible if and only if the columns of \(\mathbf{H}\) are linearly independent. For a line, this fails if all \(x_i\) are equal, e.g. the two points \((1, 2)\) and \((1, 5)\): infinitely many lines pass through \(x = 1\) and fit the average equally well, so the parameters are not determined by the data.

(d) Use \(\mathbf{H}_{x_i} = \begin{bmatrix} 1 & x_i & x_i^2 \end{bmatrix}\). The model is nonlinear in \(x\), but it is linear in the parameters \(\boldsymbol\theta\), which is what matters: the error is still a quadratic function of \(\boldsymbol\theta\), and its minimizer is the solution of a linear system.

Exercise 2 - Maximum Likelihood Estimation

(a) Starting from the log-likelihood in lecture, derive the maximum likelihood estimates \(\mu_{MLE}\) and \(\sigma^2_{MLE}\) for i.i.d. 1D Gaussian data.

(b) Evaluate them for the data \(\{2, 4, 4, 6\}\).

(c) Suppose \(z_i = \mathbf{H}_{x_i}\boldsymbol\theta + v_i\) with independent noise \(v_i \sim \mathcal{N}(0, \sigma^2)\). Show that the maximum likelihood estimate of \(\boldsymbol\theta\) is the least squares estimate.

(d) Now suppose the noise variance differs between measurements: \(v_i \sim \mathcal{N}(0, \sigma_i^2)\). What does the maximum likelihood estimate minimize? Apply it to two sensors that measure the same scalar \(\theta\) directly: sensor 1 reports \(z_1 = 10\) with \(\sigma_1 = 1\), and sensor 2 reports \(z_2 = 12\) with \(\sigma_2 = 2\).

(a) The log-likelihood is \(\ell(\mu, \sigma) = -N\log(\sqrt{2\pi}\sigma) - \frac{1}{2\sigma^2}\sum_i(d_i - \mu)^2\).

  • \(\frac{\partial\ell}{\partial\mu} = \frac{1}{\sigma^2}\sum_i(d_i - \mu) = 0 \Rightarrow \mu_{MLE} = \frac{1}{N}\sum_i d_i\).
  • \(\frac{\partial\ell}{\partial\sigma} = -\frac{N}{\sigma} + \frac{1}{\sigma^3}\sum_i(d_i - \mu)^2 = 0 \Rightarrow \sigma^2_{MLE} = \frac{1}{N}\sum_i(d_i - \mu_{MLE})^2\).

(b) \(\mu_{MLE} = 16/4 = 4\) and \(\sigma^2_{MLE} = (4 + 0 + 0 + 4)/4 = 2\). (The MLE of the variance is biased; the unbiased estimate divides by \(N - 1\) and gives \(8/3\).)

(c) Each \(z_i \sim \mathcal{N}(\mathbf{H}_{x_i}\boldsymbol\theta, \sigma^2)\) independently, so \[ \log p(z_{1:N} \mid \boldsymbol\theta) = -N\log(\sqrt{2\pi}\sigma) - \frac{1}{2\sigma^2}\sum_i(z_i - \mathbf{H}_{x_i}\boldsymbol\theta)^2. \] The first term does not depend on \(\boldsymbol\theta\), so maximizing the log-likelihood is equivalent to minimizing \(\sum_i(z_i - \mathbf{H}_{x_i}\boldsymbol\theta)^2 = e(\boldsymbol\theta)\).

(d) It minimizes the weighted least squares error \(\sum_i \frac{(z_i - \mathbf{H}_{x_i}\boldsymbol\theta)^2}{\sigma_i^2}\), so precise measurements count more. For the two sensors, setting the derivative of \(\frac{(10 - \theta)^2}{1} + \frac{(12 - \theta)^2}{4}\) to zero gives \[ \theta_{MLE} = \frac{z_1/\sigma_1^2 + z_2/\sigma_2^2}{1/\sigma_1^2 + 1/\sigma_2^2} = \frac{10 + 3}{1.25} = 10.4. \] The estimate is closer to the more precise sensor. Its variance is \(\frac{1}{1/\sigma_1^2 + 1/\sigma_2^2} = 0.8\), smaller than either sensor’s variance. This kind of fusion is exactly what the Kalman filter does recursively (Lecture 9).

Exercise 3 - Maximum A Posteriori Estimation

A robot measures the position \(\theta\) of a landmark directly, \(N\) times: \(z_i = \theta + v_i\) with \(v_i \sim \mathcal{N}(0, \sigma^2)\). A map gives a prior \(\theta \sim \mathcal{N}(\theta_0, \sigma_0^2)\).

(a) Write down the negative log-posterior (up to constants) and derive \(\theta_{MAP}\).

(b) Evaluate \(\theta_{MLE}\) and \(\theta_{MAP}\) for \(\theta_0 = 0\), \(\sigma_0^2 = 1\), \(\sigma^2 = 1\), and four measurements all equal to \(2\).

(c) What happens to \(\theta_{MAP}\) as \(N \to \infty\)? As \(\sigma_0^2 \to \infty\)? Interpret.

(d) For a general linear model \(\mathbf{z} = \mathbf{H}\boldsymbol\theta + \mathbf{v}\) with \(\mathbf{v} \sim \mathcal{N}(\mathbf{0}, \sigma^2 I)\) and prior \(\boldsymbol\theta \sim \mathcal{N}(\mathbf{0}, \sigma_0^2 I)\), show that \(\boldsymbol\theta_{MAP} = (\mathbf{H}^\T\mathbf{H} + \lambda I)^{-1}\mathbf{H}^\T\mathbf{z}\) and find \(\lambda\). Why does this also fix the non-invertible case from Exercise 1(c)?

(e) In the setting of (b), the full Bayesian posterior is Gaussian. What are its mean and variance? What extra information does it give compared to \(\theta_{MAP}\)?

(a) Since \(p(\theta \mid z_{1:N}) \propto p(z_{1:N} \mid \theta)\,p(\theta)\), \[ -\log p(\theta \mid z_{1:N}) = \frac{1}{2\sigma^2}\sum_i(z_i - \theta)^2 + \frac{1}{2\sigma_0^2}(\theta - \theta_0)^2 + \text{const}. \] Setting the derivative to zero gives \[ \theta_{MAP} = \frac{\frac{1}{\sigma^2}\sum_i z_i + \frac{1}{\sigma_0^2}\theta_0}{\frac{N}{\sigma^2} + \frac{1}{\sigma_0^2}}. \]

(b) \(\theta_{MLE} = 2\) and \(\theta_{MAP} = \frac{8 + 0}{4 + 1} = 1.6\). The prior pulls the estimate toward \(\theta_0 = 0\).

(c) As \(N \to \infty\), the data terms dominate and \(\theta_{MAP} \to \theta_{MLE}\): with enough data, the prior no longer matters. As \(\sigma_0^2 \to \infty\), the prior becomes uninformative and again \(\theta_{MAP} \to \theta_{MLE}\).

(d) The negative log-posterior is \(\frac{1}{2\sigma^2}\|\mathbf{z} - \mathbf{H}\boldsymbol\theta\|^2 + \frac{1}{2\sigma_0^2}\|\boldsymbol\theta\|^2\). Multiplying by \(2\sigma^2\) and setting the gradient to zero gives \(\mathbf{H}^\T(\mathbf{H}\boldsymbol\theta - \mathbf{z}) + \lambda\boldsymbol\theta = 0\) with \(\lambda = \sigma^2/\sigma_0^2\), hence \(\boldsymbol\theta_{MAP} = (\mathbf{H}^\T\mathbf{H} + \lambda I)^{-1}\mathbf{H}^\T\mathbf{z}\) (this is known as ridge regression). Since \(\mathbf{H}^\T\mathbf{H}\) is positive semi-definite and \(\lambda > 0\), the sum is positive definite and always invertible: the prior determines the directions that the data cannot.

(e) The posterior is \(\mathcal{N}(1.6, \frac{1}{N/\sigma^2 + 1/\sigma_0^2}) = \mathcal{N}(1.6, 0.2)\). Besides the most likely value, it quantifies the uncertainty of the estimate, e.g. a 95% confidence interval of roughly \(1.6 \pm 2\sqrt{0.2} \approx [0.71, 2.49]\).

Exercise 4 - Multivariate Gaussians and Covariance

Let \(\mathbf{x} = (x_1, x_2) \sim \mathcal{N}(\boldsymbol\mu, \Sigma)\) with \(\boldsymbol\mu = (1, 2)\) and \(\Sigma = \begin{bmatrix} 4 & 2 \\ 2 & 3 \end{bmatrix}\).

(a) What are \(\text{Var}[x_1]\), \(\text{Var}[x_2]\), and \(\text{Cov}[x_1, x_2]\)? Compute the correlation coefficient \(\rho = \frac{\text{Cov}[x_1, x_2]}{\sqrt{\text{Var}[x_1]\text{Var}[x_2]}}\).

(b) Use \(\text{Cov}[A\mathbf{x} + \mathbf{b}] = A\,\text{Cov}[\mathbf{x}]A^\T\) to compute the mean and variance of \(y = x_1 + x_2\), and of \(w = x_1 - x_2\). Express your answers in terms of \(\text{Var}[x_1]\), \(\text{Var}[x_2]\), \(\text{Cov}[x_1, x_2]\).

(c) Describe the shape of the equal-probability contours (ellipses) of this Gaussian. Along which directions do the principal axes point, and how do their lengths compare?

(d) Give an example of two random variables that are uncorrelated but not independent. Why does this not happen when \((X, Y)\) is jointly Gaussian?

(a) \(\text{Var}[x_1] = 4\), \(\text{Var}[x_2] = 3\), \(\text{Cov}[x_1, x_2] = 2\), so \(\rho = 2/\sqrt{12} \approx 0.577\): the two variables are positively correlated.

(b) For \(y\), \(A = \begin{bmatrix} 1 & 1 \end{bmatrix}\): \(\E[y] = 3\) and \(\text{Var}[y] = A\Sigma A^\T = 4 + 2 + 2 + 3 = 11\). In general, \(\text{Var}[x_1 + x_2] = \text{Var}[x_1] + \text{Var}[x_2] + 2\,\text{Cov}[x_1, x_2]\).

For \(w\), \(A = \begin{bmatrix} 1 & -1 \end{bmatrix}\): \(\E[w] = -1\) and \(\text{Var}[w] = 4 - 2 - 2 + 3 = 3\). In general, \(\text{Var}[x_1 - x_2] = \text{Var}[x_1] + \text{Var}[x_2] - 2\,\text{Cov}[x_1, x_2]\). Positive correlation increases the variance of the sum and decreases the variance of the difference.

(c) The contours are ellipses centered at \((1, 2)\) whose axes point along the eigenvectors of \(\Sigma\), with semi-axis lengths proportional to the square roots of the eigenvalues. The characteristic polynomial \(\lambda^2 - 7\lambda + 8 = 0\) gives \(\lambda = (7 \pm \sqrt{17})/2 \approx 5.56\) and \(1.44\). Because of the positive off-diagonal entry, the ellipse is tilted, with its long axis pointing up and to the right (roughly along \((0.79, 0.61)\)), and it is about \(\sqrt{5.56/1.44} \approx 2\) times longer than it is wide.

(d) Let \(X \sim \mathcal{N}(0, 1)\) and \(Y = X^2\). Then \(\text{Cov}[X, Y] = \E[X^3] - \E[X]\E[X^2] = 0\), but \(Y\) is completely determined by \(X\). Covariance only measures linear dependence. For a jointly Gaussian vector, the density factorizes into the product of the marginals exactly when the covariance matrix is diagonal, so uncorrelated implies independent.

Exercise 5 - A 1D GraphSLAM Problem

A robot moves along a line. Its initial position \(x_0 = 0\) is known. It executes two odometry commands \(u_0 = u_1 = 1\), with dynamics \(x_t = x_{t-1} + u_{t-1} + w_t\), and it observes a single landmark at unknown position \(m\) through the measurement model \(z = m - x + v\) (the relative position of the landmark). It measures \(z_0 = 3\) from \(x_0\) and \(z_2 = 0.8\) from \(x_2\). All noise terms have variance \(1\).

(a) Draw the graph of this problem (nodes for \(x_0, x_1, x_2, m\), and an edge for every factor).

(b) Write down the GraphSLAM least squares cost as a function of \(x_1, x_2, m\).

(c) Write the cost as \(\|A\boldsymbol\theta - \mathbf{b}\|^2\) with \(\boldsymbol\theta = (x_1, x_2, m)\), form the normal equations \(A^\T A\boldsymbol\theta = A^\T\mathbf{b}\), and solve them.

(d) Compare your answer to what odometry alone and the first measurement alone would suggest. How is the inconsistency distributed among the terms?

(e) What would go wrong if \(x_0\) were not given (i.e., if it were also an unknown)?

(f) Look at the sparsity pattern of \(A^\T A\). How does it relate to the graph from (a)? Why does this matter for large SLAM problems?

(a) The graph is a chain \(x_0 - x_1 - x_2\) of odometry edges, plus measurement edges \(x_0 - m\) and \(x_2 - m\). Together they form a single loop.

(b) \[ e(x_1, x_2, m) = (x_1 - 0 - 1)^2 + (x_2 - x_1 - 1)^2 + (3 - (m - 0))^2 + (0.8 - (m - x_2))^2. \]

(c) \[ A = \begin{bmatrix} 1 & 0 & 0 \\ -1 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 1 \end{bmatrix}, \quad \mathbf{b} = \begin{bmatrix} 1 \\ 1 \\ 3 \\ 0.8 \end{bmatrix}, \quad A^\T A = \begin{bmatrix} 2 & -1 & 0 \\ -1 & 2 & -1 \\ 0 & -1 & 2 \end{bmatrix}, \quad A^\T\mathbf{b} = \begin{bmatrix} 0 \\ 0.2 \\ 3.8 \end{bmatrix}. \] The first equation gives \(x_2 = 2x_1\), the third gives \(m = 1.9 + x_1\), and substituting both into the second gives \(2x_1 = 2.1\). So \(x_1 = 1.05\), \(x_2 = 2.1\), and \(m = 2.95\).

(d) Odometry alone gives \(x_2 = 2\), and \(z_0\) alone gives \(m = 3\), which predicts \(z_2 = m - x_2 = 1\). But the robot measured \(0.8\), an inconsistency of \(0.2\) around the loop. Because all four terms have the same variance, the least squares solution spreads the inconsistency equally: every residual has magnitude \(0.05\) (\(x_1 - 1 = 0.05\), \(x_2 - x_1 - 1 = 0.05\), \(3 - m = 0.05\), \(0.8 - (m - x_2) = -0.05\)), for a total cost of \(0.01\). With different variances, the less certain terms would absorb more of the error.

(e) All factors depend only on differences between variables, so adding the same constant to \(x_0, x_1, x_2, m\) leaves the cost unchanged. The minimum would not be unique and \(A^\T A\) would be singular. Fixing \(x_0\) (or adding a prior on it) anchors the solution; this is called fixing the gauge.

(f) Entry \((i, j)\) of \(A^\T A\) is nonzero only if variables \(i\) and \(j\) appear together in some factor, i.e. if they are connected by an edge in the graph. Here \(x_1\) and \(m\) never appear together, so the corresponding entries are zero. In large SLAM problems, each pose is connected to only a few other poses and landmarks, so \(A^\T A\) is very sparse, and sparse linear solvers can solve problems with many thousands of variables efficiently.

Exercise 6 - From MAP to Least Squares in GraphSLAM

(a) Lecture factorized the joint distribution as \(p(\mathbf{x}_0)\prod_t p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_{t-1})\prod_t\prod_k p(\mathbf{z}_t^{(k)} \mid \mathbf{x}_t, \mathbf{m}_k)\). Which conditional independence assumptions does this factorization rely on?

(b) Show that with Gaussian dynamics noise \(\mathcal{N}(\mathbf{0}, \mathbf{R}_t)\) and Gaussian measurement noise \(\mathcal{N}(\mathbf{0}, \mathbf{Q}_t)\), maximizing the posterior is equivalent to minimizing \(\sum_t\|\mathbf{x}_t - \mathbf{f}(\mathbf{x}_{t-1}, \mathbf{u}_{t-1})\|^2_{\mathbf{R}_t} + \sum_{t,k}\|\mathbf{z}_t^{(k)} - \mathbf{h}(\mathbf{x}_t, \mathbf{m}_k)\|^2_{\mathbf{Q}_t}\).

(c) When \(\mathbf{f}\) or \(\mathbf{h}\) are nonlinear (e.g., a unicycle model and range-bearing measurements), the problem is no longer linear least squares. How can it still be solved?

(d) What is the difference between the full SLAM posterior \(p(\mathbf{x}_{1:T}, \mathbf{m} \mid \mathbf{z}_{0:T}, \mathbf{u}_{0:T-1}, \mathbf{x}_0)\) and the filtering SLAM posterior \(p(\mathbf{x}_t, \mathbf{m} \mid \mathbf{z}_{0:t}, \mathbf{u}_{0:t-1}, \mathbf{x}_0)\)? When would you use each?

(a) (1) Markov dynamics: given \(\mathbf{x}_{t-1}\) and \(\mathbf{u}_{t-1}\), the state \(\mathbf{x}_t\) is independent of all earlier states, controls, and measurements, and of the map. (2) Each measurement depends only on the current state and the observed landmark: given \(\mathbf{x}_t\) and \(\mathbf{m}_k\), \(\mathbf{z}_t^{(k)}\) is independent of everything else. (3) Known data association, so we know which landmark produced each measurement.

(b) Taking the logarithm turns the products into sums. For a Gaussian, \(\log\mathcal{N}(\mathbf{x}; \boldsymbol\mu, \Sigma) = -\frac{1}{2}\|\mathbf{x} - \boldsymbol\mu\|^2_\Sigma + \text{const}\), where the constant does not depend on the optimization variables (the covariances are fixed). So the log-posterior is \(-\frac{1}{2}\) times the given sum plus a constant, and maximizing it is the same as minimizing the sum. The Mahalanobis norms \(\|\cdot\|_\Sigma\) weight each term by its inverse covariance, so precise sensors count more.

(c) With an iterative method such as Gauss-Newton or Levenberg-Marquardt: linearize \(\mathbf{f}\) and \(\mathbf{h}\) around the current estimate, solve the resulting linear least squares problem (a sparse linear system) for an update, apply it, and repeat until convergence. A good initial guess (e.g., from odometry) is needed, since the problem is non-convex.

(d) Full SLAM estimates the entire trajectory and the map, using all measurements, including those that arrive after each pose (smoothing). Filtering SLAM only estimates the current pose and map, summarizing the past in the current belief. Full SLAM (e.g., GraphSLAM) gives more accurate trajectories and handles loop closures well, and is used offline or in sliding windows. Filtering (e.g., EKF-SLAM, Lecture 10) is cheaper per step and naturally online, but past errors can no longer be corrected once they are absorbed into the belief.