amino  1.0-beta2
Lightweight Robot Utility Library
math.h File Reference

Numerical routines. More...

Go to the source code of this file.

Classes

struct  aa_ode_sol_opts
 Options for the differential equation solver. More...
 
struct  aa_sys_affine_t
 Context-struct for function aa_sys_affine. More...
 

Macros

#define AA_EPSILON   .001
 a small number
 
#define AA_MAX(a, b)
 maximum of a and b More...
 
#define AA_MIN(a, b)
 minimum of a and b More...
 
#define AA_MODULO(a, b)   (((a) % (b)) + (b)) % (b);
 Fortran modulo, Ada mod.
 
#define AA_MATCOL(A, lda, col)   ((A)+(col)*(lda))
 Pointer to a column of a matrix. More...
 
#define AA_MATREF(A, lda, row, col)   (AA_MATCOL(A,lda,col)[row])
 Reference an element in a column-major matrix. More...
 
#define AA_ODE_EULER   AA_ODE_RK1
 Alias for Runge-Kutta-1 integration.
 
#define AA_ODE_HEUN   AA_ODE_RK2
 Alias for Runge-Kutta-2 integration.
 

Typedefs

typedef void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)
 A "Signal" function. More...
 
typedef int aa_ode_check(void *cx, double t, double *AA_RESTRICT x, double *AA_RESTRICT y)
 Function signature to check whether a differential equation has reached its goal. More...
 
