This module implements optical flow landings in which the divergence is kept constant. More...

#include "std.h"

Include dependency graph for optical_flow_landing.h:

This graph shows which files directly or indirectly include this file:

Data Structures
struct	OpticalFlowLanding

Macros
#define	MAX_SAMPLES_LEARNING 25000

Functions
void	optical_flow_landing_init (void)
	Initialize the optical flow landing module. More...

void	guidance_module_enter (void)
	Entering the module (user switched to module) More...

void	guidance_module_run (bool in_flight)
	Main guidance loop. More...

void	save_texton_distribution (void)

void	load_texton_distribution (void)

void	fit_linear_model_OF (float targets, float samples, uint8_t D, uint16_t count, float parameters, float *fit_error)
	Fit a linear model from samples to target values. More...

void	learn_from_file (void)

float	predict_gain (float *distribution)

void	save_weights (void)

void	load_weights (void)

void	recursive_least_squares_batch (float targets, float samples, uint8_t D, uint16_t count, float params, float *fit_error)
	Recursively fit a linear model from samples to target values - batch mode, possibly for initialization. More...

void	recursive_least_squares (float target, float sample, uint8_t length_sample, float params)

Variables
struct OpticalFlowLanding	of_landing_ctrl

Detailed Description

This module implements optical flow landings in which the divergence is kept constant.

When using a fixed gain for control, the covariance between thrust and divergence is tracked, so that the drone knows when it has arrived close to the landing surface. Then, a final landing procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to the surface.

de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy. Bioinspiration & biomimetics, 11(1), 016004. http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004

Based on the above theory, we have also developed a new strategy for landing that consists of two phases: (1) while hovering, the drone determines the optimal gain by increasing the gain until oscillation (2) the drone starts landing while exponentially decreasing the gain over time

This strategy leads to smooth, high-performance constant divergence landings, as explained in the article: H.W. Ho, G.C.H.E. de Croon, E. van Kampen, Q.P. Chu, and M. Mulder (submitted) Adaptive Control Strategy for Constant Optical Flow Divergence Landing, https://arxiv.org/abs/1609.06767

The module also contains code for learning to map the visual appearance of the landing surface with self-supervised learning (SSL) to the corresponding control gains. This leads to smooth optical flow landings where the "height" expressed in the control gain is known and can be used to shut down the motors. This has been published in the following paper:

de Croon, Guido CHE, Christophe De Wagter, and Tobias Seidl. "Enhancing optical-flow-based control by learning visual appearance cues for flying robots." Nature Machine Intelligence 3.1 (2021): 33-41.

Finally, horizontal flow control has been added, so that the drone can hover without optitrack. In fact, this module also formed the basis for the experiments in which the attitude is estimated with only optical flow and gyro measurements (without accelerometers), published in Nature:

De Croon, G.C.H.E., Dupeyroux, J.J., De Wagter, C., Chatterjee, A., Olejnik, D. A., & Ruffier, F. (2022). Accommodating unobservability to control flight attitude with optic flow. Nature, 610(7932), 485-490.

Definition in file optical_flow_landing.h.

Data Structure Documentation

◆ OpticalFlowLanding

struct OpticalFlowLanding

Definition at line 52 of file optical_flow_landing.h.

Data Fields
uint32_t	active_motion	Method for actively inducing motion.
float	agl	agl = height from sonar (only used when using "fake" divergence)
float	agl_lp	low-pass version of agl
float	close_to_edge	if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored for learning
uint32_t	CONTROL_METHOD	type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control. 3 = learning control
float	cov_limit	for fixed gain control, what is the cov limit triggering the landing
uint32_t	COV_METHOD	method to calculate the covariance: between thrust and div (0) or div and div past (1)
float	cov_set_point	for adaptive gain control, setpoint of the covariance (oscillations)
float	d_err	difference of error for the D-gain
uint32_t	delay_steps	number of delay steps for div past
float	dgain	D-gain for constant divergence control.
float	dgain_adaptive	D-gain for adaptive gain control.
float	divergence	Divergence estimate.
float	divergence_setpoint	setpoint for constant divergence approach
bool	elc_oscillate	Whether or not to oscillate at beginning of elc to find optimum gain.
float	front_div_threshold	Threshold when the front div gets above this value, it will command a horizontal stop.
float	igain	I-gain for constant divergence control.
float	igain_adaptive	I-gain for adaptive gain control.
float	igain_horizontal_factor	factor multiplied with the vertical I-gain for horizontal ventral-flow-based control
volatile bool	learn_gains	set to true if the robot needs to learn a mapping from texton distributions to the p-gain
bool	load_weights	load the weights that were learned before
float	lp_const	low-pass value for divergence
float	lp_cov_div_factor	low-pass factor for the covariance of divergence in order to trigger the second landing phase in the exponential strategy.
float	lp_factor_prediction	low-pass value for the predicted P-value
float	nominal_thrust	nominal thrust around which the PID-control operates
float	omega_FB	Set point for the front-back ventral flow.
float	omega_LR	Set point for the left-right ventral flow.
float	p_land_threshold	if during the exponential landing the gain reaches this value, the final landing procedure is triggered
float	pgain	P-gain for constant divergence control (from divergence error to thrust)
float	pgain_adaptive	P-gain for adaptive gain control.
float	pgain_horizontal_factor	factor multiplied with the vertical P-gain for horizontal ventral-flow-based control
float	pitch_trim	Pitch trim angle in degrees.
float	previous_err	Previous divergence tracking error.
float	ramp_duration	ramp duration in seconds for when the divergence setpoint changes
float	reduction_factor_elc	reduction factor - after oscillation, how much to reduce the gain...
float	roll_trim	Roll trim angle in degrees.
bool	snapshot	if true, besides storing a texton distribution, an image will also be stored (when unstable)
float	sum_err	integration of the error for I-gain
float	t_transition	how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain
bool	use_bias	if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads to very large weights
float	vel	vertical velocity as determined with sonar (only used when using "fake" divergence)
uint32_t	VISION_METHOD	whether to use vision (1) or Optitrack / sonar (0)
uint32_t	window_size	number of time steps in "window" used for getting the covariance

