An Invariant Linear Quadratic Gaussian controller for a simplified car

TL;DR

This paper introduces the invariant Linear Quadratic Gaussian (ILQG) controller for a simplified car model, leveraging symmetries to improve robustness and reduce sensitivity to trajectory estimation errors, especially under high noise conditions.

Contribution

The paper presents a novel ILQG controller that exploits problem symmetries, enhancing robustness and reducing gain tuning dependency compared to traditional LQG.

Findings

ILQG outperforms LQG under large noise and initial uncertainties

Reduced gain tuning sensitivity to trajectory estimates

Potential benefits for motion planning applications

Abstract

In this paper, we consider the problem of tracking a reference trajectory for a simplified car model based on unicycle kinematics, whose position only is measured, and where the control input and the measurements are corrupted by independent Gaussian noises. To tackle this problem we devise a novel observer-controller: the invariant Linear Quadratic Gaussian controller (ILQG). It is based on the Linear Quadratic Gaussian controller, but the equations are slightly modified to account for, and to exploit, the symmetries of the problem. The gain tuning exhibits a reduced dependency on the estimated trajectory, and is thus less sensitive to misestimates. Beyond the fact the invariant approach is sensible (there is no reason why the controller performance should depend on whether the reference trajectory is heading west or south), we show through simulations that the ILQG outperforms the conventional LQG controller in case of large noises or large initial uncertainties. We show that those robustness properties may also prove useful for motion planning applications.