FactorMPC: Manifold Graph-Based MPC Toolkit

• FactorMPC is a factor-graph based toolkit designed for MPC on nonlinear manifolds, effectively addressing singularities and over-parameterization issues.
• It integrates modular factors for dynamics, reference tracking, and safety-critical obstacle avoidance using CBF and vCBF, ensuring robust control.
• The framework leverages tangent-space Gaussian uncertainty and sparse nonlinear optimization, demonstrating superior tracking performance and real-time operation in quadrotor experiments.

Searching arXiv for the cited FactorMPC paper and closely related MPC formulation papers. FactorMPC is a factor-graph based model predictive control toolkit for systems evolving on nonlinear manifolds, introduced in “Integrated Planning and Control on Manifolds: Factor Graph Representation and Toolkit” (Yang et al., 5 Oct 2025). It is designed for settings in which Euclidean MPC formulations struggle with singularities, over-parameterization, and poor convergence, particularly for robotic attitude dynamics and constrained motion planning. The framework unifies system dynamics, constraints, and objectives in a modular optimization structure, natively supports manifold-valued states, represents Gaussian uncertainties in tangent spaces, and incorporates control barrier function factors for safety-critical obstacle avoidance. In the reported quadrotor simulations and experiments, it demonstrates smaller RMS tracking error than a differential-flatness baseline and maintains real-time operation at a 100 Hz control rate (Yang et al., 5 Oct 2025).

1. Problem setting and design objective

FactorMPC addresses MPC problems in which the state evolves on a manifold $\mathcal M$ rather than in a purely Euclidean vector space. The formulation uses manifold-valued states $x_k \in \mathcal M$ for $k=0,\dots,N$ and Euclidean controls $u_k \in \mathbb R^m$. The stated motivation is that traditional Euclidean formulations encounter difficulty on nonlinear manifolds because of singularities, over-parameterization, and poor convergence, whereas a manifold-consistent treatment can preserve geometric structure (Yang et al., 5 Oct 2025).

The framework is organized as a factor graph. Variable nodes correspond to states and controls, while factor nodes encode dynamics, reference tracking, control regularization, control bounds, and obstacle-avoidance constraints. This organization is explicitly modular: factors such as $\phi_k^{\rm D}$, $\phi_k^{\rm ref}$, $\phi_k^{\rm U}$, $\phi_k^{\rm B}$, $\phi_{i,k}^{\rm CBF}$, and $\phi_{i,k}^{\rm vCBF}$ can be added to the graph according to the problem specification (Yang et al., 5 Oct 2025).

A plausible implication is that the principal contribution of FactorMPC is not a new MPC objective in isolation, but a representation and software architecture that makes manifold-consistent integrated planning and control amenable to sparse nonlinear optimization. That interpretation is consistent with the paper’s emphasis on modularity, sparsity exploitation, and plug-and-play factors (Yang et al., 5 Oct 2025).

2. Factor-graph representation of the MPC problem

The core of FactorMPC is a maximum-a-posteriori optimization problem defined on a factor graph. The dynamics factor $x_k \in \mathcal M$0 enforces one-step discrete dynamics on $x_k \in \mathcal M$1, while cost factors encode state tracking and control regularization. Inequality factors represent actuator bounds and obstacle-avoidance conditions. In the summary provided for the paper, the full optimization is written as

$x_k \in \mathcal M$2

subject to

$x_k \in \mathcal M$3

This formulation combines hard model structure with penalty-based residual terms. The dynamics residual in general manifold form is given as

$x_k \in \mathcal M$4

with quadratic penalty

$x_k \in \mathcal M$5

For the quadrotor example, the state manifold is $x_k \in \mathcal M$6. The paper summary gives the stacked dynamics residual $x_k \in \mathcal M$7 in terms of translational and rotational increments, including $x_k \in \mathcal M$8, thereby making the manifold dependence explicit (Yang et al., 5 Oct 2025).

This factor-graph construction exploits the fact that each factor contributes a small Jacobian block, so the resulting Hessian is block-sparse and banded. The paper explicitly attributes computational efficiency to this sparsity structure together with the probabilistic interpretation of the graph (Yang et al., 5 Oct 2025).

3. Manifold geometry and uncertainty modeling

A defining feature of FactorMPC is that uncertainty is represented in tangent spaces rather than by imposing a Euclidean Gaussian directly on the manifold state. Process noise is modeled as $x_k \in \mathcal M$9 and injected through the local tangent-space update

$k=0,\dots,N$0

The framework states that uncertainty is always represented as a Gaussian in $k=0,\dots,N$1, and the Mahalanobis norm uses a covariance $k=0,\dots,N$2 in that tangent space (Yang et al., 5 Oct 2025).

For Lie groups, the retraction and inverse error operators are specified as

$k=0,\dots,N$3

and

$k=0,\dots,N$4

These operators provide the geometric machinery for expressing residuals, priors, and reference-tracking errors without reverting to over-parameterized Euclidean coordinates. This suggests that the framework is intended to preserve intrinsic geometry throughout the prediction horizon rather than only at the dynamics level. The same operators appear in the objective term $k=0,\dots,N$5, linking state estimation style residuals with MPC cost construction (Yang et al., 5 Oct 2025).

A common misconception in manifold control is that a manifold-aware formulation merely replaces one coordinate chart with another. FactorMPC is more specific: the representation of the state, the definition of residuals, and the Gaussian uncertainty model are all posed through manifold operators and tangent-space covariance structure. In the source material, this geometric consistency is one of the stated reasons for improved behavior relative to Euclidean formulations on nonlinear manifolds (Yang et al., 5 Oct 2025).

