4,707 research outputs found
Batch Nonlinear Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression
In this paper, we revisit batch state estimation through the lens of Gaussian
process (GP) regression. We consider continuous-discrete estimation problems
wherein a trajectory is viewed as a one-dimensional GP, with time as the
independent variable. Our continuous-time prior can be defined by any
nonlinear, time-varying stochastic differential equation driven by white noise;
this allows the possibility of smoothing our trajectory estimates using a
variety of vehicle dynamics models (e.g., `constant-velocity'). We show that
this class of prior results in an inverse kernel matrix (i.e., covariance
matrix between all pairs of measurement times) that is exactly sparse
(block-tridiagonal) and that this can be exploited to carry out GP regression
(and interpolation) very efficiently. When the prior is based on a linear,
time-varying stochastic differential equation and the measurement model is also
linear, this GP approach is equivalent to classical, discrete-time smoothing
(at the measurement times); when a nonlinearity is present, we iterate over the
whole trajectory to maximize accuracy. We test the approach experimentally on a
simultaneous trajectory estimation and mapping problem using a mobile robot
dataset.Comment: Submitted to Autonomous Robots on 20 November 2014, manuscript #
AURO-D-14-00185, 16 pages, 7 figure
Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation
Many algorithms for control, optimization and estimation in robotics depend
on derivatives of the underlying system dynamics, e.g. to compute
linearizations, sensitivities or gradient directions. However, we show that
when dealing with Rigid Body Dynamics, these derivatives are difficult to
derive analytically and to implement efficiently. To overcome this issue, we
extend the modelling tool `RobCoGen' to be compatible with Automatic
Differentiation. Additionally, we propose how to automatically obtain the
derivatives and generate highly efficient source code. We highlight the
flexibility and performance of the approach in two application examples. First,
we show a Trajectory Optimization example for the quadrupedal robot HyQ, which
employs auto-differentiation on the dynamics including a contact model. Second,
we present a hardware experiment in which a 6 DoF robotic arm avoids a randomly
moving obstacle in a go-to task by fast, dynamic replanning
- …