typedef void aa_odestep_fixed(size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Function signature for a fixed integration step.
 
typedef void aa_odestep_adaptive(size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x_n_1, double *AA_RESTRICT x_n)
 Function signature for an adaptive integration step.
 

Enumerations

enum  aa_ode_integrator {
  AA_ODE_RK1 , AA_ODE_RK2 , AA_ODE_RK4 , AA_ODE_RK45_F ,
  AA_ODE_RK45_CK , AA_ODE_RK45_DP , AA_ODE_RK23_BS
}
 The known integration methods. More...
 

Functions

static AA_DEPRECATED double aa_clamp (double val, double level)
 force value to be within +/- level
 
static double aa_fclamp (double val, double min, double max)
 return val within range (min,max)
 
static double aa_fdeadzone (double val, double min, double max, double deadval)
 apply deadzone to val
 
static void aa_vclamp (size_t n, double *v, double min, double max)
 modify each element of v to be within range (min,max)
 
static double aa_sign (double val)
 return the sign of val, one of {-1,0,1}
 
AA_API int aa_isfok (double x)
 returns one if x is not infinity or NAN
 
static int aa_feq (double a, double b, double tol)
 Fuzzy equals.
 
AA_API int aa_veq (size_t n, const double *a, const double *b, double tol)
 Fuzzy equals.
 
static double aa_fsq (double a)
 square
 
static int aa_imodulo (int a, int b)
 Fortran modulo, Ada mod.
 
static long aa_lmodulo (long a, long b)
 Fortran modulo, Ada mod.
 
static int64_t aa_imodulo64 (int64_t a, int64_t b)
 Fortran modulo, Ada mod.
 
static int aa_iremainder (int a, int b)
 Fortran mod, Ada rem.
 
static double aa_fmodulo (double a, double b)
 Mathematical modulo, Fortran modulo, Ada mod.
 
static double aa_fremainder (double a, double b)
 Mathematical remainder, Fortran mod, Ada rem.
 
AA_API size_t aa_fminloc (size_t n, double *v)
 Returns index of minimum element in array v.
 
AA_API size_t aa_fmaxloc (size_t n, double *v)
 Returns index of maximum element in array v.
 
AA_API double aa_frand ()
 uniform pseudo-random in [0,1.0]
 
AA_API double aa_frand_minmax (double min, double max)
 uniform pseudo-random in [min,max]
 
AA_API void aa_vrand (size_t n, double *v)
 fills v with random numbers in [0,1.0]
 
static double aa_horner3 (double x, double a0, double a1, double a2)
 Evaluate three-term polynomial using horner's rule.
 
static void aa_sincos (double x, double *s, double *c)
 Portable sincos. More...
 
AA_API void aa_stat_box_muller (double x1, double x2, double *z1, double *z2)
 Generate 2 gaussian random numbers with stddev=1 from two uniform random numbers in interval (0,1]. More...
 
static double aa_stat_z2x (double z, double mu, double sigma)
 Convert z-score to x-score a normal distribution.
 
static double aa_stat_x2z (double x, double mu, double sigma)
 Convert x-score to z-score a normal distribution.
 
AA_API double aa_stat_mean (size_t n, const double *x)
 Compute mean of vector x.
 
AA_API double aa_stat_std (size_t n, const double *x)
 Compute standard deviation of vector x.
 
AA_API size_t aa_stat_excluded_mean_std (size_t n, const double *x, double *pmu, double *psigma, double zmin, double zmax, size_t max_iterations)
 Compute mean and standard deviation, excluding outliers. More...
 
AA_API double aa_stat_circ_mean (size_t n, const double *x)
 Compute mean of angles.
 
AA_API double aa_stat_circ_std (size_t n, const double *x)
 Compute standard deviation of vector x.
 
AA_API size_t aa_stat_excluded_circ_mean_std (size_t n, const double *x, double *pmu, double *psigma, double zmin, double zmax, size_t max_iterations)
 Compute mean and standard deviation, excluding outliers. More...
 
AA_API void aa_stat_vmean (size_t m, size_t n, const double *X, double *mu)
 Compute mean of vectors. More...
 
AA_API void aa_stat_vmean_cov (size_t m, size_t n, const double *X, double *mu, double *E)
 Compute sample covariance of vectors. More...
 
double aa_stat_mahalanobis (size_t m, const double *x, const double *mu, const double *E_inv)
 Mahalanobis distance.
 
static double aa_ang_rad2deg (double rad)
 convert radians to degrees
 
static double aa_ang_deg2rad (double deg)
 convert radians to degrees
 
static double aa_ang_norm_2pi (double an)
 normalize angle on interval [0,2pi)
 
static double aa_ang_norm_pi (double an)
 normalize angle on interval (-pi,pi)
 
static double aa_ang_delta (double a, double b)
 Difference between two angles, interval (-pi,pi)
 
AA_API double aa_la_min (size_t n, const double *x)
 min of vector
 
AA_API double aa_la_max (size_t n, const double *x)
 max of vector
 
AA_API double aa_la_dot (size_t n, const double *x, const double *y)
 Dot product. More...
 
AA_API double aa_la_norm (size_t n, const double *x)
 Euclidean norm of x. More...
 
AA_API double aa_la_ssd (size_t n, const double *x, const double *y)
 Sum of Squared Differences. More...
 
AA_API double aa_la_dist (size_t n, const double *x, const double *y)
 Euclidean Distance. More...
 
AA_API void aa_la_sadd (size_t n, double alpha, const double *x, double *r)
 vector-scalar addition. More...
 
AA_API void aa_la_scal (size_t n, double alpha, double *x)
 vector-scalar multiplication. More...
 
AA_API void aa_la_vinc (size_t n, const double *x, double *y)
 increment by vector. More...
 
AA_API void aa_la_sinc (size_t n, double alpha, double *x)
 increment by scalar. More...
 
AA_API void aa_la_axpy (size_t n, double alpha, const double *x, double *y)
 increment by scale times vector. More...
 
AA_API void aa_la_axpy3 (size_t n, double alpha, const double *x, const double *y, double *z)
 increment by scale times vector. More...
 
AA_API void aa_la_smul (size_t n, double alpha, const double *x, double *r)
 vector-scalar multiplication. More...
 
AA_API void aa_la_ssub (size_t n, double alpha, const double *x, double *r)
 vector-scalar subtraction. More...
 
AA_API void aa_la_sdiv (size_t n, double alpha, const double *x, double *r)
 vector-scalar division. More...
 
AA_API void aa_la_vadd (size_t n, const double *x, const double *y, double *r)
 Elementwise addition. More...
 
AA_API void aa_la_vsub (size_t n, const double *x, const double *y, double *r)
 Elementwise subtraction. More...
 
AA_API void aa_la_vmul (size_t n, const double *x, const double *y, double *r)
 Elementwise multiplication. More...
 
AA_API void aa_la_vdiv (size_t n, const double *x, const double *y, double *r)
 Elementwise division. More...
 
AA_API void aa_la_cross (const double a[3], const double b[3], double c[3])
 Cross product. More...
 
AA_API void aa_la_normalize (size_t n, double *x)
 Make x unit vector. More...
 
AA_API double aa_la_point_plane (size_t n, const double *point, const double *plane)
 Point Plane Distance. More...
 
AA_API void aa_la_transpose (size_t n, double *A)
 transpose square matrix A in place
 
AA_API void aa_la_transpose2 (size_t m, size_t n, const double *A, double *At)
 transpose m*n matrix A into n*m matrix At
 
static void aa_la_diag (size_t n, double *A, double x)
 Set diagonal of A to x.
 
static void aa_la_ident (size_t n, double *A)
 Set A to the identity matrix.
 
static void aa_la_mvmul (size_t m, size_t n, const double *A, const double *x, double *b)
 matrix-vector multiplication
 
AA_API double aa_la_wdot (size_t n, const double *x, const double *A, const double *y)
 Weighted inner product. More...
 
AA_API int aa_la_svd (size_t m, size_t n, const double *A, double *U, double *S, double *Vt)
 Singular Value Decomposition of A. More...
 
AA_API int aa_la_inv (size_t n, double *A)
 Inverse of A. More...
 
AA_API void aa_la_inverse3x3_ (const double R[9], double S[9])
 Inverse of 3x3 matrix R. More...
 
static void aa_la_inverse3x3 (const double R[9], double S[9])
 Inverse of 3x3 matrix R. More...
 
double aa_la_det3x3 (const double R[AA_RESTRICT 9])
 Determinant of 3x3 matrix R.
 
AA_API double aa_la_trace (size_t n, const double *A)
 Trace of a matrix.
 
AA_API void aa_la_dpinv (size_t m, size_t n, double k, const double *A, double *A_star)
 Damped Pseudo Inverse of A. More...
 
AA_API void aa_la_dzdpinv (size_t m, size_t n, double s2_min, const double *A, double *A_star)
 Deadzone, Damped Pseudo Inverse of A. More...
 
AA_API void aa_la_dls (size_t m, size_t n, double k, const double *A, const double *b, double *x)
 Damped Least Squares. More...
 
AA_API void aa_la_np (size_t m, size_t n, const double *A, const double *A_star, double *B)
 Computer Nullspace Projection Matrix. More...
 
AA_API void aa_la_xlsnp (size_t m, size_t n, const double *A, const double *A_star, const double *b, const double *xp, double *x)
 Least Squares with Nullspace projection. More...
 
AA_API void aa_la_dlsnp (size_t m, size_t n, double k, const double *A, const double *b, const double *xp, double *x)
 Damped Least Squares with Nullspace projection. More...
 
AA_API void aa_la_lls (size_t m, size_t n, size_t p, const double *A, const double *b, double *x)
 Linear Least Squares. More...
 
AA_API int aa_la_care_laub (size_t m, size_t n, size_t p, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT C, double *AA_RESTRICT X)
 Solve the continuous-time Riccati equation. More...
 
AA_API void aa_la_linterp (size_t n, double t0, const double *X0, double t1, const double *X1, double ti, double *Xi)
 Linear interpolation. More...
 
AA_API void aa_la_quadterp (size_t n, double t0, const double *X0, double t1, const double *X1, double t2, const double *X2, double ti, double *Xi)
 Quadratic interpolation. More...
 
AA_API void aa_la_quadterp_dx (size_t n, double t0, const double *X0, double t1, const double *X1, double t2, const double *X2, double ti, double *dXi)
 Quadratic interpolation, derivative.
 
AA_API void aa_la_plane_hessian (size_t n, double *plane)
 Convert plane to hessian normal form. More...
 
AA_API void aa_la_plane_fit (size_t m, size_t n, const double *points, double *plane)
 Fit a plane to a set of points. More...
 
AA_API void aa_lsim_dstep (size_t m, size_t n, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
 Linear simulation step in discrete time. More...
 
AA_API void aa_lsim_estep (size_t m, size_t n, double dt, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
 Linear simulation step with euler integration. More...
 
AA_API int aa_ode_sol (enum aa_ode_integrator integrator, const struct aa_ode_sol_opts *AA_RESTRICT opts, size_t n, aa_sys_fun sys, const void *sys_cx, aa_ode_check check, void *check_cx, double t0, double dt0, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Solve an ordinary differential equation.
 
AA_API int aa_ode_path (enum aa_ode_integrator integrator, const struct aa_ode_sol_opts *AA_RESTRICT opts, size_t n, aa_sys_fun sys, const void *sys_cx, aa_ode_check check, void *check_cx, double t0, double dt0, const double *AA_RESTRICT x0, struct aa_mem_region *region, size_t *n_points, double **path)
 Compute the path followed during integration.
 
AA_API void aa_odestep_euler (size_t n, double dt, const double *AA_RESTRICT dx, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Euler / Runge-Kutta-1 integration. More...
 
AA_API void aa_odestep_rk1 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Euler / Runge-Kutta-1 integration. More...
 
AA_API void aa_odestep_rk2 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Runge-Kutta-2 (Heun's Method) integration. More...
 
AA_API void aa_odestep_rk4 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT x1)
 Runge-Kutta-4 integration. More...
 
AA_API void aa_odestep_rkf45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
 Runge-Kutta-4-5 (Fehlberg Method) integration. More...
 
AA_API void aa_odestep_rkck45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
 Runge-Kutta-4-5 (Cash-Karp Method) integration. More...
 
AA_API void aa_odestep_dorpri45 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
 Runge-Kutta-4-5 (Dormand-Prince Method) integration. More...
 
AA_API void aa_odestep_rkbs23 (size_t n, aa_sys_fun sys, const void *cx, double t0, double dt, const double *AA_RESTRICT x0, double *AA_RESTRICT k, double *AA_RESTRICT x4, double *AA_RESTRICT x5)
 Runge-Kutta-2-3 (Bogacki-Shampine Method) integration. More...
 
AA_API void aa_lsim_rk4step (size_t m, size_t n, double dt, const double *AA_RESTRICT A, const double *AA_RESTRICT B, const double *AA_RESTRICT x0, const double *AA_RESTRICT u, double *AA_RESTRICT x1)
 Linear simulation step with Runge-Kutta-4 integration. More...
 
AA_API void aa_sys_affine (const aa_sys_affine_t *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT dx)
 Affine system model function. More...
 

Detailed Description

Numerical routines.

See also
amino/tf.h
amino/la.h

Definition in file math.h.

Macro Definition Documentation

◆ AA_MATCOL

#define AA_MATCOL (   A,
  lda,
  col 
)    ((A)+(col)*(lda))

Pointer to a column of a matrix.

Parameters
AMatrix pointer
ldaLeading dimension of A
colcolumn of the matrix (indexed from zero)

Definition at line 339 of file math.h.

◆ AA_MATREF

#define AA_MATREF (   A,
  lda,
  row,
  col 
)    (AA_MATCOL(A,lda,col)[row])

Reference an element in a column-major matrix.

Parameters
AMatrix pointer
ldaLeading dimension of A
rowrow of the matrix (indexed from zero)
colcolumn of the matrix (indexed from zero)

Definition at line 348 of file math.h.

◆ AA_MAX

#define AA_MAX (   a,
 
)
Value:
({ const __typeof__(a) aa_$_max_a = (a); \
const __typeof__(b) aa_$_max_b = (b); \
(aa_$_max_a > aa_$_max_b) ? aa_$_max_a : aa_$_max_b; })

maximum of a and b

Definition at line 63 of file math.h.

◆ AA_MIN

#define AA_MIN (   a,
 
)
Value:
({ const __typeof__(a) aa_$_min_a = (a); \
const __typeof__(b) aa_$_min_b = (b); \
(aa_$_min_a < aa_$_min_b) ? aa_$_min_a : aa_$_min_b; })

minimum of a and b

Definition at line 69 of file math.h.

Typedef Documentation

◆ aa_ode_check

typedef int aa_ode_check(void *cx, double t, double *AA_RESTRICT x, double *AA_RESTRICT y)

Function signature to check whether a differential equation has reached its goal.

Return zero if not at goal. Return non-zero if goal is reached.

Definition at line 845 of file math.h.

◆ aa_sys_fun

typedef void aa_sys_fun(const void *cx, double t, const double *AA_RESTRICT x, double *AA_RESTRICT y)

A "Signal" function.

\[ y = f(cx, x) \]

Definition at line 804 of file math.h.

Enumeration Type Documentation

◆ aa_ode_integrator

The known integration methods.

Enumerator
AA_ODE_RK1 

Runge-Kutta 1 integration.

AA_ODE_RK2 

Runge-Kutta 2 integration.

AA_ODE_RK4 

Runge-Kutta 4 integration.

AA_ODE_RK45_F 

Runge-Kutta-Fehlberg integration.

AA_ODE_RK45_CK 

Runge-Kutta 4-5 / Cash Karp integration.

AA_ODE_RK45_DP 

Runge-Kutta 4-5 / Dormand-Prince integration.

AA_ODE_RK23_BS 

Runge-Kutta 2-3 / Bogacki-Shampine integration.

Definition at line 811 of file math.h.

Function Documentation

◆ aa_la_axpy()

AA_API void aa_la_axpy ( size_t  n,
double  alpha,
const double *  x,
double *  y 
)

increment by scale times vector.

\[ y_i \leftarrow \alpha x_i + y_i \]

◆ aa_la_axpy3()

AA_API void aa_la_axpy3 ( size_t  n,
double  alpha,
const double *  x,
const double *  y,
double *  z 
)

increment by scale times vector.

\[ z_i \leftarrow \alpha x_i + y_i \]

three address version of the regular axpy

◆ aa_la_care_laub()

AA_API int aa_la_care_laub ( size_t  m,
size_t  n,
size_t  p,
const double *AA_RESTRICT  A,
const double *AA_RESTRICT  B,
const double *AA_RESTRICT  C,
double *AA_RESTRICT  X 
)

Solve the continuous-time Riccati equation.

\[ A^TX + XA - XBX + C = 0 \]

See Laub, Alan. "A Schur Method for Solving Algebraic Riccati Equations". IEEE Transactions on Automatic Control. Dec 1979.

◆ aa_la_cross()

AA_API void aa_la_cross ( const double  a[3],
const double  b[3],
double  c[3] 
)

Cross product.

\[ c \leftarrow a \times b \]

◆ aa_la_dist()

AA_API double aa_la_dist ( size_t  n,
const double *  x,
const double *  y 
)

Euclidean Distance.

\[ \sqrt{\sum_{i=0}^n (x_i-y_i)^2} \]

◆ aa_la_dls()

AA_API void aa_la_dls ( size_t  m,
size_t  n,
double  k,
const double *  A,
const double *  b,
double *  x 
)

Damped Least Squares.

\[ x = A^\ddagger b \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters
mrows in A
ncols in A
ksquare of damping factor
A\( A \in \Re^m\times\Re^n \)
b\( b \in \Re^m \)
x\( x \in \Re^n \)

◆ aa_la_dlsnp()

AA_API void aa_la_dlsnp ( size_t  m,
size_t  n,
double  k,
const double *  A,
const double *  b,
const double *  xp,
double *  x 
)

Damped Least Squares with Nullspace projection.

\[ x = A^\ddagger b + (I-A^\ddagger A)x_p \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters
mrows in A
ncols in A
ksquare of damping factor
A\( A \in \Re^m\times\Re^n \)
b\( b \in \Re^m \)
xp\( x \in \Re^n \)
x\( x \in \Re^n \)

◆ aa_la_dot()

AA_API double aa_la_dot ( size_t  n,
const double *  x,
const double *  y 
)

Dot product.

\[ {\mathbf x}^T {\mathbf y} \]

◆ aa_la_dpinv()

AA_API void aa_la_dpinv ( size_t  m,
size_t  n,
double  k,
const double *  A,
double *  A_star 
)

Damped Pseudo Inverse of A.

\[ A^\ddagger \leftarrow A^T (AA^T + kI)^{-1} \]

\[ A^\ddagger \leftarrow \sum_{i=0}^r \frac{\sigma_i}{{\sigma_i}^2+k} v_i {u_i}^T \]

The above two formulas are equivalent. This function uses the SVD method shown in the second formula.

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters
mrows of A
ncols of A
A\( A \in \Re^m\times\Re^n\)
A_star\( A^\ddagger \in \Re^n\times\Re^m\)
ksquare of damping factor

◆ aa_la_dzdpinv()

AA_API void aa_la_dzdpinv ( size_t  m,
size_t  n,
double  s2_min,
const double *  A,
double *  A_star 
)

Deadzone, Damped Pseudo Inverse of A.

\[ A^\ddagger \leftarrow \sum_{i=0}^r \frac{\sigma_i}{{\sigma_i}^2+k} v_i {u_i}^T \]

The denominator \({\sigma_i}^2 \) goes to zero near singularities. This function fixes the minimum value of the denominator at parameter s2_min.

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters
mrows of A
ncols of A
A\( A \in \Re^m\times\Re^n\)
A_star\( A^\ddagger \in \Re^n\times\Re^m\)
s2_minMinimum acceptable value for squared Singular Value

◆ aa_la_inv()

AA_API int aa_la_inv ( size_t  n,
double *  A 
)

Inverse of A.

\[ A \leftarrow A^{-1} \]

Parameters
A\(A \in \Re^n \times \Re^n\), column major
nrows and columns

◆ aa_la_inverse3x3()

static void aa_la_inverse3x3 ( const double  R[9],
double  S[9] 
)
inlinestatic

Inverse of 3x3 matrix R.

wrapper for the maxima-generated function

Definition at line 547 of file math.h.

◆ aa_la_inverse3x3_()

AA_API void aa_la_inverse3x3_ ( const double  R[9],
double  S[9] 
)

Inverse of 3x3 matrix R.

This function is generated by Maxima.

◆ aa_la_linterp()

AA_API void aa_la_linterp ( size_t  n,
double  t0,
const double *  X0,
double  t1,
const double *  X1,
double  ti,
double *  Xi 
)

Linear interpolation.

Parameters
nsize of the X vectors
t0independent variable
X0dependent variable at t0
t1independent variable
X1dependent variable at t1
tiindependent variable, interpolation point
Xidependent variable, interpolated values

◆ aa_la_lls()

AA_API void aa_la_lls ( size_t  m,
size_t  n,
size_t  p,
const double *  A,
const double *  b,
double *  x 
)

Linear Least Squares.

\[ b = Ax \]

Solves for x.

Parameters
mrows in A
ncols in A
pcols in b and x
Amatrix
boffset matrix
xsolution matrix

◆ aa_la_norm()

AA_API double aa_la_norm ( size_t  n,
const double *  x 
)

Euclidean norm of x.

\[ \sqrt{ {\mathbf x}^T {\mathbf y} } \]

◆ aa_la_normalize()

AA_API void aa_la_normalize ( size_t  n,
double *  x 
)

Make x unit vector.

\[ x \leftarrow \frac{x}{\|x\|}\]

◆ aa_la_np()

AA_API void aa_la_np ( size_t  m,
size_t  n,
const double *  A,
const double *  A_star,
double *  B 
)

Computer Nullspace Projection Matrix.

\[ B = (A^* A) - I \]

Parameters
mrows in A
ncols in A
Amatrix
A_starpseudoinverse of A
Bnullspace projection matrix

◆ aa_la_plane_fit()

AA_API void aa_la_plane_fit ( size_t  m,
size_t  n,
const double *  points,
double *  plane 
)

Fit a plane to a set of points.

Parameters
msize of space
nnumber of points
pointsm*n matrix of points, point per column, column major
planevector length m+1 to hold plane coefficients in hessian-normal form

◆ aa_la_plane_hessian()

AA_API void aa_la_plane_hessian ( size_t  n,
double *  plane 
)

Convert plane to hessian normal form.

Parameters
planeCoefficients for plane
nlength of plane vector, size of plane space is n-1

◆ aa_la_point_plane()

AA_API double aa_la_point_plane ( size_t  n,
const double *  point,
const double *  plane 
)

Point Plane Distance.

Parameters
nsize of space
pointvector of length n
planevector of length n+1

◆ aa_la_quadterp()

AA_API void aa_la_quadterp ( size_t  n,
double  t0,
const double *  X0,
double  t1,
const double *  X1,
double  t2,
const double *  X2,
double  ti,
double *  Xi 
)

Quadratic interpolation.

Parameters
nsize of the X vectors
t0independent variable
X0dependent variable at t0
t1independent variable
X1dependent variable at t1
t2independent variable
X2dependent variable at t2
tiindependent variable, interpolation point
Xidependent variable, interpolated values

◆ aa_la_sadd()

AA_API void aa_la_sadd ( size_t  n,
double  alpha,
const double *  x,
double *  r 
)

vector-scalar addition.

\[ r_i \leftarrow \alpha + x_i \]

◆ aa_la_scal()

AA_API void aa_la_scal ( size_t  n,
double  alpha,
double *  x 
)

vector-scalar multiplication.

\[ x_i \leftarrow \alpha * x_i \]

◆ aa_la_sdiv()

AA_API void aa_la_sdiv ( size_t  n,
double  alpha,
const double *  x,
double *  r 
)

vector-scalar division.

\[ r_i \leftarrow \alpha / x_i \]

◆ aa_la_sinc()

AA_API void aa_la_sinc ( size_t  n,
double  alpha,
double *  x 
)

increment by scalar.

\[ x_i \leftarrow \alpha + x_i \]

◆ aa_la_smul()

AA_API void aa_la_smul ( size_t  n,
double  alpha,
const double *  x,
double *  r 
)

vector-scalar multiplication.

\[ r_i \leftarrow \alpha * x_i \]

◆ aa_la_ssd()

AA_API double aa_la_ssd ( size_t  n,
const double *  x,
const double *  y 
)

Sum of Squared Differences.

\[ \sum_{i=0}^n (x_i-y_i)^2 \]

◆ aa_la_ssub()

AA_API void aa_la_ssub ( size_t  n,
double  alpha,
const double *  x,
double *  r 
)

vector-scalar subtraction.

\[ r_i \leftarrow \alpha - x_i \]

◆ aa_la_svd()

AA_API int aa_la_svd ( size_t  m,
size_t  n,
const double *  A,
double *  U,
double *  S,
double *  Vt 
)

Singular Value Decomposition of A.

\[ A = U \Sigma V^T \]

Parameters
mrows
ncolumns
A\(A \in \Re^m \times \Re^n\), column major
U\(U \in \Re^m \times \Re^m\), column major. If null, U is returned in A. U and Vt cannot both be null.
S\(S \in \Re^m min(m,n)\), singular values
Vt\(V^T \in \Re^n \times \Re^n\), singular vectors. If null, Vt is returned in A. Vt and U cannot both be null.

◆ aa_la_vadd()

AA_API void aa_la_vadd ( size_t  n,
const double *  x,
const double *  y,
double *  r 
)

Elementwise addition.

\[ r_i \leftarrow x_i + y_i \]

◆ aa_la_vdiv()

AA_API void aa_la_vdiv ( size_t  n,
const double *  x,
const double *  y,
double *  r 
)

Elementwise division.

\[ r_i \leftarrow x_i / y_i \]

◆ aa_la_vinc()

AA_API void aa_la_vinc ( size_t  n,
const double *  x,
double *  y 
)

increment by vector.

\[ y_i \leftarrow x_i + y_i \]

◆ aa_la_vmul()

AA_API void aa_la_vmul ( size_t  n,
const double *  x,
const double *  y,
double *  r 
)

Elementwise multiplication.

\[ r_i \leftarrow x_i * y_i \]

◆ aa_la_vsub()

AA_API void aa_la_vsub ( size_t  n,
const double *  x,
const double *  y,
double *  r 
)

Elementwise subtraction.

\[ r_i \leftarrow x_i - y_i \]

◆ aa_la_wdot()

AA_API double aa_la_wdot ( size_t  n,
const double *  x,
const double *  A,
const double *  y 
)

Weighted inner product.

\[ x^T A y \]

◆ aa_la_xlsnp()

AA_API void aa_la_xlsnp ( size_t  m,
size_t  n,
const double *  A,
const double *  A_star,
const double *  b,
const double *  xp,
double *  x 
)

Least Squares with Nullspace projection.

\[ x = A^* b - (A^* A - I) x_p \]

See "Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods". Buss, S.R. 2004

Parameters
mrows in A
ncols in A
A\( A \in \Re^m\times\Re^n \)
A_star\( A \in \Re^m\times\Re^n \)
b\( b \in \Re^m \)
xp\( x \in \Re^n \)
x\( x \in \Re^n \)

◆ aa_lsim_dstep()

AA_API void aa_lsim_dstep ( size_t  m,
size_t  n,
const double *AA_RESTRICT  A,
const double *AA_RESTRICT  B,
const double *AA_RESTRICT  x0,
const double *AA_RESTRICT  u,
double *AA_RESTRICT  x1 
)

Linear simulation step in discrete time.

\[ x_1 \leftarrow Ax_0 + Bu \]

Parameters
mstate size
ninput size
Aprocess model, m*m matrix
Binput model, m*n matrix
uinput, n length vector
x0initial state, m length vector
x1resulting state, m length vector

◆ aa_lsim_estep()

AA_API void aa_lsim_estep ( size_t  m,
size_t  n,
double  dt,
const double *AA_RESTRICT  A,
const double *AA_RESTRICT  B,
const double *AA_RESTRICT  x0,
const double *AA_RESTRICT  u,
double *AA_RESTRICT  x1 
)

Linear simulation step with euler integration.

\[ \dot{x} = Ax_0 + Bu \]

\[ x_1 \leftarrow x_0 + \Delta t \dot{x} \]

Parameters
mstate size
ninput size
dttime step
Aprocess model, m*m matrix
Binput model, m*n matrix
uinput, n length vector
x0initial state, m length vector
x1resulting state, m length vector

◆ aa_lsim_rk4step()

AA_API void aa_lsim_rk4step ( size_t  m,
size_t  n,
double  dt,
const double *AA_RESTRICT  A,
const double *AA_RESTRICT  B,
const double *AA_RESTRICT  x0,
const double *AA_RESTRICT  u,
double *AA_RESTRICT  x1 
)

Linear simulation step with Runge-Kutta-4 integration.

\[ \dot{x} = Ax_0 + Bu \]

Parameters
mstate size
ninput size
dttime step
Aprocess model, m*m matrix
Binput model, m*n matrix
uinput, n length vector
x0initial state, m length vector
x1resulting state, m length vector

◆ aa_odestep_dorpri45()

AA_API void aa_odestep_dorpri45 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  k,
double *AA_RESTRICT  x4,
double *AA_RESTRICT  x5 
)

Runge-Kutta-4-5 (Dormand-Prince Method) integration.

See Dormand, J. R.; Prince, P. J. (1980), "A family of embedded Runge-Kutta formulae", Journal of Computational and Applied Mathematics 6 (1): 19–26,

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
kderivitive matix,size n*6, initially first column contains sys evaluated at x0, overwritten with sys evalutated at x4
x4fourth-order step
x5fifth-order step
Precondition
  • First (initial) column of n*6 matrix k contains sys evalutated at x0
Postcondition
  • First (initial) column of n*6 matrix k unmodified
  • Sixth (last) column of n*6 matrix k contains sys evalutated at x4

◆ aa_odestep_euler()

AA_API void aa_odestep_euler ( size_t  n,
double  dt,
const double *AA_RESTRICT  dx,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  x1 
)

Euler / Runge-Kutta-1 integration.

\[ x_1 \leftarrow dt*dx + x_0 \]

◆ aa_odestep_rk1()

AA_API void aa_odestep_rk1 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  x1 
)

Euler / Runge-Kutta-1 integration.

\[ x_1 \leftarrow dt*dx + x_0 \]

◆ aa_odestep_rk2()

AA_API void aa_odestep_rk2 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  x1 
)

