|
Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Dynamic parametric step adaptation for the GVF algorithm. More...
Include dependency graph for gvf_adapted_step.c:Go to the source code of this file.
Macros | |
| #define | ADAPTED_STEP_NULL_TOLERANCE 1e-6 |
| #define | ADAPTED_STEP_MAX_ROOTFINDING_STEPS 1e6 |
Functions | |
| static float | p4_eval (float x, float a4, float a3, float a2, float a1, float a0) |
| Horner's method for fast degree 4 polynomial evaluation. | |
| static float | p4_halley (float a4, float a3, float a2, float a1, float a0, float tol, float init, int max_steps) |
| Implementation of Halley's method for degree 4 polynomial root finding. | |
| float | step_adaptation (float ds, float f1d, float f2d, float f3d, float f1dd, float f2dd, float f3dd) |
| Compute the adapted parametric step given the wanted geometric distance. | |
Dynamic parametric step adaptation for the GVF algorithm.
Definition in file gvf_adapted_step.c.
Definition at line 40 of file gvf_adapted_step.c.
| #define ADAPTED_STEP_NULL_TOLERANCE 1e-6 |
Definition at line 35 of file gvf_adapted_step.c.
Horner's method for fast degree 4 polynomial evaluation.
Cf https://en.wikipedia.org/wiki/Horner's_method
| x | : Evaluation point for the polynomial |
| a4 | : Degree 4 coefficient of the polynomial |
| a3 | : Degree 3 coefficient of the polynomial |
| a2 | : Degree 2 coefficient of the polynomial |
| a1 | : Degree 1 coefficient of the polynomial |
| a0 | : Degree 0 coefficient of the polynomial |
x Definition at line 56 of file gvf_adapted_step.c.
References foo.
Referenced by p4_halley().
Here is the caller graph for this function:
|
static |
Implementation of Halley's method for degree 4 polynomial root finding.
Cf https://en.wikipedia.org/wiki/Halley%27s_method
| a4 | : Degree 4 coefficient of the polynomial |
| a3 | : Degree 3 coefficient of the polynomial |
| a2 | : Degree 2 coefficient of the polynomial |
| a1 | : Degree 1 coefficient of the polynomial |
| a0 | : Degree 0 coefficient of the polynomial |
| tol | : Tolerance; the root is found if the absolute value of the polynomial evaluation is below the tolerance |
| init | : Initial guess for the root |
| max_steps | : Maximal number of iterations allowed |
max_steps iteration) Definition at line 76 of file gvf_adapted_step.c.
References foo, init, p4_eval(), and step.
Referenced by step_adaptation().
Here is the call graph for this function:
Here is the caller graph for this function:| float step_adaptation | ( | float | ds, |
| float | f1d, | ||
| float | f2d, | ||
| float | f3d, | ||
| float | f1dd, | ||
| float | f2dd, | ||
| float | f3dd | ||
| ) |
Compute the adapted parametric step given the wanted geometric distance.
Given the 3D parametric curve described by f at p and a wanted geometric distance ds, computes the parametric step dp such that: |ds| = || f(p+dp) - f(p) || and (ds)(dp) > 0
To do so, it uses second order Taylor approximation around p to reduce the problem to finding a root of a degree 4 polynomial. This root is found using Halley's method.
We assume the curve non 2-singular (that is both f' and f'' does not vanish). (If it is the case, then the method falls back to returning dp = ds, and print an error to stderr).
| ds | Wanted geometric distance |
| f1d | x value of f'(p) |
| f2d | y value of f'(p) |
| f3d | z value of f'(p) |
| f1dd | x value of f''(p) |
| f2dd | y value of f''(p) |
| f3dd | z value of f''(p) |
Definition at line 108 of file gvf_adapted_step.c.
References ADAPTED_STEP_MAX_ROOTFINDING_STEPS, ADAPTED_STEP_NULL_TOLERANCE, foo, init, and p4_halley().
Referenced by gvf_parametric_control_2D().
Here is the call graph for this function:
Here is the caller graph for this function: