Linear Algebra and Optimization for Robotics

Linear algebra and optimization form the mathematical backbone upon which all modern autonomous robotics systems are built, serving as the fundamental language for describing spatial relationships, sensor measurements, and control objectives. Whether one is processing point cloud data from LiDAR, optimizing a trajectory for a mobile manipulator, or tuning a feedback controller for aerial autonomy, the ability to formulate problems in matrix form and solve them efficiently is not merely advantageous—it is absolutely essential. This chapter establishes the rigorous mathematical foundations required for the subsequent chapters on perception, planning, and control, bridging abstract theory with concrete implementation details that practitioners encounter daily in real-world robotics deployments.

The ubiquity of linear algebraic structures in robotics stems from the fact that most physical systems exhibit approximately linear behavior within small operating envelopes, and even strongly nonlinear systems can be linearized for local control or approximated using piecewise linear models. Consider the kinematic equations of a six-degree-of-freedom robotic arm: the forward kinematics map joint angles to end-effector poses through a series of homogeneous transformation matrices, each representing a rotation and translation. When expressed in the Lie algebra $\mathfrak{se}(3)$, these transformations become vectors in $\mathbb{R}^6$, and the composition of motions becomes matrix multiplication in the exponential coordinates. This algebraic structure allows us to compute derivatives of pose with respect to joint angles, a prerequisite for Jacobian-based control and inverse kinematics solving.

Matrix decomposition techniques provide the computational engines that transform raw linear systems into forms amenable to stable numerical solution. The Singular Value Decomposition (SVD), for instance, reveals the intrinsic dimensionality of a transformation matrix and exposes the condition number—a critical metric for assessing numerical stability in sensor fusion problems. In visual SLAM (Simultaneous Localization and Mapping), the bundle adjustment problem involves optimizing thousands of camera poses and landmark positions simultaneously; the Jacobian matrix of this optimization is typically sparse and ill-conditioned, requiring careful decomposition strategies to avoid divergence. Similarly, QR decomposition offers superior numerical stability compared to direct normal equation solving for overdetermined linear systems, making it the preferred method for fitting geometric primitives to noisy point cloud data in perception pipelines.

```mermaid graph TD A[Robotics Problem] --> B[Mathematical Formulation] B --> C{Linear or Nonlinear?} C -->|Linear| D[Matrix Decomposition<br/>SVD/QR/Eigen] C -->|Nonlinear| E[Iterative Optimization<br/>Gauss-Newton/LM] D --> F[Direct Solution] E --> G[Linearization] G --> D F --> H[Robotics Application] E --> H H --> I[Perception/Planning/Control] ```

Convex optimization provides the theoretical framework that guarantees global optimality for a vast class of problems encountered in robotics, from motion planning to resource allocation. A convex optimization problem minimizes a convex objective function over a convex feasible set, ensuring that any local minimum is also a global minimum—a property of immense practical value when designing safety-critical autonomous systems. The formulation of path planning as a convex optimization problem, for instance, allows us to certify that a computed trajectory is not merely feasible but optimal with respect to specified criteria such as energy consumption or travel time. When constraints are non-convex, techniques such as sequential convex programming (SCP) iteratively linearize constraints and solve convex subproblems, converging to locally optimal solutions under suitable conditions.

Least squares problems represent the most ubiquitous optimization tasks in robotics, arising naturally from the need to fit models to noisy sensor data. Linear least squares problems, characterized by a quadratic objective function with linear residuals, admit closed-form solutions via the normal equations or more stable solutions via QR decomposition. However, the normal equations $\mathbf{A}^T\mathbf{A}\mathbf{x} = \mathbf{A}^T\mathbf{b}$ amplify the condition number of $\mathbf{A}$ by squaring it, making them numerically disastrous for ill-conditioned problems common in robotics where measurement matrices often exhibit high correlation between columns. Nonlinear least squares extends this framework to models that are nonlinear in the parameters, requiring iterative methods such as Gauss-Newton or Levenberg-Marquardt that alternate between linearizing the residual function and solving the resulting linear subproblem.

The mathematical notation employed throughout this chapter follows standard conventions while emphasizing the structural properties most relevant to robotics applications. Scalars are denoted by lowercase letters $x \in \mathbb{R}$, vectors by bold lowercase letters $\mathbf{x} \in \mathbb{R}^n$, and matrices by bold uppercase letters $\mathbf{A} \in \mathbb{R}^{m \times n}$. The Euclidean norm of a vector is denoted $\|\mathbf{x}\|_2 = \sqrt{\mathbf{x}^T\mathbf{x}}$, while the Frobenius norm of a matrix is $\|\mathbf{A}\|_F = \sqrt{\sum_{i,j} A_{ij}^2}$. The identity matrix of size $n$ is $\mathbf{I}_n$, and the zero matrix of size $m \times n$ is $\mathbf{0}_{m \times n}$. For any matrix $\mathbf{A}$, $\mathbf{A}^T$ denotes the transpose, $\mathbf{A}^{-1}$ the inverse (for square, nonsingular matrices), and $\mathbf{A}^\dagger$ the Moore-Penrose pseudoinverse. The Kronecker product $\otimes$ and the Khatri-Rao product $\odot$ appear frequently in tensor formulations of multi-robot systems and batch estimation problems.

Matrix Decomposition Techniques

Matrix decomposition, or matrix factorization, refers to the process of expressing a matrix as a product of simpler matrices with specific structural properties. These decompositions serve dual purposes: they enable efficient numerical solution of linear systems, and they reveal intrinsic geometric or algebraic properties of the underlying transformation. In robotics, the three most critical decompositions are the Singular Value Decomposition (SVD), the QR decomposition, and the eigendecomposition, each serving distinct purposes in perception, planning, and control.

Singular Value Decomposition

The Singular Value Decomposition is arguably the most versatile matrix factorization in all of numerical linear algebra, applicable to any real or complex matrix regardless of rank or conditioning. For any matrix $\mathbf{A} \in \mathbb{R}^{m \times n}$ with rank $r$, the SVD expresses $\mathbf{A}$ as the product of three matrices: $\mathbf{A} = \mathbf{U}\mathbf{\Sigma}\mathbf{V}^T$, where $\mathbf{U} \in \mathbb{R}^{m \times m}$ has orthonormal columns (left singular vectors), $\mathbf{V} \in \mathbb{R}^{n \times n}$ has orthonormal columns (right singular vectors), and $\mathbf{\Sigma} \in \mathbb{R}^{m \times n}$ is a diagonal matrix with nonnegative real entries $\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_r > 0$ on the diagonal (the singular values). The singular values represent the square roots of the eigenvalues of $\mathbf{A}^T\mathbf{A}$ and $\mathbf{A}\mathbf{A}^T$, providing a measure of the matrix's "stretching" power along orthogonal directions.

The geometric interpretation of SVD is particularly illuminating for robotics applications. Any linear transformation $\mathbf{A}$ can be decomposed into a rotation $\mathbf{V}^T$, a scaling operation along the coordinate axes $\mathbf{\Sigma}$, and a final rotation $\mathbf{U}$. This interpretation explains why SVD is the preferred method for solving the orthogonal Procrustes problem, which arises in point cloud registration and hand-eye calibrati