Runge-Kutta-2 (Heun's Method) integration.

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
x1integrated state

◆ aa_odestep_rk4()

AA_API void aa_odestep_rk4 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  x1 
)

Runge-Kutta-4 integration.

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
x1integrated state

◆ aa_odestep_rkbs23()

AA_API void aa_odestep_rkbs23 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  k,
double *AA_RESTRICT  x4,
double *AA_RESTRICT  x5 
)

Runge-Kutta-2-3 (Bogacki-Shampine Method) integration.

See Bogacki, Przemyslaw; Shampine, Lawrence F. (1989), "A 3(2) pair of Runge–Kutta formulas", Applied Mathematics Letters 2 (4)

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
kderivitive matix,size n*4, initially first column contains sys evaluated at x0, overwritten with sys evalutated at x4
x4fourth-order step
x5fifth-order step
Precondition
  • First (initial) column of n*4 matrix k contains sys evalutated at x0
Postcondition
  • First (initial) column of n*4 matrix k unmodified
  • Fourth (last) column of n*4 matrix k contains sys evalutated at x4

◆ aa_odestep_rkck45()

AA_API void aa_odestep_rkck45 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  k,
double *AA_RESTRICT  x4,
double *AA_RESTRICT  x5 
)

Runge-Kutta-4-5 (Cash-Karp Method) integration.

