Question:
A robot navigates in 1D environment and closes loop by returning to starting point. Our measurements or observed values are given by:
$$ \begin{array}{ccc}\hline \text { Quantity } & \text { Ground Truth } & \text { Observed / Measured } \\\hline \mathbf{u}{0} & 1.0 & 1.1 \\\mathbf{u}{1} & 1.0 & 1.0 \\\mathbf{u}{2} & 1.0 & 1.1 \\\mathbf{u}{3} & -3.0 & -2.7 \\\mathbf{u}_{0,4} & 0.0 & 0.0 \\\hline\end{array} $$
$\mathbf{u_0} \rightarrow \mathbf{u_3}$ is obtained through odometry control inputs from an odometer/IMU. $\mathbf{u_{0,4}}$ is obtained from a loop closure method, say Bag of Visual Words.
Image just for illustrative purposes and isn't representative of metric locations.
Note that in a SLAM setting, we wouldn't have the ground truth. It's been mentioned above so that we can compare and know if we are improving after we apply our algorithm.
Above, there are 4 odometry constraints of the form $\left\|f\left(\mathbf{x}{i}, \mathbf{u}{i}\right)-\mathbf{x}{i+1}\right\|{\mathbf{\Sigma}{i}}^{2}$ and one loop closure of the form $\left\|f\left(\mathbf{x}{0}, \mathbf{u}{0,4}\right)-\mathbf{x}{4}\right\|{\mathbf{\Lambda}{0,4}}^{2}$. Recollect here. Standard deviations for both are initialized as $0.1$. ($\boldsymbol{\sigma}{i}=0.01 \text { and } \boldsymbol{\lambda}{0,4}=0.01$)
Now the question is to get a better estimate of the robot states through optimization.
Answer:
Next robot state is given by (A simple motion model given by $f$) :
$$ \mathbf{x}{i+1} = f\left(\mathbf{x}{i}, \mathbf{u}_{i}\right) = \mathbf{x}_i + \mathbf{u}_i $$
Robot states according to odometry alone:
$$ \begin{array}{ccc}\hline \text { Quantity } & \text { Ground Truth } & \text { According to Odometry } \\\hline \mathrm{x}{0} & 0.0 & 0.0 \\\mathrm{x}{1} & 1.0 & 1.1 \\\mathrm{x}{2} & 2.0 & 2.1 \\\mathrm{x}{3} & 3.0 & 3.2 \\\mathrm{x}_{4} & 0.0 & 0.5 \\\hline\end{array} $$
According to odometry, $\mathbf{x}{4}=0.5,$ but according to the loop closure constraint $\mathbf{x}{4}=\mathbf{x}{0}=0.0$ (In the below residual, $\mathbf{x}{4}=0.5$ but $\mathbf{x}{0} + \mathbf{u}{0,4} = 0$.)
Note in the above table that according to loop closure constraint, $\mathbf{x}{4}=\mathbf{x}{0}=0.0$. Using this "additional" measurement, we want to "correct" our robot state which has gone wrong because odometry drifted eventually (which happens in all practical scenarios).
Our objective function is