/nearness_estimation

This repository estimates nearness (inverse of the distance) using planar tangential optic flow, radar body velocities and IMU angular rates.

Primary LanguageC++

Nearness Estimation

This repository estimates 2D nearness (inverse of the distance) using a Kalman Filter to fuse optic flow and radar measurements.

The Kalman Filter works at 10 Hz, which is the slowest sensor rate (radar). All the matrix operations are done manually using a single for loop. We can do this due to the fact that we know the shape of all matrices before hand. This allows us to run the filter real-time.

The optic flow data is planar and tangential and is computed from an image distorted by a parabolic mirror. It uses Lucas-Kanade pyramidal algorithm and is filtered through a low-pass filter. Note that the quality of optic flow measurements depend on the texture of the environment.

The radar data is filtered by its intensity, range and height. The height filtering strictly depends on the radar location in the robot. For instance, if you do not want to sense the floor, you will need to filter the altitude depending on the height of the radar. Radar data can read through walls and output some outliers. We will implement an outlier removal after converting the 3D pointcloud data into a 2D laserscan. This allows us to handle better the outliers. We remove them by projecting the previous nearness estimate with the robot dynamics into the next time step. This relies on the fact that radar outliers do not appear twice.

A Lowess filter is used to smooth out the data. The algorithm performs locally-weighted polynomial regression in two dimensions.

Installation

The Nearness Estimation package depends on the following libraries and additional packages.

Libraries

  • Eigen3

ROS Packages

Nodes

kf_nearness_node

This node uses a Kalman Filter to fuse data and is thoroughly described above.

roslaunch nearness_estimation kf_nearness.launch

simple_nearness_node

This is a simple version that uses the motion parallax equations to derive the nearness from optic flow measurements. It runs at 30 Hz which is the publishing rate of the measured optic flow. It does not use radar.

roslaunch nearness_estimation simple_nearness.launch

Kalman Filter Node

Subscribed topics

Published topics

Parameters

~ covariance_process (double, default: 100): process noise covariance diagonal values.

~ covariance_optic_flow (double, default: 5): covariance noise diagonal values of the radar measurement set.

~ covariance_radar (double,default: 0.001): covariance noise diagonal values of the optic flow measurement set.

~ threshold (double, default: 0.3): threshold to remove radar outliers.

~ minimum_velocity (double, default: 0.4): minimum velocity required to initialize the filter.

~ lowess_smoothness (double, default: 0.2): smoothness parameter f of the Lowess filter.