See J. R. Cash, A. H. Karp. "A variable order Runge-Kutta method for initial value problems with rapidly varying right-hand sides", ACM Transactions on Mathematical Software 16: 201-222, 1990.

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
kderivitive matix,size n*5, initially first column contains sys evaluated at x0
x4fourth-order step
x5fifth-order step
Precondition
  • First (initial) column of n*5 matrix k contains sys evalutated at x0
Postcondition
  • First (initial) column of n*5 matrix k unmodified

◆ aa_odestep_rkf45()

AA_API void aa_odestep_rkf45 ( size_t  n,
aa_sys_fun  sys,
const void *  cx,
double  t0,
double  dt,
const double *AA_RESTRICT  x0,
double *AA_RESTRICT  k,
double *AA_RESTRICT  x4,
double *AA_RESTRICT  x5 
)

Runge-Kutta-4-5 (Fehlberg Method) integration.

See Erwin Fehlberg (1969). Low-order classical Runge-Kutta formulas with step size control and their application to some heat transfer problems. NASA Technical Report 315.

Parameters
nstate size
sysfunction to integrate
cxContext struct for sys
t0time
dttime step
x0initial state
kderivitive matix,size n*5, initially first column contains sys evaluated at x0
x4fourth-order step
x5fifth-order step
Precondition
  • First (initial) column of n*5 matrix k contains sys evalutated at x0
