Description

Exercise 1.1
The odometry function ho is defined as:

where the states are: ]. Therefore, its Jacobian matrix can be defined as:

Similarly for measurement function hl:

where the states are: ]. Therefore, its Jacobian matrix can be defined as:

Exercise 1.4.4
Table 1: 2d linear.npz
Method Average Time (s)
pinv 6.903
QR 1.088
QR COLAMD 2.356
LU 0.1817
LU COLAMD 0.1496
The table above shows that LU COLAMD is the fastest method in solve the system in 2d linear.npz. The order of solver efficiency ranks from LU COLAMD being best, followed by LU, QR,
QR COLAMD, pinv in this order. The sparsity of the different decomposition methods can be seen in Figure 1 below. From the figures, it can be seen that there is a correlation in the decrease of solving time and sparsity of the matrix. It was, however, interesting to see that QR manages to finish faster than QR COLAMD despite not having its columns reorganized for a faster traverse through its back-end column tracking. Going back to the comparison between QR and LU decompositions, it is a known fact that QR back-substitution makes it so that it is more numerically stable than the LU solver at the cost of solving time [3]. Hence it is expected for LU-class solvers to find a solution for the system of equations much faster QR-class solvers. The visualization of the trajectory and landmarks from the solvers can be referenced in the Appendix. In summary, all 4 methods can generate similar estimation as observed in the Appendix.

(a) QR
(b) QR COLAMD

(c) LU (d) LU COLAMD
Figure 1: Visualization of all triangular matrices in 2d linear.npz
Exercise 1.4.5
Table 2: 2d linear loop.npz
Method Average Time (s)
pinv 0.512
QR 0.717
QR COLAMD 0.098
LU 0.060
LU COLAMD 0.009
Exercise 2.1
From the provided equation in Equation 2 in the problem statement, the Jacobian in respect to state is as follow as below:

(a) QR (b) QR COLAMD

(c) LU (d) LU COLAMD
Figure 2: Visualization of all triangular matrices in 2d linear loop.npz
Exercise 2.3
The visualization of trajectory and landmarks before and after optimization using LU COLAMD can be seen in Figure 14 & 15 in the Appendix. In the linear case, the least-square error solution Θ can be isolated and solved through decomposition methods in a batch method. However, it is impossible to realize the solution using the same methodology in the nonlinear case. The least-square error must be minimized incrementally through combination of Taylor series approximation and gradient descent algorithm. Once the nonlinear system is linearized to an acceptable error tolerance, only then can the solution be achieved.
Appendix
The trajectory and landmark visualization from all test cases are recorded in this section.

Figure 3: 2d linear.npz estimation using QR

Figure 4: 2d linear.npz estimation using QR COLAMD

Figure 5: 2d linear.npz estimation using LU

Figure 6: 2d linear.npz estimation using LU COLAMD

Figure 7: 2d linear loop.npz estimation using QR

Figure 8: 2d linear loop.npz estimation using QR Figure 9: 2d linear loop.npz estimation using LU

Figure 10: 2d linear loop.npz estimation using LU Figure 11: 2d nonlinear.npz estimation using QR

Figure 12: 2d nonlinear.npz estimation using QR

Figure 13: 2d nonlinear.npz estimation using LU

Figure 14: 2d nonlinear.npz prior estimate optimization using LU COLAMD Figure 15: 2d nonlinear.npz estimation using LU COLAMD
References