Rigid Body Pose Estimation based on the Lagrange-d'Alembert Principle

Maziar Izadi; Amit Kumar Sanyal

Home/Archive/arXiv

arXiv · 1508.07671

Rigid Body Pose Estimation based on the Lagrange-d'Alembert Principle

Maziar Izadi·Amit Kumar Sanyal

View original source ↗Read source document ↗Export metadata ↓

Abstract

Stable estimation of rigid body pose and velocities from noisy measurements, without any knowledge of the dynamics model, is treated using the Lagrange-d'Alembert principle from variational mechanics. With body-fixed optical and inertial sensor measurements, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in velocity estimation error and the sum of two artificial potential functions; one obtained from a generalization of Wahba's function for attitude estimation and another which is quadratic in the position estimate error. An additional dissipation term that is linear in the velocity estimation error is introduced, and the Lagrange-d'Alembert principle is applied to the Lagrangian with this dissipation. A Lyapunov analysis shows that the state estimation scheme so obtained provides stable asymptotic convergence of state estimates to actual states in the absence of measurement noise, with an almost global domain of attraction. This estimation scheme is discretized for computer implementation using discrete variational mechanics, as a first order Lie group variational integrator. The continuous and discrete pose estimation schemes require optical measurements of at least three inertially fixed landmarks or beacons in order to estimate instantaneous pose. The discrete estimation scheme can also estimate velocities from such optical measurements. Moreover, all states can be estimated during time periods when measurements of only two inertial vectors, the angular velocity vector, and one feature point position vector are available in body frame. In the presence of bounded measurement noise in the vector measurements, numerical simulations show that the estimated states converge to a bounded neighborhood of the actual states.

Explore related subjects

math.OC

Keep this discovery

Explore connections, maps & timelines

BibTeX RIS

Maziar Izadi, Amit Kumar Sanyal. 2015-09-15. Rigid Body Pose Estimation based on the Lagrange-d'Alembert Principle. https://arxiv.org/abs/1508.07671

Cite the original work for its findings. Save a collection to share your selection of sources.

KEEP EXPLORING

Related papers

arXiv2026

AdGT: Decentralized Gradient Tracking with Adaptive Per-Agent Stepsizes

In decentralized optimization, gradient-tracking methods typically rely on a single global stepsize. This choice can be conservative when agents have local objectives with different smoothness constants, since the stepsize must remain stable for the agent with the largest smoothness constant. This paper proposes AdGT, a decentralized gradient-tracking method in which each agent adapts its own stepsize using local gradient variation and a single global safety factor. The method reduces fixed-stepsize tuning effort and allows agents to exploit local smoothness information during the iterations. For smooth and strongly convex local objectives over undirected networks, we prove that the analyzed AdGT update converges linearly to the exact consensus optimizer. We also study two adaptive stepsize updates that use changes in the gradient-tracking direction. We characterize when the corresponding candidate determines the stepsize and prove conditional lower and upper stepsize bounds and linear convergence under an additional relative tracking-disagreement condition. Experiments on logistic regression, ridge regression, synthetic quadratic problems, and a linear-regression benchmark against state-of-the-art decentralized solvers show that AdGT often reaches a given accuracy in fewer iterations or gradient evaluations than tuned fixed-stepsize GT and the tested baselines, especially under heterogeneous local smoothness. In the topology experiments, each tested AdGT update uses one common safety factor across all graphs, whereas the fixed GT stepsize is tuned separately for each graph and seed.

math.OC↗

arXiv2026

Brockett cost function for symplectic eigenvalues

The sum of symplectic eigenvalues and corresponding eigenvectors of symmetric positive-definite matrices in the sense of Williamson's theorem can be computed via minimization of a trace cost function under the symplecticity constraint. Optimal solutions to this problem only offer a symplectic basis for the symplectic eigenspace corresponding to the sought symplectic eigenvalues. In this note, we introduce a Brockett cost function and investigate its properties and the connection with the symplectic eigenvalues and eigenvectors of the considered matrix. Specifically, we prove that any stationary point consists of symplectic eigenvectors, characterize the saddle points and global minimizers based on which the trace minimization theorem for the symplectic eigenvalues is re-established, and the nonexistence of local nonglobal minimizers is justified.

math.OC↗

arXiv2026

Riemannian Bilevel Optimization with Gradient Aggregation

We study bilevel optimization on Riemannian manifolds when the lower-level solution set is a positive-dimensional submanifold, so that implicit differentiation fails. We propose Riemannian Bilevel Descent Aggregation (RBDA), which extends bilevel descent aggregation to manifolds. Its inner loop aggregates the lower-level descent direction with the upper-level gradient under a decaying multiplier, and its hypergradient is the reverse-mode derivative of the unrolled loop. Under geodesic convexity and quadratic growth of the lower level, the inner iterates converge to a point of the optimistic solution set at a polynomial rate. Approximate minimizers of the objective with a finite number of inner iterations converge to minimizers of the optimistic value. In the experiments RBDA selects the optimistic solution where the implicit and unrolled estimators remain at the initial point or stop at a larger query loss. While each of its outer steps costs more than that of the unrolled estimator, it attains the highest test accuracy in data hyper-cleaning.

math.OC↗