smooth_feedback: Control and estimation on Lie groups

Tool collection for control and estimation on Lie groups leveraging the smooth library.

• Requirements: C++20, Eigen 3.4, boost::numeric::odeint, smooth
• Documentation

Control on Lie groups

These controllers are implemented for systems with dynamics on the form where T is a smooth::feedback::Time, X is a smooth::LieGroup, and U is a smooth::Manifold.

Nonlinearities are handled via linearization around a reference point or trajectory. For group-linear dynamics this automatically results in a linear system in the tangent space, in which case these algorithms are expected to work very well. Linearization is done via automatic differentiation. For this to work with the most performant methods (e.g. autodiff) the functions must be templated on the scalar type. The dynamical system

can be defined via a lambda function that supports automatic differentiation as follows:

#include <Eigen/Core>
 
#include <smooth/bundle.hpp>
#include <smooth/se2.hpp>
 
#include <chrono>
 
using T = std::chrono::duration<double>;
template<typename S>
using X = smooth::Bundle<smooth::SE2<S>, Eigen::Matrix<S, 3, 1>>;
template<typename S>
using U = Eigen::Matrix<S, 2, 1>;
 
const Eigen::Matrix3d A{
  {-0.2, 0, 0},
  {0, 0, 0},
  {0, 0, -0.4},
};
const Eigen::Matrix<double, 3, 2> B{
  {1, 0},
  {0, 0},
  {0, 1},
};
 
auto Sigma = []<typename S>(T, const X<S> & x, const U<S> & u) -> smooth::Tangent<X<S>> {
  smooth::Tangent<X<S>> dx_dt;
  dx_dt.head(3) = x.template part<1>();
  dx_dt.tail(3) = A * x.template part<1>() + B * u;
  return dx_dt;
};

PID Control

• Model-free
• Assumes that inputs control body acceleration. See examples/pid_se2.cpp for an example of allocating PID inputs to actuators.

Example PID controller on SE(2)

#include <smooth/feedpack/pid.hpp>
 
smooth::feedback::PID<T, smooth::SE2d> pid;
 
// set desired motion
pid.set_xdes([](T T) -> std::tuple<smooth::SE2d, Eigen::Vector3d, Eigen::Vector3d> {
  return {
    smooth::SE2d::Identity(),  // position
    Eigen::Vector3d::Zero(),   // velocity (right derivative of position w.r.t. t)
    Eigen::Vector3d::Zero(),   // acceleration (second right derivative of position w.r.t. t)
  };
});
 
T t               = T(1);                       // current time
smooth::SE2d x    = smooth::SE2d::Random();     // current state
Eigen::Vector3d v = Eigen::Vector3d::Random();  // current body velocity
 
Eigen::Vector3d u = pid(t, x, v);

Model-Predictive Control

• Automatic linearization and time discretization of nonlinear continuous dynamics
• Define state and input reference trajectories via arbitrary functions T -> X and T -> U for a time type T. The bus in the video above uses MPC to track the boundary of the circle.

Example: Model-predictive control for the system Sigma (see also examples/mpc_asif_vehicle.cpp)

#include <smooth/feedback/mpc.hpp>
 
smooth::feedback::MPC<T, X<double>, U<double>, decltype(Sigma)> mpc(Sigma, {.T = 5, .K = 50});
 
// set desired input and state trajectories
mpc.set_udes([]<typename S>(S t) -> U<S> { return U<S>::Zero(); });
mpc.set_xdes([]<typename S>(S t) -> X<S> { return X<S>::Identity(); });
 
T t(0);
X<double> x = X<double>::Identity();
 
// get control input for time t and state x
auto [u, code] = mpc(t, x);

Active Set Invariance Filtering (ASIF)

• Minimally invasive filtering of a control input in order to enforce state constraints. The bus in the video above is using an ASIF that avoids the red cylinder.
• Automatic differentiation of nonlinear continuous dynamics and constraints
• Theory (non-Lie group case) is described in e.g. Thomas Gurriet's Ph.D. thesis

Example: Safety filtering for the system Sigma

#include <smooth/feedback/asif.hpp>
 
smooth::feedback::ASIFilter<T, X<double>, U<double>, decltype(Sigma)> asif(Sigma);
 
// safety set S(t) = { x : h(t, x) >= 0 }
auto h = []<typename S>(S, const X<S> & x) -> Eigen::Matrix<S, 1, 1> {
  return Eigen::Matrix<S, 1, 1>(x.template part<0>().r2().x() - S(0.2));
};
 
// backup controller
auto bu = []<typename S>(S, const X<S> &) -> U<S> { return U<S>(1, 1); };
 
T t             = T(1);
X<double> x     = X<double>::Random();
U<double> u_des = U<double>::Zero();
 
// get control input for time t, state x, and reference input u_des
auto [u_asif, code] = asif(t, x, u_des, h, bu);

Estimation on Lie groups

Estimators take system models on the form where X is a smooth::LieGroup, and measurements on the form .

To use in a feedback loop for a controlled system use partial application:

// variable that holds current input
U<double> u = U<double>::Random();
// closed-loop dynamics (time type must be Scalar<X>)
auto SigmaCL = [&u]<typename S>(double t, const X<S> & x) -> smooth::Tangent<X<S>> {
  return Sigma(T(t), x, u.template cast<S>());
};

Extended Kalman Filter

• Templated over dynamics and measurement models
• Automatic differentiation
• Reduces to standard Kalman filter (KF) for linear models on Rn
• Reduces to Invariant Extended Kalman Filter (IEKF) for group-linear models on Lie groups

Example: localization with a known 2D landmark for the system SigmaCL

#include <smooth/feedback/ekf.hpp>
 
// create filter
smooth::feedback::EKF<X<double>> ekf;
 
// measurement model
Eigen::Vector2d landmark(1, 1);
auto h = [&landmark]<typename S>(const X<S> & x) -> Eigen::Matrix<S, 2, 1> {
  return x.template part<0>().inverse() * landmark;
};
 
// PREDICT STEP: propagate filter over time
ekf.predict(SigmaCL,
  Eigen::Matrix<double, 6, 6>::Identity(),  // motion covariance Q
  1.                                        // time step length
);
 
// UPDATE STEP: register a measurement of the known landmark
ekf.update(h,
  Eigen::Vector2d(0.3, 0.6),   // measurement result y
  Eigen::Matrix2d::Identity()  // measurement covariance R
);
 
// access estimate
auto x_hat = ekf.estimate();
auto P_hat = ekf.covariance();

Optimization

MPC and ASIF relies on online quadratic program optimization.

Fast QP Solver

• Eigen-native port of the operator splitting QP solver.
• Solves both dense and sparse problems.
• Eigen lazy evaluations enable fast SIMD in the compiled assembly.

The plot below compares solution times (lower is better) for random square QPs over three different levels of sparsity. The results suggest that the dense solver is the best choice except for problems that are both large and very sparse. Performance is however highly problem-dependent and should ideally be evaluated on a per-application basis.

The results are generated from the benchmarking program in benchmark/.

Example: Define and solve a dynamically sized dense quadratic program.

#include <smooth/feedback/qp.hpp>
 
// define the QP
//  min 0.5 x' * P * x + q' * x
//  s.t l <= A * x <= u
smooth::feedback::QuadraticProgram<-1, -1> qp{
  .P = P,  // n x n matrix
  .q = q,  // n x 1 vector
  .A = A,  // m x n matrix
  .l = l,  // m x 1 vector
  .u = u,  // m x 1 vector
};
 
smooth::feedback::QPSolverParams prm{};
auto sol = smooth::feedback::solve_qp(qp, prm);