Postcondition
  • First (initial) column of n*5 matrix k unmodified

◆ aa_sincos()

static void aa_sincos ( double  x,
double *  s,
double *  c 
)
inlinestatic

Portable sincos.

GCC will usually optimize to sincos()

Definition at line 192 of file math.h.

◆ aa_stat_box_muller()

AA_API void aa_stat_box_muller ( double  x1,
double  x2,
double *  z1,
double *  z2 
)

Generate 2 gaussian random numbers with stddev=1 from two uniform random numbers in interval (0,1].

See Box, G. E. P. and Muller, M. E. "A Note on the Generation of Random Normal Deviates." Ann. Math. Stat. 29, 610-611, 1958.

◆ aa_stat_excluded_circ_mean_std()

AA_API size_t aa_stat_excluded_circ_mean_std ( size_t  n,
const double *  x,
double *  pmu,
double *  psigma,
double  zmin,
double  zmax,
size_t  max_iterations 
)

Compute mean and standard deviation, excluding outliers.

Parameters
nlength of x
xvector of angles in radians
[out]pmumean
[out]psigmastandard deviation
zminexclude all outliers below zmin standard deviations
zmaxexclude all outliers above zmax standard deviations
max_iterationsmaximum number of iterations

◆ aa_stat_excluded_mean_std()