Macro Definition Documentation

◆ MAX_SAMPLES_LEARNING

#define MAX_SAMPLES_LEARNING 25000

Definition at line 50 of file optical_flow_landing.h.

Function Documentation

◆ fit_linear_model_OF()

void fit_linear_model_OF	(	float *	targets,
		float **	samples,
		uint8_t	D,
		uint16_t	count,
		float *	params,
		float *	fit_error
	)

Fit a linear model from samples to target values.

Parameters

[in]	targets	The target values
[in]	samples	The samples / feature vectors
[in]	D	The dimensionality of the samples
[in]	count	The number of samples
[out]	parameters*	Parameters of the linear fit
[out]	fit_error*	Total error of the fit

Definition at line 1415 of file optical_flow_landing.c.

References D, MAKE_MATRIX_PTR, MAT_MUL, MAT_SUB, n_samples, of_landing_ctrl, parameters, pprz_svd_float(), pprz_svd_solve_float(), and OpticalFlowLanding::use_bias.

Referenced by learn_from_file().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_module_enter()

void guidance_module_enter ( void )

Entering the module (user switched to module)

Definition at line 77 of file ctrl_module_innerloop_demo.c.

◆ guidance_module_run()

void guidance_module_run ( bool in_flight )

Main guidance loop.

Parameters

[in] in_flight Whether we are in flight or not

Definition at line 82 of file ctrl_module_innerloop_demo.c.

◆ learn_from_file()

void learn_from_file ( void )

Definition at line 1251 of file optical_flow_landing.c.

References fit_linear_model_OF(), gains, load_texton_distribution(), n_read_samples, n_textons, RECURSIVE_LEARNING, recursive_least_squares_batch(), save_weights(), text_dists, and weights.

Referenced by vertical_ctrl_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ load_texton_distribution()

void load_texton_distribution ( void )

Definition at line 1213 of file optical_flow_landing.c.

References cov_divs_log, distribution_logger, gains, MAX_SAMPLES_LEARNING, n_read_samples, n_textons, sonar_OF, text_dists, and TEXTON_DISTRIBUTION_PATH.

Referenced by learn_from_file().

Here is the caller graph for this function:

◆ load_weights()

void load_weights ( void )

Definition at line 1521 of file optical_flow_landing.c.

References n_textons, TEXTON_DISTRIBUTION_PATH, weights, and weights_file.

Referenced by vertical_ctrl_module_run().

Here is the caller graph for this function:

◆ optical_flow_landing_init()

void optical_flow_landing_init ( void )

Initialize the optical flow landing module.

Definition at line 334 of file optical_flow_landing.c.

Here is the call graph for this function:

◆ predict_gain()

float predict_gain ( float * distribution )

Definition at line 1486 of file optical_flow_landing.c.

References n_textons, of_landing_ctrl, OpticalFlowLanding::use_bias, and weights.

Referenced by recursive_least_squares(), recursive_least_squares_batch(), and vertical_ctrl_module_run().

Here is the caller graph for this function:

◆ recursive_least_squares()

void recursive_least_squares	(	float	target,
		float *	sample,
		uint8_t	length_sample,
		float *	params
	)

◆ recursive_least_squares_batch()

void recursive_least_squares_batch	(	float *	targets,
		float **	samples,
		uint8_t	D,
		uint16_t	count,
		float *	params,
		float *	fit_error
	)

Recursively fit a linear model from samples to target values - batch mode, possibly for initialization.

Parameters

[in]	targets	The target values
[in]	samples	The samples / feature vectors
[in]	D	The dimensionality of the samples
[in]	count	The number of samples
[out]	parameters*	Parameters of the linear fit
[out]	fit_error*	Total error of the fit // TODO: relevant for RLS?