Intro
Least Squares Optimization is the fundamental algorithm for Robotics.
Planning, Control, State-Estimation, Tracking, Vision, Calibration can be formulated as Least Squares problems.
I want to formulate those problems by Least Squares and implement them.
The goal is to explain the state-of-the-art algorithms clearly and intuitively, (and having fun for myself).
Index
- Least Squares refresher.
- link: Least-squares-refresher
- Gave the definitions of common math concepts.
- Derived the solution of Least Squares problems.
- A warm-up Least Squares problem.
- Given the theoretical background, we can do something fun. I formulated the Lucas-Kanade Visual Tracking algorithm as the Least Squares problem.
- link:Lucas-kanade-tracker
- Formulate the Lucas-Kanade as a Least Squares problem.
- Use a few tricks to solve problems originated from discrete functions.
- With a clear understanding, implementing in C++ is easy.
- Discuss some of the applications/usages of Lucas-Kanade.
- Time Calibration is another interesting application of Least Squares.
- link: Time-calibration
- Formulate the Time Calibration problem as Least Squares.
- Discuss the similarity to Lucas-Kanade.
- Discuss the IROS 2018 best paper: Online Temporal Calibration for Monocular Visual-Inertial Systems.
- Now, it’s time to do something super cool. Let’s land a rocket.
- link: How to Land a Rocket? Section 1
- Define the rocket state and rocket motion model.
- Formulate the rocket landing problem as the Least Squares problem.
- Apply regularization to constrain the trajectory.
- To make it real-time. I used the separable cost function property and sparse system solver.
- Discuss a classical planning paper.
- We didn’t handle constraints in the previous section. In this section, we will use Primal-Dual Interior-Point Methods to handle inequality constraints.
- link: How to Land a Rocket? Section 2
- A simple introduction to the KKT conditions, which are the optimality condition for constrained optimization problems.
- To solve the KKT conditions, we break the KKT conditions into two parts and solve them separately.
- Solve a numerical issue in the KKT conditions.
- Put everything together, we have the Primal-Dual Interior-Point Methods.
- Implementation in C++.
- There are other ways to handle constraints. The barrier method and active set method. I discussed two papers.
- Some interesting reading about KKT conditions and rocket landing.
- In the previous section, we solve the KKT linearized system by sparse LU decomposition. We can make it faster by exploiting the structure for the linear system. Before that, we need to understand the basics of how to solve linear equations.
- link: Variable Elimination, Smoothing, and Marginalization Explained
- Introduce Variable Elimination.
- Explain the Schur Complement and Marginalization in the perspective of Variable Elimination.
- Explain Fixed-lag smoothing by an example.
- Implement the fixed-lag smoothing and show it’s the same as solving the Normal Equation.
- Discuss an IROS 2018 best paper candidate, which discussed how to do the marginalization novelty.
- Some discussion of the general Variable Elimination and it’s an application in ISAM2.
- In this section, we will apply the Schur Complement to solve the rocket landing problem faster.
- Prove that eliminating variables in index order solves a block-diagonal linear system of equations in \(O(n)\) time.
- Show that the KKT conditions for the constrained rocket landing problem can be converted to a block-diagonal system.
- Implement a new solver which uses these techniques in C++.
- Discuss a famous paper which used a similar trick to exploit the structure of a MPC problem.
Cite
Most of the work is not original. But the explanation and interpretation are. It will be fun if you can cite this series of articles.
@misc{Least Squares Robotics,
author = "Yimu Wang",
title = " Least Squares Robotics ",
howpublished = "\url{https://wang-yimu.com/least-squares-robotics/}",
}