An observer cascade for velocity and multiple line estimation

TL;DR

This paper introduces a novel multi-line incremental estimation approach using an observer cascade that exploits environment structure and integrates IMU data to improve velocity and line mapping accuracy.

Contribution

It presents the first multi-line incremental estimation method with a reduced state space and stability analysis, integrating IMU data for improved robustness.

Findings

Reduced state space from 4N to 3N+3 for structured environments

Cascade observer is asymptotically stable and converges in simulations

Coupling IMU with the observer improves velocity estimation accuracy

Abstract

Previous incremental estimation methods consider estimating a single line, requiring as many observers as the number of lines to be mapped. This leads to the need for having at least $4N$ state variables, with $N$ being the number of lines. This paper presents the first approach for multi-line incremental estimation. Since lines are common in structured environments, we aim to exploit that structure to reduce the state space. The modeling of structured environments proposed in this paper reduces the state space to $3N + 3$ and is also less susceptible to singular configurations. An assumption the previous methods make is that the camera velocity is available at all times. However, the velocity is usually retrieved from odometry, which is noisy. With this in mind, we propose coupling the camera with an Inertial Measurement Unit (IMU) and an observer cascade. A first observer retrieves the scale of the linear velocity and a second observer for the lines mapping. The stability of the entire system is analyzed. The cascade is shown to be asymptotically stable and shown to converge in experiments with simulated data.