4. Safety-critical obstacle avoidance via CBF and vCBF factors

FactorMPC incorporates safety-critical obstacle avoidance directly into the factor graph through control barrier function factors. The safe set is defined by

$k=0,\dots,N$6

and the Lie-derivative safety condition is

$k=0,\dots,N$7

with $k=0,\dots,N$8 (Yang et al., 5 Oct 2025).

The distance-based CBF factor residual is

$k=0,\dots,N$9

The framework extends this construction with a velocity-extended CBF, denoted vCBF, which adds relative velocity along the obstacle normal $u_k \in \mathbb R^m$0. The corresponding function is

$u_k \in \mathbb R^m$1

and its residual is

$u_k \in \mathbb R^m$2

The summary states that the Jacobian of this residual with respect to $u_k \in \mathbb R^m$3 is derived in closed form. It also characterizes the velocity-extended, on-manifold CBF-based obstacle avoidance factors as intended for safety-critical applications (Yang et al., 5 Oct 2025).

In the reported quadrotor experiments, static cylinder and moving ball scenarios were used, and the quadrotor never violated $u_k \in \mathbb R^m$4. The paper summary describes this as guaranteed safety in those demonstrated scenarios. A careful reading suggests that the guarantee is tied to the modeled CBF conditions and the experimental setup reported in the paper, rather than constituting a universal guarantee for arbitrary modeling error or solver failure (Yang et al., 5 Oct 2025).

5. Solver architecture, software toolkit, and reported performance

FactorMPC is implemented on top of GTSAM. The solver uses GTSAM’s nonlinear Gauss–Newton on manifolds and iSAM2 for incremental updates, with Levenberg–Marquardt damping when factors become ill-conditioned, such as when $u_k \in \mathbb R^m$5 in rotation (Yang et al., 5 Oct 2025). The software is distributed as an open-source toolkit at:

https://github.com/RoboticsPolyu/FactorMPC

The implementation is described as plug-and-play. The provided summary states that each factor—such as DynamicsFactor, CBFactor, vCBFFactor, and BoundaryFactor—derives from NoiseModelFactor. The typical integration pattern is to create a NonlinearFactorGraph, add priors, dynamics factors, bound factors, control-rate factors, vCBF factors for obstacles, and reference factors, then optimize with a LevenbergMarquardtOptimizer (Yang et al., 5 Oct 2025).

$\phi_k^{\rm D}$7

To integrate a new robot, the summary lists four required specifications: the state manifold type and $u_k \in \mathbb R^m$6 operators, the discrete dynamics model $u_k \in \mathbb R^m$7, obstacle descriptions for CBF factors, and solver parameters such as horizon $u_k \in \mathbb R^m$8, weights $u_k \in \mathbb R^m$9, and related terms (Yang et al., 5 Oct 2025).

The reported real-time performance is explicit. The typical MPC loop runs at 100 Hz, and the factor-graph solve takes approximately $\phi_k^{\rm D}$0 ms on a laptop, with 99.6% of solves under $\phi_k^{\rm D}$1 ms even with CBFs active. In the quadrotor experiment summary, the average computation time is $\phi_k^{\rm D}$2 ms, the maximum is less than $\phi_k^{\rm D}$3 ms at a 100 Hz control rate, and the RMS tracking error over an eight-figure trajectory is smaller than the differential-flatness baseline (DFBC) (Yang et al., 5 Oct 2025).

6. Position within the MPC literature

FactorMPC belongs to a line of work that treats MPC as a structured optimization problem, but it differs from classical linear-quadratic MPC methods in both state representation and solver architecture. In a standard linear, discrete-time setting, the system is written as

$\phi_k^{\rm D}$4

with a quadratic horizon cost, and the dominant online computation is often the Newton-step solve via Riccati recursion (Nielsen et al., 2014). The paper “A Parallel Riccati Factorization Algorithm with Applications to Model Predictive Control” describes the classical backward Riccati recursion and a tree-based parallelization that reduces the serial $\phi_k^{\rm D}$5 factorization cost to $\phi_k^{\rm D}$6 parallel time when sufficient processing units are available (Nielsen et al., 2014).

This comparison clarifies the scope of FactorMPC. The Riccati-based framework in (Nielsen et al., 2014) is tailored to equality-constrained linear MPC with quadratic costs and exploits the special structure of that setting. FactorMPC, by contrast, is formulated for manifold-valued states, nonlinear dynamics, tangent-space Gaussian uncertainty, and inequality factors including distance-based CBF and velocity-extended vCBF terms (Yang et al., 5 Oct 2025). The two approaches therefore address different structural regimes of MPC.

A plausible implication is that FactorMPC should be understood less as a replacement for Riccati methods in their native linear-quadratic domain than as an alternative sparse optimization framework for problems where geometry and safety constraints dominate the modeling requirements. The source material directly supports the claim that its key advantages arise from manifold consistency, modular factors, and sparse nonlinear optimization rather than from a condensed linear-quadratic factorization strategy (Yang et al., 5 Oct 2025).

Within that framing, FactorMPC is notable for connecting graphical-model methodology with safety-critical MPC. The paper explicitly describes the work as bridging graphical models with safety-critical MPC, and the resulting framework is characterized as scalable and geometrically consistent for integrated planning and control (Yang et al., 5 Oct 2025).