AA_API size_t aa_stat_excluded_mean_std ( size_t  n,
const double *  x,
double *  pmu,
double *  psigma,
double  zmin,
double  zmax,
size_t  max_iterations 
)

Compute mean and standard deviation, excluding outliers.

Parameters
nlength of x
xvector of data
[out]pmumean
[out]psigmastandard deviation
zminexclude all outliers below zmin standard deviations
zmaxexclude all outliers above zmax standard deviations
max_iterationsmaximum number of iterations

◆ aa_stat_vmean()

AA_API void aa_stat_vmean ( size_t  m,
size_t  n,
const double *  X,
double *  mu 
)

Compute mean of vectors.

Parameters
msize of space
nnumber of samples
Xmatrix of samples, one per column
[out]muvector, length m

◆ aa_stat_vmean_cov()

AA_API void aa_stat_vmean_cov ( size_t  m,
size_t  n,
const double *  X,
double *  mu,
double *  E 
)

Compute sample covariance of vectors.

Parameters
msize of space
nnumber of samples
Xmatrix of samples, one per column
muvector, length m
Ematrix, m*m

◆ aa_sys_affine()

AA_API void aa_sys_affine ( const aa_sys_affine_t cx,
double  t,
const double *AA_RESTRICT  x,
double *AA_RESTRICT  dx 
)

Affine system model function.

\[ \dot{x} = Ax + D \]